feat: tracking

.vscode/settings.json

@@ -8,6 +8,58 @@
   "files.associations": {
     "*.toml": "toml",
     "Comfyfile": "raw",
-    "*.tcc": "cpp"
+    "*.tcc": "cpp",
+    "array": "cpp",
+    "atomic": "cpp",
+    "cctype": "cpp",
+    "chrono": "cpp",
+    "clocale": "cpp",
+    "cmath": "cpp",
+    "condition_variable": "cpp",
+    "cstdarg": "cpp",
+    "cstddef": "cpp",
+    "cstdint": "cpp",
+    "cstdio": "cpp",
+    "cstdlib": "cpp",
+    "ctime": "cpp",
+    "cwchar": "cpp",
+    "cwctype": "cpp",
+    "deque": "cpp",
+    "unordered_map": "cpp",
+    "unordered_set": "cpp",
+    "vector": "cpp",
+    "exception": "cpp",
+    "algorithm": "cpp",
+    "functional": "cpp",
+    "iterator": "cpp",
+    "map": "cpp",
+    "memory": "cpp",
+    "memory_resource": "cpp",
+    "numeric": "cpp",
+    "optional": "cpp",
+    "random": "cpp",
+    "ratio": "cpp",
+    "string": "cpp",
+    "string_view": "cpp",
+    "system_error": "cpp",
+    "tuple": "cpp",
+    "type_traits": "cpp",
+    "utility": "cpp",
+    "fstream": "cpp",
+    "initializer_list": "cpp",
+    "iomanip": "cpp",
+    "iosfwd": "cpp",
+    "iostream": "cpp",
+    "istream": "cpp",
+    "limits": "cpp",
+    "mutex": "cpp",
+    "new": "cpp",
+    "ostream": "cpp",
+    "sstream": "cpp",
+    "stdexcept": "cpp",
+    "streambuf": "cpp",
+    "thread": "cpp",
+    "cinttypes": "cpp",
+    "typeinfo": "cpp"
   }
 }

include/handlers.h

@@ -19,6 +19,9 @@
 
 #define CMD_SET_NAME 0xA1
 #define CMD_SET_PID 0xA2
+#define CMD_SET_TRACKING_SPEED 0xA3
+#define CMD_SET_TRACKING_SENSITIVE 0xA4
+
 #define CMD_MOTOR_MOVE_CONTROL 0x20
 #define CMD_MOTOR_STEER_CONTROL 0x21
 #define CMD_MOTOR_SINGLE_CONTROL 0x22
@@ -46,6 +49,8 @@ public:
   static void motorXYRControl(BLECharacteristic &characteristic, uint8_t *packet);
   static void modeControl(BLECharacteristic &characteristic, uint8_t *packet);
   static void demoPID(BLECharacteristic &characteristic, uint8_t *packet);
+  static void setTrackingSensitive(BLECharacteristic &characteristic, uint8_t *packet);
+  static void setTrackingSpeed(BLECharacteristic &characteristic, uint8_t *packet);
 };
 
 #endif

include/ir.h

@@ -1,14 +1,15 @@
 #ifndef IR_H
 #define IR_H
 
-#include <SoftwareSerial.h>
+#include <Wire.h>
 #include <Arduino.h>
+#include <algorithm>
 #include "consts.h"
 
 enum IRMode
 {
   IR_MODE_GPIO,
-  IR_MODE_UART
+  IR_MODE_I2C
 };
 
 struct IRData
@@ -22,7 +23,7 @@ struct IRData
 class IR
 {
 public:
-  static SoftwareSerial *uart;
+  static TwoWire *i2c;
   static IRData data;
   static void init();
   static void update();

include/mode.h

@@ -13,6 +13,7 @@ class Mode
 {
 public:
   static ModeType mode;
+  static ModeType lastMode;
   static void processDeviceMode();
 };
 

include/motor.h

@@ -19,6 +19,11 @@ struct MotorStatus
   unsigned char pwm;
 };
 
+typedef enum {
+  ROTATE_CLOCKWISE = 1,
+  ROTATE_ANTICLOCKWISE = -1,
+} RotateDirection;
+
 class MotorController
 {
 public:
@@ -26,6 +31,7 @@ public:
   static void pwmControl(char motor, unsigned char pwm);
   static void motorControl(char motor, int speed);
   static void motorXYRControl(int8_t x, int8_t y, int8_t r);
+  static void rotate(RotateDirection direction, uint8_t speed);
 
   // 获取电机状态
   static MotorStatus getMotorStatus(char motor);

include/storage.h

@@ -16,6 +16,8 @@ public:
   static unsigned int getSensitivity();
   static void setName(String name);
   static String getName();
+  static void setTrackingParams(uint8_t baseSpeed, uint8_t turnSpeed, uint8_t rotateSensitive);
+  static void getTrackingParams(uint8_t &baseSpeed, uint8_t &turnSpeed, uint8_t &rotateSensitive);
 };
 
 #endif

include/tracking.h

@@ -4,10 +4,14 @@
 #include "ir.h"
 #include "motor.h"
 #include "pid.h"
+#include "storage.h"
+
 class TrackingController {
 public:
     static uint8_t baseSpeed;  // 基础前进速度
     static uint8_t turnSpeed;  // 转弯速度
+    static uint8_t rotateSensitive;  // 旋转灵敏度
+    static float lastOffset;  // 上一次的偏移量
 
     // 初始化循迹控制器
     static void init();
@@ -18,11 +22,18 @@ public:
     // 设置循迹速度
     static void setSpeed(uint8_t baseSpeed, uint8_t turnSpeed);
 
+    // 设置旋转灵敏度
+    static void setRotateSensitive(uint8_t sensitive);
+
     // 计算偏移量
     static float calculateOffset(const IRData& irData);
 
 private:
     static constexpr float LOST_LINE = 999.0f;  // 使用一个特殊值表示丢线状态
+    static bool isSearching;  // 标记是否处于搜索模式
+    static unsigned long allHighStartTime;  // 记录全高状态开始的时间
+    static bool isAllHighState;  // 记录是否处于全高状态
+    static constexpr unsigned long ALL_HIGH_TIMEOUT = 1000;  // 全高状态超时时间（1秒）
 };
 
 #endif

packet.md

@@ -125,7 +125,7 @@
 
 包体 `00 05 30 模式 FF`
 
-控制示例 `00 05 30 01 FE`
+控制示例 `00 05 30 01 FF`
 
 ## 设置
 
@@ -147,6 +147,18 @@
 
 设置示例 `00 11 A2 01 01 01 01 01 01 01 01 01 01 01 01 01 FF`
 
+### 设置循迹速度 `0xA3`
+
+包体 `00 06 A3 基础速度 转弯速度 FF`
+
+设置示例 `00 06 A3 50 50 FF`
+
+### 设置循迹转弯灵敏度 `0xA4`
+
+包体 `00 05 A4 灵敏度 FF`
+
+设置示例 `00 05 A4 03 FF`
+
 ## 状态回报
 
 ### 设备状态回报 `0xE0`

src/handlers.cpp

@@ -129,18 +129,12 @@ void Handlers::motorRotateControl(BLECharacteristic &characteristic, uint8_t *pa
   // 顺时针
   if (direction == 0x00)
   {
-    MotorController::motorControl('A', 255);
+    MotorController::rotate(ROTATE_CLOCKWISE, 255);
-    MotorController::motorControl('B', 255);
-    MotorController::motorControl('C', -255);
-    MotorController::motorControl('D', -255);
   }
   // 逆时针
   else if (direction == 0x01)
   {
-    MotorController::motorControl('A', -255);
+    MotorController::rotate(ROTATE_ANTICLOCKWISE, 255);
-    MotorController::motorControl('B', -255);
-    MotorController::motorControl('C', 255);
-    MotorController::motorControl('D', 255);
   }
 }
 
@@ -194,6 +188,21 @@ void Handlers::modeControl(BLECharacteristic &characteristic, uint8_t *packet)
   }
 }
 
+// 设置循迹转弯灵敏度
+void Handlers::setTrackingSensitive(BLECharacteristic &characteristic, uint8_t *packet)
+{
+  uint8_t sensitive = packet[3];
+  TrackingController::setRotateSensitive(sensitive);
+}
+
+// 设置循迹速度
+void Handlers::setTrackingSpeed(BLECharacteristic &characteristic, uint8_t *packet)
+{
+  uint8_t baseSpeed = packet[3];
+  uint8_t turnSpeed = packet[4];
+  TrackingController::setSpeed(baseSpeed, turnSpeed);
+}
+
 void Handlers::demoPID(BLECharacteristic &characteristic, uint8_t *packet)
 {
   Serial.println("CMD_DEMO_PID");

src/ir.cpp

@@ -1,7 +1,7 @@
 #include "ir.h"
 
 IRData IR::data;
-SoftwareSerial *IR::uart;
+TwoWire *IR::i2c;
 
 void IR::init()
 {
@@ -16,13 +16,23 @@ void IR::init()
       data.pins[i] = pins[i];
     };
   }
-  else if (IR_MODE == IRMode::IR_MODE_UART)
+  else if (IR_MODE == IRMode::IR_MODE_I2C)
   {
     uint8_t pins[2] = IR_PINS;
-    data.pins[0] = pins[0];
+    data.pins[0] = pins[0]; // SCL
-    data.pins[1] = pins[1];
+    data.pins[1] = pins[1]; // SDA
-    uart = new SoftwareSerial(pins[0], pins[1]);
+    
-    uart->begin(IR_BAUD);
+    // 检查引脚值是否有效
+    if (data.pins[0] >= 0 && data.pins[1] >= 0) {
+      i2c = new TwoWire(0);
+      if (i2c->begin(data.pins[1], data.pins[0])) {
+        Serial.println("I2C initialized successfully");
+      } else {
+        Serial.println("I2C initialization failed");
+      }
+    } else {
+      Serial.println("Invalid I2C pins");
+    }
   }
 }
 
@@ -32,14 +42,52 @@ void IR::update()
   {
     for (int i = 0; i < data.count; i++)
     {
-      data.data[i] = digitalRead(data.pins[i]);
+      // 读取传感器数据
+      bool value = digitalRead(data.pins[i]);
+      
+      // 根据 IR_REVERSE 判断是否需要反转电平
+      #if IR_OUTPUT_REVERSE
+      value = !value;
+      #endif
+
+      // 根据传感器排列方向存储数据
+      #ifdef IR_DIRECTION_REVERSE
+      data.data[data.count - 1 - i] = value;
+      #else
+      data.data[i] = value;
+      #endif
     }
   }
-  else if (data.mode == IRMode::IR_MODE_UART)
+  else if (data.mode == IRMode::IR_MODE_I2C && i2c != nullptr)
   {
-    if (uart->available())
+    byte value = 0;
-    {
+    
-      data.data[0] = uart->read();
+    delay(10);
+    
+    i2c->requestFrom(0x12, 1);
+    if(i2c->available()) {
+      value = i2c->read();
+      
+      // 根据 IR_COUNT 读取数据
+      for(int i = 0; i < std::min<int>(data.count, IR_COUNT); i++) {
+        // 读取传感器数据
+        bool sensorValue = !((value >> (7-i)) & 0x01);
+        
+        // 根据 IR_REVERSE 判断是否需要反转电平
+        #if IR_OUTPUT_REVERSE
+        sensorValue = !sensorValue;
+        #endif
+
+        // 根据传感器排列方向存储数据
+        #ifdef IR_DIRECTION_REVERSE
+        data.data[std::min<int>(data.count, IR_COUNT) - 1 - i] = sensorValue;
+        #else
+        data.data[i] = sensorValue;
+        #endif
+      }
+    } else {
+      // 通信失败时清零数据
+      memset(data.data, 0, IR_COUNT);
     }
   }
 }

src/mode.cpp

@@ -8,4 +8,10 @@ void Mode::processDeviceMode()
   {
     TrackingController::update();
   }
+  if (lastMode != mode)
+  {
+    lastMode = mode;
+    MotorController::motorXYRControl(0, 0, 0);
+    Serial.println("Mode changed to: " + String(mode));
+  }
 }

src/motor.cpp

@@ -176,34 +176,26 @@ void MotorController::motorXYRControl(int8_t x, int8_t y, int8_t r)
   motorControl('D', speedD);
 }
 
+void MotorController::rotate(RotateDirection direction, uint8_t speed)
+{
+  motorControl('A', direction == ROTATE_CLOCKWISE ? speed : -speed);
+  motorControl('B', direction == ROTATE_CLOCKWISE ? speed : -speed);
+  motorControl('C', direction == ROTATE_CLOCKWISE ? -speed : speed);
+  motorControl('D', direction == ROTATE_CLOCKWISE ? -speed : speed);
+}
+
 MotorStatus MotorController::getMotorStatus(char motor)
 {
   switch (motor)
   {
   case 'A':
-    return {
+    return motorA;
-        digitalRead(motorA.pin.in1),
-        digitalRead(motorA.pin.in2),
-        motorA.pwm,
-    };
   case 'B':
-    return {
+    return motorB;
-        digitalRead(motorB.pin.in1),
-        digitalRead(motorB.pin.in2),
-        motorB.pwm,
-    };
   case 'C':
-    return {
+    return motorC;
-        digitalRead(motorC.pin.in1),
-        digitalRead(motorC.pin.in2),
-        motorC.pwm,
-    };
   case 'D':
-    return {
+    return motorD;
-        digitalRead(motorD.pin.in1),
-        digitalRead(motorD.pin.in2),
-        motorD.pwm,
-    };
   default:
     return {false, false, 0};
   }

src/storage.cpp

@@ -86,3 +86,38 @@ String Storage::getName()
 
   return name;
 }
+
+void Storage::setTrackingParams(uint8_t baseSpeed, uint8_t turnSpeed, uint8_t rotateSensitive)
+{
+    if (!isMounted)
+        return;
+
+    File file = SPIFFS.open("/tracking.txt", "w");
+    file.println(baseSpeed);
+    file.println(turnSpeed);
+    file.println(rotateSensitive);
+    file.close();
+}
+
+void Storage::getTrackingParams(uint8_t &baseSpeed, uint8_t &turnSpeed, uint8_t &rotateSensitive)
+{
+    if (!isMounted) {
+        baseSpeed = 50;
+        turnSpeed = 50;
+        rotateSensitive = 3;
+        return;
+    }
+
+    File file = SPIFFS.open("/tracking.txt", "r");
+    if (!file) {
+        baseSpeed = 50;
+        turnSpeed = 50;
+        rotateSensitive = 3;
+        return;
+    }
+
+    baseSpeed = file.readStringUntil('\n').toInt();
+    turnSpeed = file.readStringUntil('\n').toInt();
+    rotateSensitive = file.readStringUntil('\n').toInt();
+    file.close();
+}

src/tracking.cpp

@@ -1,17 +1,36 @@
 #include "tracking.h"
 
-uint8_t TrackingController::baseSpeed = 10; // 默认速度50%
+uint8_t TrackingController::baseSpeed = 50;
-uint8_t TrackingController::turnSpeed = 10; // 默认转弯速度30%
+uint8_t TrackingController::turnSpeed = 50;
+uint8_t TrackingController::rotateSensitive = 3;
+float TrackingController::lastOffset = 0;
+bool TrackingController::isSearching = false;
+unsigned long TrackingController::allHighStartTime = 0;
+bool TrackingController::isAllHighState = false;
 
 void TrackingController::init()
 {
   Serial.println("Tracking Init");
+  
+  // 从存储中读取参数
+  Storage::getTrackingParams(baseSpeed, turnSpeed, rotateSensitive);
 }
 
 void TrackingController::setSpeed(uint8_t base, uint8_t turn)
 {
   baseSpeed = base;
   turnSpeed = turn;
+  
+  // 保存参数到存储
+  Storage::setTrackingParams(baseSpeed, turnSpeed, rotateSensitive);
+}
+
+void TrackingController::setRotateSensitive(uint8_t sensitive)
+{
+  rotateSensitive = sensitive;
+  
+  // 保存参数到存储
+  Storage::setTrackingParams(baseSpeed, turnSpeed, rotateSensitive);
 }
 
 void TrackingController::update()
@@ -21,66 +40,169 @@ void TrackingController::update()
   const IRData &irData = IR::data;
   float offset = calculateOffset(irData);
 
-  Serial.print("Offset: ");
-  Serial.println(offset);
-
   // 检查是否丢线
   if (offset == LOST_LINE)
-  { // 需要定义 LOST_LINE 常量
+  {
-    // 丢线时停止移动
+    // 如果是因为全高超时导致的停止
-    Serial.println("Lost line!");
+    if (isAllHighState) {
+      Serial.println("Vehicle likely lifted, stopping all motors");
       MotorController::motorXYRControl(0, 0, 0);
       return;
     }
 
-  // 正常巡线控制
+    // 正常丢线处理逻辑
-  float correction = PIDController::update(0, offset);
+    
-  MotorController::motorXYRControl(0, baseSpeed, -correction);
+    isSearching = true;
+    MotorController::rotate(lastOffset < 0 ? ROTATE_CLOCKWISE : ROTATE_ANTICLOCKWISE, baseSpeed * rotateSensitive);
+    return;
+  }
+
+  // 如果之前在搜索模式，现在找到线了，打印日志
+  if (isSearching) {
+    Serial.println("Line found!");
+    isSearching = false;
+  }
+
+  // 添加防抖：如果偏移量很小，认为是直线
+  if (abs(offset) < 0.2) {
+    offset = 0;
+  }
+
+  // 更新最后的偏移量，用于丢线时的方向判断
+  lastOffset = offset;
+
+  // 直接使用偏移量控制转向，降低灵敏度
+  float correction = offset * 30.0f;  // 降低转向系数
+  
+  // 调整速度控制逻辑
+  float speedFactor;
+  if (abs(offset) > 1.5) {
+    // 大转弯时的速度因子
+    speedFactor = 0.6;
+  } else if (abs(offset) > 0.5) {
+    // 小转弯时的速度因子
+    speedFactor = 0.8;
+  } else {
+    // 直线时全速
+    speedFactor = 1.0;
+  }
+  
+  float currentSpeed = baseSpeed * speedFactor;
+  
+  // 调试信息
+  Serial.print("Offset: "); Serial.print(offset);
+  Serial.print(" Correction: "); Serial.print(correction);
+  Serial.print(" Speed: "); Serial.println(currentSpeed);
+
+  // 直接控制电机
+  MotorController::motorXYRControl(0, currentSpeed, -correction);
 }
 
 // 计算偏移量，返回范围 [-2, 2]
 float TrackingController::calculateOffset(const IRData &irData)
 {
   float offset = 0;
-  int activeCount = 0;
   bool allHigh = true;
   bool allLow = true;
 
   Serial.print("IR Data: ");
-  for (int i = 0; i < irData.length; i++) {
+  for (int i = 0; i < IR_COUNT; i++)
+  {
     Serial.print(irData.data[i]);
     Serial.print(" ");
   }
   Serial.println();
 
   // 检查是否所有传感器都是高电平或低电平
-  for (int i = 0; i < irData.length; i++) {
+  for (int i = 0; i < IR_COUNT; i++)
-    if (irData.data[i]) {
+  {
-      allLow = false;
+    if (irData.data[i]) allLow = false;
-    } else {
+    else allHigh = false;
-      allHigh = false;
-    }
   }
 
-  // 如果所有传感器都是高电平或低电平，说明不在线上
+  // 全高状态处理
-  if (allHigh || allLow) {
+  if (allHigh) {
+    // 如果刚进入全高状态
+    if (!isAllHighState) {
+      isAllHighState = true;
+      allHighStartTime = millis();
+      Serial.println("All sensors high, continue moving forward");
+      return 0;  // 返回0表示直行
+    }
+    
+    // 检查是否超过超时时间
+    if (millis() - allHighStartTime >= ALL_HIGH_TIMEOUT) {
+      Serial.println("All sensors high for too long, stopping (possibly lifted)");
+      return LOST_LINE;  // 触发停止
+    }
+    
+    return 0;  // 继续直行
+  }
+  
+  if (isAllHighState) {
+    isAllHighState = false;
+  }
+
+  // 全低说明全离线
+  if (allLow)
     return LOST_LINE;
+
+  // 寻找最长的连续激活区域
+  int maxConsecutiveCount = 0;
+  int maxConsecutiveStart = -1;
+  int currentConsecutiveCount = 0;
+  int currentConsecutiveStart = -1;
+
+  for (int i = 0; i < IR_COUNT; i++) {
+    if (irData.data[i]) {
+      if (currentConsecutiveCount == 0) {
+        currentConsecutiveStart = i;
+      }
+      currentConsecutiveCount++;
+      
+      if (currentConsecutiveCount > maxConsecutiveCount) {
+        maxConsecutiveCount = currentConsecutiveCount;
+        maxConsecutiveStart = currentConsecutiveStart;
+      }
+    } else {
+      currentConsecutiveCount = 0;
+    }
   }
 
-  // 计算加权平均偏移
+  // 如果找到连续区域
-  for (int i = 0; i < irData.length; i++)
+  if (maxConsecutiveCount > 0) {
-  {
+    // 计算连续区域的中心位置
-    if (irData.data[i])
+    float centerPos = maxConsecutiveStart + (maxConsecutiveCount - 1) / 2.0f;
-    {
+    // 将中心位置转换为偏移量
-      // 将传感器位置映射到 [-2, 2] 范围
+    offset = (centerPos - (IR_COUNT - 1) / 2.0f) * (4.0f / (IR_COUNT - 1));
-      offset += (i - (irData.length - 1) / 2.0f) * (4.0f / (irData.length - 1));
+    
+    // 根据连续激活的数量调整权重
+    float weight = 1.0;
+    if (maxConsecutiveCount == 2) {
+      weight = 1.2;  // 小幅转弯
+    } else if (maxConsecutiveCount >= 3) {
+      weight = 1.5;  // 大幅转弯
+    }
+    
+    offset *= weight;
+    
+    Serial.print("Center at sensor: "); Serial.print(centerPos);
+    Serial.print(" Consecutive count: "); Serial.println(maxConsecutiveCount);
+  } else {
+    // 如果没有找到连续区域，使用所有激活点的加权平均
+    int activeCount = 0;
+    for (int i = 0; i < IR_COUNT; i++) {
+      if (irData.data[i]) {
+        float positionOffset = (i - (IR_COUNT - 1) / 2.0f) * (4.0f / (IR_COUNT - 1));
+        offset += positionOffset;
         activeCount++;
       }
     }
-
+    if (activeCount > 0) {
-  // 如果有传感器检测到线，返回平均偏移
+      offset /= activeCount;
-  if (activeCount > 0)
+    }
-    return offset / activeCount;
+  }
-
+
-  return LOST_LINE;
+  lastOffset = offset;
+  return offset;
 }

src/transmit.cpp

@@ -78,6 +78,14 @@ void handleSerialPacket(uint8_t *packet, int length, BLECharacteristic &characte
     Handlers::setPID(characteristic, packet);
     break;
 
+  case CMD_SET_TRACKING_SENSITIVE:
+    Handlers::setTrackingSensitive(characteristic, packet);
+    break;
+
+  case CMD_SET_TRACKING_SPEED:
+    Handlers::setTrackingSpeed(characteristic, packet);
+    break;
+
   case CMD_MOTOR_MOVE_CONTROL:
     Handlers::motorMoveControl(characteristic, packet);
     break;
@@ -132,9 +140,9 @@ void sendStatus()
     buffer[bufferIndex++] = MotorController::getMotorStatus('C').pwm;
     buffer[bufferIndex++] = (MotorController::getMotorStatus('D').in2 << 1) | MotorController::getMotorStatus('D').in1;
     buffer[bufferIndex++] = MotorController::getMotorStatus('D').pwm;
-    buffer[bufferIndex++] = IR::data.length;
+    buffer[bufferIndex++] = IR_COUNT;
     uint16_t irData = 0;
-    for (uint8_t i = 0; i < IR::data.length; i++)
+    for (uint8_t i = 0; i < IR_COUNT; i++)
     {
       irData |= (IR::data.data[i] << i);
     }