feat: tracking

include/handlers.h

@@ -8,6 +8,8 @@
 #include "transmit.h"
 #include "pid.h"
 #include "ir.h"
+#include "tracking.h"
+#include "mode.h"
 #include "utils.h"
 
 // 指令定义
@@ -23,7 +25,9 @@
 #define CMD_MOTOR_ROTATE_CONTROL 0x23
 #define CMD_DEMO_PID 0xf0
 #define CMD_DEMO_PATH 0xf1
-#define CMD_MOTOR_XYR_CONTROL 0x24 // XYR轴向控制指令
+#define CMD_MOTOR_XYR_CONTROL 0x24
+
+#define CMD_MODE_CONTROL 0x30
 
 #define CMD_STATUS_MOTOR 0xE0
 
@@ -40,6 +44,7 @@ public:
   static void motorRotateControl(BLECharacteristic &characteristic, uint8_t *packet);
   static void motorSingleControl(BLECharacteristic &characteristic, uint8_t *packet);
   static void motorXYRControl(BLECharacteristic &characteristic, uint8_t *packet);
+  static void modeControl(BLECharacteristic &characteristic, uint8_t *packet);
   static void demoPID(BLECharacteristic &characteristic, uint8_t *packet);
 };
 

include/mode.h

@@ -0,0 +1,19 @@
+#ifndef MODE_H
+#define MODE_H
+
+#include "Arduino.h"
+#include "tracking.h"
+enum ModeType
+{
+  MODE_MANUAL_CONTROL,
+  MODE_TRACK,
+};
+
+class Mode
+{
+public:
+  static ModeType mode;
+  static void processDeviceMode();
+};
+
+#endif

include/pid.h

@@ -1,6 +1,7 @@
-#ifndef __PID_H__
-#define __PID_H__
+#ifndef _PID_H_
+#define _PID_H_
 
+#include <Arduino.h>
 #include "consts.h"
 
 struct PIDConfig
@@ -8,22 +9,30 @@ struct PIDConfig
   float Kp;
   float Ki;
   float Kd;
+  float outputMax;   // 输出限幅
+  float outputMin;   // 输出限幅
+  float integralMax; // 积分限幅
 };
 
 struct PID
 {
   float error;
+  float last_error;
   float integral;
   float derivative;
-  float last_error;
+  unsigned long lastTime; // 上次更新时间
 };
 
 class PIDController
 {
 public:
-  static PIDConfig pidConfig;
   static PID pid;
-  static void setConfig(float Kp, float Ki, float Kd);
+
+  static PIDConfig pidConfig;
+  static void reset(); // 重置PID状态
+  static void setConfig(float Kp, float Ki, float Kd,
+                        float outputMax = 255, float outputMin = -255,
+                        float integralMax = 1000);
   static float update(float target, float current);
 };
 

include/tracking.h

@@ -0,0 +1,28 @@
+#ifndef _TRACKING_H_
+#define _TRACKING_H_
+
+#include "ir.h"
+#include "motor.h"
+#include "pid.h"
+class TrackingController {
+public:
+    static uint8_t baseSpeed;  // 基础前进速度
+    static uint8_t turnSpeed;  // 转弯速度
+
+    // 初始化循迹控制器
+    static void init();
+    
+    // 更新循迹状态并控制电机
+    static void update();
+    
+    // 设置循迹速度
+    static void setSpeed(uint8_t baseSpeed, uint8_t turnSpeed);
+
+    // 计算偏移量
+    static float calculateOffset(const IRData& irData);
+
+private:
+    static constexpr float LOST_LINE = 999.0f;  // 使用一个特殊值表示丢线状态
+};
+
+#endif

include/transmit.h

@@ -2,8 +2,7 @@
 #define TRANSMIT_H
 
 #include <Arduino.h>
-#include <BLEDevice.h>
-#include <BLECharacteristic.h>
+#include "ble.h"
 #include "handlers.h"
 
 #define PACKET_R_HEAD 0x00
@@ -14,6 +13,6 @@
 
 void handleSerialPacket(uint8_t *packet, int length, BLECharacteristic &characteristic);
 void processSerialIncomingByte(uint8_t incomingByte, BLECharacteristic &characteristic);
-void sendStatus(BLECharacteristic &characteristic);
+void sendStatus();
 
 #endif

packet.md

@@ -116,6 +116,17 @@
 
 控制示例 `00 07 24 01 01 01 FF`
 
+## 模式切换 `0x30`
+
+| 模式数据 | 含义 |
+| -------- | ---- |
+| 0x00     | 手动模式 |
+| 0x01     | 巡线模式 |
+
+包体 `00 05 30 模式 FF`
+
+控制示例 `00 05 30 01 FE`
+
 ## 设置
 
 ### 设置设备名称 `0xA1`
@@ -140,13 +151,14 @@
 
 ### 设备状态回报 `0xE0`
 
-包体 `01 0E 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 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`
 
 | 数据 | 类型 | 表示 |
 | -------- | ---- | ---- |
+| 运行模式 | 无符号整数 | 0x00 手动模式，0x01 巡线模式 |
 | IN_1_2  | 无符号整数 | 按位表示 IN1 IN2 的值， 0000 00 IN2 IN1 |
 | PWM     | 无符号整数 | - |
 | 红外引脚数量 | 无符号整数 | - |
 | 红外数据 | 无符号整数 | 按位表示红外循迹模块数据， 0001 1111 |
 
-示例 `01 0E E0 01 FF 02 FF 02 FF 01 FF 05 1F FE`
+示例 `01 0F E0 00 01 FF 02 FF 02 FF 01 FF 05 1F FE`

src/handlers.cpp

@@ -178,6 +178,22 @@ void Handlers::motorXYRControl(BLECharacteristic &characteristic, uint8_t *packe
   MotorController::motorXYRControl(x, y, r);
 }
 
+void Handlers::modeControl(BLECharacteristic &characteristic, uint8_t *packet)
+{
+  uint8_t mode = packet[3];
+  switch (mode)
+  {
+  case 0x01:
+    Mode::mode = ModeType::MODE_TRACK;
+    break;
+
+  case 0x00:
+  default:
+    Mode::mode = ModeType::MODE_MANUAL_CONTROL;
+    break;
+  }
+}
+
 void Handlers::demoPID(BLECharacteristic &characteristic, uint8_t *packet)
 {
   Serial.println("CMD_DEMO_PID");

src/main.cpp

@@ -5,6 +5,8 @@
 #include "ble.h"
 #include "led.h"
 #include "ir.h"
+#include "tracking.h"
+#include "mode.h"
 #include "consts.h"
 
 void setup()
@@ -33,17 +35,28 @@ void setup()
   {
     LED::init(STATUS_LED);
   }
+
+  // 初始化循迹控制器
+  TrackingController::init();
 }
 
 void loop()
 {
+  // 循环 1 部分，获取传感器数据
   IR::update();
+
+  // 循环 2 部分，更新状态灯
   if (STATUS_LED_ENABLE)
   {
     LED::updateStatusLED(BLEManager::deviceConnected);
   }
+
+  // 循环 3 部分，处理设备模式
+  Mode::processDeviceMode();
+
+  // 循环 4 部分，反馈状态
   if (BLEManager::deviceConnected)
   {
-    sendStatus(*BLEManager::pTxCharacteristic);
+    sendStatus();
   }
 }

src/mode.cpp

@@ -0,0 +1,11 @@
+#include "mode.h"
+
+ModeType Mode::mode = ModeType::MODE_MANUAL_CONTROL;
+
+void Mode::processDeviceMode()
+{
+  if (mode == ModeType::MODE_TRACK)
+  {
+    TrackingController::update();
+  }
+}

src/pid.cpp

@@ -1,22 +1,80 @@
 #include "pid.h"
 
+PID PIDController::pid = {0, 0, 0, 0, 0};
+
 PIDConfig PIDController::pidConfig = {
     PID_KP,
     PID_KI,
     PID_KD};
 
-void PIDController::setConfig(float Kp, float Ki, float Kd)
+void PIDController::reset()
+{
+  pid.error = 0;
+  pid.last_error = 0;
+  pid.integral = 0;
+  pid.derivative = 0;
+  pid.lastTime = millis();
+}
+
+void PIDController::setConfig(float Kp, float Ki, float Kd,
+                              float outputMax, float outputMin,
+                              float integralMax)
 {
   pidConfig.Kp = Kp;
   pidConfig.Ki = Ki;
   pidConfig.Kd = Kd;
+  pidConfig.outputMax = outputMax;
+  pidConfig.outputMin = outputMin;
+  pidConfig.integralMax = integralMax;
 }
 
 float PIDController::update(float target, float current)
 {
-  pid.error = target - current;
-  pid.integral += pid.error;
-  pid.derivative = pid.error - pid.last_error;
-  pid.last_error = pid.error;
-  return pidConfig.Kp * pid.error + pidConfig.Ki * pid.integral + pidConfig.Kd * pid.derivative;
+  // 计算时间间隔
+  unsigned long now = millis();
+  float dt = (now - pid.lastTime) / 1000.0f; // 转换为秒
+  pid.lastTime = now;
+
+  // 如果时间间隔过大，说明可能是第一次运行或长时间未运行
+  if (dt > 1.0f)
+  {
+    dt = 0.01f; // 使用默认值
+  }
+
+  // 计算误差
+  pid.error = target - current;
+
+  // 计算积分项
+  pid.integral += pid.error * dt;
+
+  // 积分限幅
+  if (pid.integral > pidConfig.integralMax)
+  {
+    pid.integral = pidConfig.integralMax;
+  }
+  else if (pid.integral < -pidConfig.integralMax)
+  {
+    pid.integral = -pidConfig.integralMax;
+  }
+
+  // 计算微分项（使用错误微分而不是输出微分，避免微分突跳）
+  pid.derivative = (pid.error - pid.last_error) / dt;
+  pid.last_error = pid.error;
+
+  // 计算PID输出
+  float output = pidConfig.Kp * pid.error +
+                 pidConfig.Ki * pid.integral +
+                 pidConfig.Kd * pid.derivative;
+
+  // 输出限幅
+  if (output > pidConfig.outputMax)
+  {
+    output = pidConfig.outputMax;
+  }
+  else if (output < pidConfig.outputMin)
+  {
+    output = pidConfig.outputMin;
+  }
+
+  return output;
 }

src/tracking.cpp

@@ -0,0 +1,86 @@
+#include "tracking.h"
+
+uint8_t TrackingController::baseSpeed = 10; // 默认速度50%
+uint8_t TrackingController::turnSpeed = 10; // 默认转弯速度30%
+
+void TrackingController::init()
+{
+  Serial.println("Tracking Init");
+}
+
+void TrackingController::setSpeed(uint8_t base, uint8_t turn)
+{
+  baseSpeed = base;
+  turnSpeed = turn;
+}
+
+void TrackingController::update()
+{
+  IR::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!");
+    MotorController::motorXYRControl(0, 0, 0);
+    return;
+  }
+
+  // 正常巡线控制
+  float correction = PIDController::update(0, offset);
+  MotorController::motorXYRControl(0, baseSpeed, -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++) {
+    Serial.print(irData.data[i]);
+    Serial.print(" ");
+  }
+  Serial.println();
+
+  // 检查是否所有传感器都是高电平或低电平
+  for (int i = 0; i < irData.length; i++) {
+    if (irData.data[i]) {
+      allLow = false;
+    } else {
+      allHigh = false;
+    }
+  }
+
+  // 如果所有传感器都是高电平或低电平，说明不在线上
+  if (allHigh || allLow) {
+    return LOST_LINE;
+  }
+
+  // 计算加权平均偏移
+  for (int i = 0; i < irData.length; i++)
+  {
+    if (irData.data[i])
+    {
+      // 将传感器位置映射到 [-2, 2] 范围
+      offset += (i - (irData.length - 1) / 2.0f) * (4.0f / (irData.length - 1));
+      activeCount++;
+    }
+  }
+
+  // 如果有传感器检测到线，返回平均偏移
+  if (activeCount > 0)
+    return offset / activeCount;
+
+  return LOST_LINE;
+}

src/transmit.cpp

@@ -94,20 +94,24 @@ void handleSerialPacket(uint8_t *packet, int length, BLECharacteristic &characte
     Handlers::motorSingleControl(characteristic, packet);
     break;
 
-  case CMD_DEMO_PID:
-    Handlers::demoPID(characteristic, packet);
-    break;
-
   case CMD_MOTOR_XYR_CONTROL:
     Handlers::motorXYRControl(characteristic, packet);
     break;
 
+  case CMD_MODE_CONTROL:
+    Handlers::modeControl(characteristic, packet);
+    break;
+
+  case CMD_DEMO_PID:
+    Handlers::demoPID(characteristic, packet);
+    break;
+
   default:
     break;
   }
 }
 
-void sendStatus(BLECharacteristic &characteristic)
+void sendStatus()
 {
   unsigned long currentMillis = millis();
   if (currentMillis - lastMotorStatusUpdate >= MOTOR_STATUS_INTERVAL)
@@ -117,8 +121,9 @@ void sendStatus(BLECharacteristic &characteristic)
     uint8_t buffer[PACKET_MAX_LENGTH];
     uint8_t bufferIndex = 0;
     buffer[bufferIndex++] = PACKET_T_HEAD;
-    buffer[bufferIndex++] = 0x0E;
+    buffer[bufferIndex++] = 0x0F;
     buffer[bufferIndex++] = CMD_STATUS_MOTOR;
+    buffer[bufferIndex++] = Mode::mode;
     buffer[bufferIndex++] = (MotorController::getMotorStatus('A').in2 << 1) | MotorController::getMotorStatus('A').in1;
     buffer[bufferIndex++] = MotorController::getMotorStatus('A').pwm;
     buffer[bufferIndex++] = (MotorController::getMotorStatus('B').in2 << 1) | MotorController::getMotorStatus('B').in1;
@@ -135,7 +140,7 @@ void sendStatus(BLECharacteristic &characteristic)
     }
     buffer[bufferIndex++] = irData;
     buffer[bufferIndex++] = PACKET_T_TAIL;
-    characteristic.setValue(buffer, bufferIndex);
-    characteristic.notify();
+    BLEManager::pTxCharacteristic->setValue(buffer, bufferIndex);
+    BLEManager::pTxCharacteristic->notify();
   }
 }