feat: status push

packet.md

@@ -56,21 +56,19 @@
 | 0x00     | 停止 |
 | 0x01     | 前进 |
 | 0x02     | 后退 |
-| 0x03     | 左转 |
-| 0x04     | 右转 |
 
 包体 `00 06 20 方向 速度 FF`
 
 控制示例 `00 06 20 01 FF FF`
 
-### 原地控制 `0x21`
+### 方向控制 `0x21`
 
 | 方向数据 | 含义 |
 | -------- | ---- |
-| 0x00     | 顺时针 |
+| 0x00     | 左转 |
-| 0x01     | 逆时针 |
+| 0x01     | 右转 |
 
-包体 `00 06 21 方向 时间 FF`
+包体 `00 06 21 方向 差速 FF`
 
 控制示例 `00 06 21 01 01 FF`
 
@@ -93,12 +91,21 @@
 
 控制示例 `00 07 22 01 01 01 FF`
 
-## 设置
+### 原地控制 `0x23`
 
-### 设置转向灵敏度 `0x30`
+| 方向数据 | 含义 |
+| -------- | ---- |
+| 0x00     | 顺时针 |
+| 0x01     | 逆时针 |
 
-含义为 1 度对应差速
+包体 `00 06 23 方向 时间 FF`
 
-包体 `00 05 30 差速 FF`
+控制示例 `00 06 23 01 01 FF`
 
-设置示例 `00 05 30 10 FF`
+## 状态回报
+
+### 电机状态汇报 `0xE0`
+
+包体 `01 0C 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 0C E0 01 FF 02 FF 02 FF 01 FF FE`

src/main.cpp

@@ -20,9 +20,14 @@
 #define CMD_GET_SPIFFS_STATUS 0x11
 #define CMD_GET_DISTANCE 0x12
 #define CMD_MOTOR_MOVE_CONTROL 0x20
-#define CMD_MOTOR_ROTATE_CONTROL 0x21
+#define CMD_MOTOR_STEER_CONTROL 0x21
 #define CMD_MOTOR_SINGLE_CONTROL 0x22
+#define CMD_MOTOR_ROTATE_CONTROL 0x23
 #define CMD_DEMO_PID 0xf0
+#define CMD_DEMO_PATH 0xf1
+
+#define CMD_STATUS_MOTOR 0xE0
+
 // 全局变量
 int currentSpeed = 0;   // 当前速度
 int turnOffset = 0;     // 转向偏移量 (-100 到 100)
@@ -44,6 +49,23 @@ bool isReceivingPacket = false;
 unsigned long lastLedToggle = 0; // 上次LED切换状态的时间
 bool ledState = false;           // LED当前状态
 
+#define MOTOR_STATUS_INTERVAL 200        // 发送电机状态的时间间隔(ms)
+unsigned long lastMotorStatusUpdate = 0; // 上次发送电机状态的时间
+
+// 在全局变量区域添加电机方向状态跟踪
+struct MotorStatus
+{
+  bool in1;
+  bool in2;
+  unsigned char pwm;
+};
+
+// 定义四个电机的状态
+MotorStatus motorA = {false, false, 0};
+MotorStatus motorB = {false, false, 0};
+MotorStatus motorC = {false, false, 0};
+MotorStatus motorD = {false, false, 0};
+
 void processSerialIncomingByte(uint8_t incomingByte, BLECharacteristic &characteristic);
 
 class CarBLEServerCallbacks : public BLEServerCallbacks
@@ -92,26 +114,101 @@ void floatToBytes(float val, uint8_t *bytes)
   }
 }
 
-// 电机控制函数
+void pwmControl(char motor, unsigned char pwm)
-void motorControl(int pwmPin, int in1Pin, int in2Pin, int speed)
 {
+  int pwmPin;
+  switch (motor)
+  {
+  case 'A':
+    pwmPin = MOTOR_A_PWMA;
+    motorA.pwm = pwm;
+    break;
+  case 'B':
+    pwmPin = MOTOR_B_PWMB;
+    motorB.pwm = pwm;
+    break;
+  case 'C':
+    pwmPin = MOTOR_C_PWMA;
+    motorC.pwm = pwm;
+    break;
+  case 'D':
+    pwmPin = MOTOR_D_PWMB;
+    motorD.pwm = pwm;
+    break;
+  }
+  analogWrite(pwmPin, pwm);
+}
+
+// 电机控制函数
+void motorControl(char motor, int speed)
+{
+  int in1Pin, in2Pin;
+  MotorStatus *status;
+
+  switch (motor)
+  {
+  case 'A':
+    in1Pin = MOTOR_A_AIN1;
+    in2Pin = MOTOR_A_AIN2;
+    status = &motorA;
+    break;
+  case 'B':
+    in1Pin = MOTOR_B_BIN2;
+    in2Pin = MOTOR_B_BIN1;
+    status = &motorB;
+    break;
+  case 'C':
+    in1Pin = MOTOR_C_AIN2;
+    in2Pin = MOTOR_C_AIN1;
+    status = &motorC;
+    break;
+  case 'D':
+    in1Pin = MOTOR_D_BIN1;
+    in2Pin = MOTOR_D_BIN2;
+    status = &motorD;
+    break;
+  }
+
   if (speed > 0)
   {
     digitalWrite(in1Pin, HIGH);
     digitalWrite(in2Pin, LOW);
+    if (motor == 'A' || motor == 'D')
+    {
+      status->in1 = true;
+      status->in2 = false;
+    }
+    else
+    {
+      status->in1 = false;
+      status->in2 = true;
+    }
   }
   else if (speed < 0)
   {
     digitalWrite(in1Pin, LOW);
     digitalWrite(in2Pin, HIGH);
+    if (motor == 'A' || motor == 'D')
+    {
+      status->in1 = false;
+      status->in2 = true;
+    }
+    else
+    {
+      status->in1 = true;
+      status->in2 = false;
+    }
+
     speed = -speed;
   }
   else
   {
     digitalWrite(in1Pin, LOW);
     digitalWrite(in2Pin, LOW);
+    status->in1 = false;
+    status->in2 = false;
   }
-  analogWrite(pwmPin, speed);
+  pwmControl(motor, speed);
 }
 
 float getDistance()
@@ -169,7 +266,7 @@ bool CarStorage::isMounted = false;
 
 void setup()
 {
-  // 初始化串口
+  // 初始化<EFBFBD><EFBFBD><EFBFBD>口
   Serial.begin(115200);
 
   // 初始化 BLE
@@ -228,7 +325,7 @@ void handleSerialPacket(uint8_t *packet, int length, BLECharacteristic &characte
 
   uint8_t packetLength = packet[1]; // 获取长度
   uint8_t cmd = packet[2];          // 获取指令
-  uint8_t direction, speed, time, wheel;
+  uint8_t direction, speed, time, wheel, diffSpeed;
   float distance;
   uint8_t buffer[PACKET_MAX_LENGTH]; // 用于存储要发送的数据
   int bufferIndex = 0;
@@ -279,26 +376,44 @@ void handleSerialPacket(uint8_t *packet, int length, BLECharacteristic &characte
     // 移动
     if (direction == 0x00)
     {
-      motorControl(MOTOR_A_PWMA, MOTOR_A_AIN1, MOTOR_A_AIN2, 0);
+      motorControl('A', 0);
-      motorControl(MOTOR_B_PWMB, MOTOR_B_BIN1, MOTOR_B_BIN2, 0);
+      motorControl('B', 0);
-      motorControl(MOTOR_C_PWMA, MOTOR_C_AIN1, MOTOR_C_AIN2, 0);
+      motorControl('C', 0);
-      motorControl(MOTOR_D_PWMB, MOTOR_D_BIN1, MOTOR_D_BIN2, 0);
+      motorControl('D', 0);
     }
     // 前进
     else if (direction == 0x01)
     {
-      motorControl(MOTOR_A_PWMA, MOTOR_A_AIN1, MOTOR_A_AIN2, speed);
+      motorControl('A', speed);
-      motorControl(MOTOR_B_PWMB, MOTOR_B_BIN2, MOTOR_B_BIN1, speed);
+      motorControl('B', speed);
-      motorControl(MOTOR_C_PWMA, MOTOR_C_AIN2, MOTOR_C_AIN1, speed);
+      motorControl('C', speed);
-      motorControl(MOTOR_D_PWMB, MOTOR_D_BIN1, MOTOR_D_BIN2, speed);
+      motorControl('D', speed);
     }
     // 后退
     else if (direction == 0x02)
     {
-      motorControl(MOTOR_A_PWMA, MOTOR_A_AIN1, MOTOR_A_AIN2, -speed);
+      motorControl('A', -speed);
-      motorControl(MOTOR_B_PWMB, MOTOR_B_BIN2, MOTOR_B_BIN1, -speed);
+      motorControl('B', -speed);
-      motorControl(MOTOR_C_PWMA, MOTOR_C_AIN2, MOTOR_C_AIN1, -speed);
+      motorControl('C', -speed);
-      motorControl(MOTOR_D_PWMB, MOTOR_D_BIN1, MOTOR_D_BIN2, -speed);
+      motorControl('D', -speed);
+    }
+    break;
+
+  case CMD_MOTOR_STEER_CONTROL:
+    direction = packet[3];
+    diffSpeed = packet[4];
+    Serial.println("CMD_MOTOR_STEER_CONTROL, direction: " + String(direction) + ", diffSpeed: " + String(diffSpeed));
+    // 左转
+    if (direction == 0x00)
+    {
+      motorControl('A', motorA.pwm - diffSpeed);
+      motorControl('B', motorB.pwm - diffSpeed);
+    }
+    // 右转
+    else if (direction == 0x01)
+    {
+      motorControl('C', motorC.pwm - diffSpeed);
+      motorControl('D', motorD.pwm - diffSpeed);
     }
     break;
 
@@ -309,18 +424,18 @@ void handleSerialPacket(uint8_t *packet, int length, BLECharacteristic &characte
     // 顺时针
     if (direction == 0x00)
     {
-      motorControl(MOTOR_A_PWMA, MOTOR_A_AIN1, MOTOR_A_AIN2, 255);
+      motorControl('A', 255);
-      motorControl(MOTOR_B_PWMB, MOTOR_B_BIN2, MOTOR_B_BIN1, 255);
+      motorControl('B', 255);
-      motorControl(MOTOR_C_PWMA, MOTOR_C_AIN2, MOTOR_C_AIN1, -255);
+      motorControl('C', -255);
-      motorControl(MOTOR_D_PWMB, MOTOR_D_BIN1, MOTOR_D_BIN2, -255);
+      motorControl('D', -255);
     }
     // 逆时针
     else if (direction == 0x01)
     {
-      motorControl(MOTOR_A_PWMA, MOTOR_A_AIN1, MOTOR_A_AIN2, -255);
+      motorControl('A', -255);
-      motorControl(MOTOR_B_PWMB, MOTOR_B_BIN2, MOTOR_B_BIN1, -255);
+      motorControl('B', -255);
-      motorControl(MOTOR_C_PWMA, MOTOR_C_AIN2, MOTOR_C_AIN1, 255);
+      motorControl('C', 255);
-      motorControl(MOTOR_D_PWMB, MOTOR_D_BIN1, MOTOR_D_BIN2, 255);
+      motorControl('D', 255);
     }
     break;
 
@@ -333,16 +448,16 @@ void handleSerialPacket(uint8_t *packet, int length, BLECharacteristic &characte
     switch (wheel)
     {
     case 0x00:
-      motorControl(MOTOR_A_PWMA, MOTOR_A_AIN1, MOTOR_A_AIN2, speed);
+      motorControl('A', speed);
       break;
     case 0x01:
-      motorControl(MOTOR_B_PWMB, MOTOR_B_BIN2, MOTOR_B_BIN1, speed);
+      motorControl('B', speed);
       break;
     case 0x02:
-      motorControl(MOTOR_C_PWMA, MOTOR_C_AIN2, MOTOR_C_AIN1, speed);
+      motorControl('C', speed);
       break;
     case 0x03:
-      motorControl(MOTOR_D_PWMB, MOTOR_D_BIN1, MOTOR_D_BIN2, speed);
+      motorControl('D', speed);
       break;
     }
     break;
@@ -350,12 +465,115 @@ void handleSerialPacket(uint8_t *packet, int length, BLECharacteristic &characte
   case CMD_DEMO_PID:
     Serial.println("CMD_DEMO_PID");
 
-    // // 获取 PID 参数
+    // PID 参数
-    // unsigned int sensitivity = CarStorage::getSensitivity();
+    float Kp = 2.0;
-    // Serial.println("CMD_DEMO_PID, sensitivity: " + String(sensitivity));
+    float Ki = 0.5;
+    float Kd = 0.1;
 
-    // // 设置 PID 参数
+    // PID 变量
-    // CarStorage::setSensitivity(sensitivity + 1);
+    float targetSpeed = 255; // 目标速度
+    float currentSpeed = 0;  // 当前速度
+    float lastError = 0;     // 上一次误差
+    float integral = 0;      // 积分项
+    unsigned long lastTime = millis();
+
+    // 加速阶段 - PID控制
+    while (millis() - lastTime < 2000)
+    { // 2秒加速过程
+      float error = targetSpeed - currentSpeed;
+      integral += error;
+      float derivative = error - lastError;
+
+      float output = Kp * error + Ki * integral + Kd * derivative;
+      output = constrain(output, 0, 255); // 限制输出范围
+
+      currentSpeed = output;
+
+      // 应用到所有电机
+      motorControl('A', output);
+      motorControl('B', output);
+      motorControl('C', output);
+      motorControl('D', output);
+
+      lastError = error;
+      delay(10); // 控制周期
+    }
+
+    // 停止阶段 - PID控制
+    lastTime = millis();
+    targetSpeed = 0; // 目标速度设为0
+
+    while (millis() - lastTime < 1000)
+    { // 1秒减速过程
+      float error = targetSpeed - currentSpeed;
+      integral += error;
+      float derivative = error - lastError;
+
+      float output = Kp * error + Ki * integral + Kd * derivative;
+      output = constrain(output, 0, 255);
+
+      currentSpeed = output;
+
+      // 应用到所有电机
+      motorControl('A', output);
+      motorControl('B', output);
+      motorControl('C', output);
+      motorControl('D', output);
+
+      lastError = error;
+      delay(10);
+    }
+
+    // 确保完全停止
+    motorControl('A', 0);
+    motorControl('B', 0);
+    motorControl('C', 0);
+    motorControl('D', 0);
+    break;
+
+  case CMD_DEMO_PATH:
+    Serial.println("CMD_DEMO_PATH");
+
+    // 第一段：全速前进1秒
+    currentSpeed = 255;
+    unsigned long startTime = millis();
+    while (millis() - startTime < 1000)
+    {
+      motorControl('A', currentSpeed);
+      motorControl('B', currentSpeed);
+      motorControl('C', currentSpeed);
+      motorControl('D', currentSpeed);
+      delay(10);
+    }
+
+    // 第二段：左转90度
+    startTime = millis();
+    while (millis() - startTime < 500)
+    { // 假设500ms可以转90度
+      // 左侧电机反转,右侧电机正转
+      motorControl('A', 200);
+      motorControl('B', 200);
+      motorControl('C', 200);
+      motorControl('D', 200);
+      delay(10);
+    }
+
+    // 第三段：全速前进1秒
+    startTime = millis();
+    while (millis() - startTime < 1000)
+    {
+      motorControl('A', currentSpeed);
+      motorControl('B', currentSpeed);
+      motorControl('C', currentSpeed);
+      motorControl('D', currentSpeed);
+      delay(10);
+    }
+
+    // 最后停止所有电机
+    motorControl('A', 0);
+    motorControl('B', 0);
+    motorControl('C', 0);
+    motorControl('D', 0);
 
     break;
 
@@ -423,7 +641,36 @@ void updateStatusLED()
   }
 }
 
+void sendMotorStatus()
+{
+  unsigned long currentMillis = millis();
+  if (currentMillis - lastMotorStatusUpdate >= MOTOR_STATUS_INTERVAL)
+  {
+    lastMotorStatusUpdate = currentMillis;
+
+    uint8_t buffer[PACKET_MAX_LENGTH];
+    buffer[0] = PACKET_T_HEAD;
+    buffer[1] = 0x0C;
+    buffer[2] = CMD_STATUS_MOTOR;
+    buffer[3] = (motorA.in2 << 1) | motorA.in1;
+    buffer[4] = motorA.pwm;
+    buffer[5] = (motorB.in2 << 1) | motorB.in1;
+    buffer[6] = motorB.pwm;
+    buffer[7] = (motorC.in2 << 1) | motorC.in1;
+    buffer[8] = motorC.pwm;
+    buffer[9] = (motorD.in2 << 1) | motorD.in1;
+    buffer[10] = motorD.pwm;
+    buffer[11] = PACKET_T_TAIL;
+    pTxCharacteristic->setValue(buffer, 12);
+    pTxCharacteristic->notify();
+  }
+}
+
 void loop()
 {
   updateStatusLED();
+  if (deviceConnected)
+  {
+    sendMotorStatus();
+  }
 }