From 3420bf4a4a5f0fb160f4e933cf33e4cd41f5e3e0 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?=E7=8E=96=E5=8F=81?= <colour_93@furry.top>
Date: Sun, 15 Dec 2024 21:42:43 +0800
Subject: [PATCH] =?UTF-8?q?feat:=20=E8=B6=85=E5=A3=B0=E6=B3=A2?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 packet.md    |  8 +++++++
 src/main.cpp | 66 +++++++++++++++++++++++++++++++++++++++++++---------
 2 files changed, 63 insertions(+), 11 deletions(-)

diff --git a/packet.md b/packet.md
index 0ed53dd..548160b 100644
--- a/packet.md
+++ b/packet.md
@@ -28,6 +28,14 @@
 
 返回示例 `01 05 10 01 FE`
 
+### 查询超声波距离 `0x11`
+
+查询示例 `00 04 11 FF`
+
+返回一个 4 字节浮点数，表示距离，单位 m
+
+返回示例 `01 08 11 距离3 距离2 距离1 距离0 FE`
+
 ## 控制
 
 ### 行进控制 `0x20`
diff --git a/src/main.cpp b/src/main.cpp
index 7636622..1e09460 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -3,8 +3,8 @@
 
 #define STATUS_LED 2
 
-#define HC_SR04_TRIG 34
-#define HC_SR04_ECHO 35
+#define HC_SR04_TRIG 23
+#define HC_SR04_ECHO 22
 
 // 使用 2 片 TB6612 控制 4 个电机
 // 使用差速控制转向
@@ -54,7 +54,8 @@ BluetoothSerial SerialBT;
 #define PACKET_MAX_LENGTH 32 // 数据包最大长度
 
 // 指令定义
-#define CMD_CHECK_BT_STATUS 0x10
+#define CMD_GET_BT_STATUS 0x10
+#define CMD_GET_DISTANCE 0x11
 #define CMD_MOTOR_MOVE_CONTROL 0x20
 #define CMD_MOTOR_ROTATE_CONTROL 0x21
 
@@ -68,6 +69,22 @@ bool isReceivingPacket = false;
 unsigned long lastLedToggle = 0; // 上次LED切换状态的时间
 bool ledState = false;           // LED当前状态
 
+void floatToBytes(float val, uint8_t *bytes)
+{
+  union
+  {
+    float f;
+    uint8_t bytes[4];
+  } u;
+  u.f = val;
+
+  // 考虑大小端问题
+  for (int i = 0; i < 4; i++)
+  {
+    bytes[i] = u.bytes[i];
+  }
+}
+
 // 电机控制函数
 void motorControl(int pwmPin, int in1Pin, int in2Pin, int speed)
 {
@@ -90,6 +107,17 @@ void motorControl(int pwmPin, int in1Pin, int in2Pin, int speed)
   analogWrite(pwmPin, speed);
 }
 
+
+float getDistance()
+{
+  digitalWrite(HC_SR04_TRIG, HIGH);
+  delayMicroseconds(1);
+  digitalWrite(HC_SR04_TRIG, LOW);
+  float distance = pulseIn(HC_SR04_ECHO, HIGH); // 计数接收高电平时间
+  distance = distance * 340 / 2 / 10000;        // 计算距离 1：声速：340M/S  2：实际距离为1/2声速距离 3：计数时钟为1US//温补公式：c=(331.45+0.61t/℃)m•s-1 (其中331.45是在0度）
+  return distance;
+}
+
 u_char getSerialBTStatus()
 {
   return SerialBT.connected() ? 0x01 : 0x00;
@@ -123,7 +151,7 @@ void setup()
   pinMode(MOTOR_D_BIN1, OUTPUT);
   pinMode(MOTOR_D_BIN2, OUTPUT);
 
-  pinMode(HC_SR04_TRIG, INPUT);
+  pinMode(HC_SR04_TRIG, OUTPUT);
   pinMode(HC_SR04_ECHO, INPUT);
 }
 
@@ -135,19 +163,35 @@ void handleSerialPacket(uint8_t *packet, int length, Stream &serial)
   uint8_t packetLength = packet[1]; // 获取长度
   uint8_t cmd = packet[2];          // 获取指令
   uint8_t direction, speed, time;
+  float distance;
+
+  uint8_t transmitBuffer[PACKET_MAX_LENGTH];
 
   switch (cmd)
   {
-  case CMD_CHECK_BT_STATUS:
-    Serial.println("CMD_CHECK_BT_STATUS");
+  case CMD_GET_BT_STATUS:
+    Serial.println("CMD_GET_BT_STATUS");
     // 回复状态
     serial.write(PACKET_T_HEAD);
     serial.write(0x05); // 包体长度
-    serial.write(CMD_CHECK_BT_STATUS);
+    serial.write(CMD_GET_BT_STATUS);
     serial.write(getSerialBTStatus());
     serial.write(PACKET_T_TAIL);
     break;
 
+  case CMD_GET_DISTANCE:
+    Serial.println("CMD_GET_DISTANCE");
+    distance = getDistance();
+    Serial.printf("distance = %f\n", distance);
+    floatToBytes(distance, transmitBuffer);
+    // 回复距离
+    serial.write(PACKET_T_HEAD);
+    serial.write(0x08); // 包体长度
+    serial.write(CMD_GET_DISTANCE);
+    serial.write(transmitBuffer, 4);
+    serial.write(PACKET_T_TAIL);
+    break;
+
   case CMD_MOTOR_MOVE_CONTROL:
     // 获取方向和速度
     direction = packet[3];
@@ -195,10 +239,10 @@ void handleSerialPacket(uint8_t *packet, int length, Stream &serial)
     if (direction == 0x00)
     {
       // 左侧电机向前转
-      motorControl(MOTOR_A_PWMA, MOTOR_A_AIN1, MOTOR_A_AIN2, 255);  // 左前
+      motorControl(MOTOR_A_PWMA, MOTOR_A_AIN1, MOTOR_A_AIN2, 255); // 左前
       motorControl(MOTOR_B_PWMB, MOTOR_B_BIN2, MOTOR_B_BIN1, 255); // 左后
       // 右侧电机向后转
-      motorControl(MOTOR_C_PWMA, MOTOR_C_AIN2, MOTOR_C_AIN1, -255);  // 右后
+      motorControl(MOTOR_C_PWMA, MOTOR_C_AIN2, MOTOR_C_AIN1, -255); // 右后
       motorControl(MOTOR_D_PWMB, MOTOR_D_BIN1, MOTOR_D_BIN2, -255); // 右前
     }
 
@@ -207,9 +251,9 @@ void handleSerialPacket(uint8_t *packet, int length, Stream &serial)
     {
       // 左侧电机向后转
       motorControl(MOTOR_A_PWMA, MOTOR_A_AIN1, MOTOR_A_AIN2, -255); // 左前
-      motorControl(MOTOR_B_PWMB, MOTOR_B_BIN2, MOTOR_B_BIN1, -255);  // 左后
+      motorControl(MOTOR_B_PWMB, MOTOR_B_BIN2, MOTOR_B_BIN1, -255); // 左后
       // 右侧电机向前转
-      motorControl(MOTOR_C_PWMA, MOTOR_C_AIN2, MOTOR_C_AIN1, 255);  // 右后
+      motorControl(MOTOR_C_PWMA, MOTOR_C_AIN2, MOTOR_C_AIN1, 255); // 右后
       motorControl(MOTOR_D_PWMB, MOTOR_D_BIN1, MOTOR_D_BIN2, 255); // 右前
     }
     break;