feat: uw

include/tracking.h

@@ -5,6 +5,7 @@
 #include "motor.h"
 #include "pid.h"
 #include "storage.h"
+#include "ultrasound.h"
 
 class TrackingController {
 public:
@@ -13,6 +14,10 @@ public:
     static uint8_t rotateSensitive;  // 旋转灵敏度
     static float lastOffset;  // 上一次的偏移量
 
+    // 添加避障相关常量
+    static constexpr float OBSTACLE_DISTANCE = 30.0f;  // 避障距离阈值（厘米）
+    static bool isAvoiding;  // 是否正在避障
+
     // 初始化循迹控制器
     static void init();
     

include/ultrasound.h

@@ -13,10 +13,12 @@ struct UltrasonicPin
 struct Ultrasonic
 {
   static UltrasonicPin pin;
+  static float distance;
   static void init(UltrasonicPin pin, int servoPin);
-  static float getDistance();
+  static void update();
 
   static int servoPin;
+  static int angle;
   static Servo servo;
   static void servoControl(int angle);
 };

include/utils.h

@@ -7,5 +7,4 @@
 
 void floatToBytes(float val, uint8_t *bytes);
 float bytesToFloat(uint8_t *bytes);
-void updateStatusLED(bool deviceConnected, int ledPin);
 #endif

packet.md

@@ -163,7 +163,7 @@
 
 ### 设备状态回报 `0xE0`
 
-包体 `01 0F E0 运行模式 电机A_IN_1_2 电机A_PWM 电机B_IN_1_2 电机B_PWM 电机C_IN_1_2 电机C_PWM 电机D_IN_1_2 电机D_PWM 红外引脚数量 红外数据 FE`
+包体 `01 13 E0 运行模式 电机A_IN_1_2 电机A_PWM 电机B_IN_1_2 电机B_PWM 电机C_IN_1_2 电机C_PWM 电机D_IN_1_2 电机D_PWM 红外引脚数量 红外数据 超声波距离3 超声波距离2 超声波距离1 超声波距离0 FE`
 
 | 数据 | 类型 | 表示 |
 | -------- | ---- | ---- |
@@ -172,5 +172,6 @@
 | PWM     | 无符号整数 | - |
 | 红外引脚数量 | 无符号整数 | - |
 | 红外数据 | 无符号整数 | 按位表示红外循迹模块数据， 0001 1111 |
+| 超声波距离 | 无符号整数 | 4 字节浮点数，表示距离，单位 m |
 
-示例 `01 0F E0 00 01 FF 02 FF 02 FF 01 FF 05 1F FE`
+示例 `01 13 E0 00 01 FF 02 FF 02 FF 01 FF 05 1F FE 01 01 01 01 FE`

src/handlers.cpp

@@ -31,7 +31,7 @@ void Handlers::getSPIFFSStatus(BLECharacteristic &characteristic)
 
 void Handlers::getDistance(BLECharacteristic &characteristic)
 {
-  float distance = Ultrasonic::getDistance();
+  float distance = Ultrasonic::distance;
   uint8_t bufferIndex = 0;
   Serial.println("CMD_GET_DISTANCE, distance: " + String(distance));
   // 构建响应数据

src/main.cpp

@@ -62,4 +62,6 @@ void loop()
   {
     sendStatus();
   }
+
+  delay(500);
 }

src/tracking.cpp

@@ -7,6 +7,7 @@ float TrackingController::lastOffset = 0;
 bool TrackingController::isSearching = false;
 unsigned long TrackingController::allHighStartTime = 0;
 bool TrackingController::isAllHighState = false;
+bool TrackingController::isAvoiding = false;
 
 void TrackingController::init()
 {
@@ -40,6 +41,9 @@ void TrackingController::update()
   const IRData &irData = IR::data;
   float offset = calculateOffset(irData);
 
+  Serial.print("Ultrasonic Distance: ");
+  Serial.println(Ultrasonic::distance);
+
   // 检查是否丢线
   if (offset == LOST_LINE)
   {
@@ -60,11 +64,32 @@ void TrackingController::update()
     return;
   }
 
-  // 如果之前在搜索模式，现在找到线了，打印日志
+  // 检查前方障碍物
+  if (!isAvoiding && (Ultrasonic::distance < OBSTACLE_DISTANCE || Ultrasonic::distance > 500 ))
+  {
+    Serial.print("Obstacle detected at ");
+    Serial.print(Ultrasonic::distance);
+    Serial.println("cm, turning back");
+
+    isAvoiding = true;
+    // 原地旋转180度
+    MotorController::rotate(ROTATE_CLOCKWISE, baseSpeed * rotateSensitive);
+    return;
+  }
+
+  // 如果正在避障但已经丢线，说明已经转过头，可以开始寻找新的线
+  if (isAvoiding && offset == LOST_LINE)
+  {
+    isAvoiding = false;
+    isSearching = true;
+  }
+
+  // 如果之前在搜索模式，现在找到线了
   if (isSearching)
   {
     Serial.println("Line found!");
     isSearching = false;
+    isAvoiding = false; // 确保避障标志被清除
   }
 
   // 添加防抖：如果偏移量很小，认为是直线
@@ -76,14 +101,19 @@ void TrackingController::update()
   // 更新最后的偏移量，用于丢线时的方向判断
   lastOffset = offset;
 
+  // 如果没有在避障，则正常循迹
+  if (!isAvoiding)
+  {
     // 计算基础速度和转向调整
     float leftSpeed = baseSpeed;
     float rightSpeed = baseSpeed;
 
-  if (offset != 0) {
+    if (offset != 0)
+    {
       // 在大偏移时降低基础速度
       float speedFactor = 1.0f;
-    if (abs(offset) > 1.0f) {
+      if (abs(offset) > 1.0f)
+      {
         speedFactor = 0.7f;
         leftSpeed *= speedFactor;
         rightSpeed *= speedFactor;
@@ -132,6 +162,7 @@ void TrackingController::update()
     MotorController::motorControl('C', rightSpeed); // 右后
     MotorController::motorControl('D', rightSpeed); // 右前
   }
+}
 
 // 计算偏移量，返回范围 [-2, 2]
 float TrackingController::calculateOffset(const IRData &irData)

src/transmit.cpp

@@ -122,14 +122,22 @@ void handleSerialPacket(uint8_t *packet, int length, BLECharacteristic &characte
 void sendStatus()
 {
   unsigned long currentMillis = millis();
+  static unsigned long lastUltrasonicUpdate = 0;
+  
   if (currentMillis - lastMotorStatusUpdate >= MOTOR_STATUS_INTERVAL)
   {
     lastMotorStatusUpdate = currentMillis;
 
+    // 每1秒更新一次超声波数据
+    if (currentMillis - lastUltrasonicUpdate >= 1000) {
+      lastUltrasonicUpdate = currentMillis;
+      Ultrasonic::update();
+    }
+
     uint8_t buffer[PACKET_MAX_LENGTH];
     uint8_t bufferIndex = 0;
     buffer[bufferIndex++] = PACKET_T_HEAD;
-    buffer[bufferIndex++] = 0x0F;
+    buffer[bufferIndex++] = 0x13;
     buffer[bufferIndex++] = CMD_STATUS_MOTOR;
     buffer[bufferIndex++] = Mode::mode;
     buffer[bufferIndex++] = (MotorController::getMotorStatus('A').in2 << 1) | MotorController::getMotorStatus('A').in1;
@@ -147,6 +155,8 @@ void sendStatus()
       irData |= (IR::data.data[i] << i);
     }
     buffer[bufferIndex++] = irData;
+    floatToBytes(Ultrasonic::distance, &buffer[bufferIndex]);
+    bufferIndex += 4;
     buffer[bufferIndex++] = PACKET_T_TAIL;
     BLEManager::pTxCharacteristic->setValue(buffer, bufferIndex);
     BLEManager::pTxCharacteristic->notify();

src/ultrasound.cpp

@@ -1,9 +1,11 @@
 #include "ultrasound.h"
 
 Servo Ultrasonic::servo;
+int Ultrasonic::servoPin = 0;
 
 UltrasonicPin Ultrasonic::pin = {0, 0};
-int Ultrasonic::servoPin = 0;
+float Ultrasonic::distance = 0;
+int Ultrasonic::angle = 0;
 
 void Ultrasonic::init(UltrasonicPin pin, int servoPin)
 {
@@ -13,27 +15,27 @@ void Ultrasonic::init(UltrasonicPin pin, int servoPin)
   pinMode(pin.echo, INPUT);
 
   servo.attach(servoPin);
-  servo.write(0);
-  delay(500);
-  servo.write(180);
-  delay(500);
   servo.write(90);
 }
 
-float Ultrasonic::getDistance()
+void Ultrasonic::update()
 {
   digitalWrite(pin.trig, HIGH);
   delayMicroseconds(1);
   digitalWrite(pin.trig, LOW);
   float distance = pulseIn(pin.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;
+  Ultrasonic::distance = distance;
 }
 
 void Ultrasonic::servoControl(int angle)
 {
-  if (angle < 0) angle = 0;
+  if (angle < 0)
-  if (angle > 180) angle = 180;
+    angle = 0;
+  if (angle > 180)
+    angle = 180;
+
+  Ultrasonic::angle = angle;
 
   servo.write(angle);
 }