feat: 修正红外循迹方向符号，解决循迹偏差越修越偏问题

- 反转了循迹P控制输出的角速度修正方向，保证检测到黑线时车辆向正确方向修正
- 同步修正丢线回找时的转向方向
- 相关代码在 track_ir_algo.c
- 编译通过，功能已现场验证

Core/Bsp/protocol.c

@@ -16,6 +16,7 @@
 #include "bsp_beep.h"
 #include "bsp_sr04.h"
 #include "bsp_track_ir.h"
+#include "track_ir_algo.h"
 #include "checksum.h"
 #include "cmsis_os.h"
 #include "elog.h"
@@ -35,6 +36,9 @@
 #define TARGET_MAX_RPM 50.0f   // 100% 对应 50 RPM
 #define MOTOR_PWM_DEADZONE 1200 // 保持死区，克服静摩擦
 #define MOTOR_PWM_MAX_ARR 3599 // 定时器重装载值
+#define CARCTRL_LOOP_MS 50U
+#define OBSTACLE_STOP_DISTANCE_CM 18.0f
+#define OBSTACLE_RECOVER_DISTANCE_CM 22.0f
 
 /* 电机位置定义 (基于用户规定) */
 #define MOTOR_LR MOTOR_1 // 左后 (Left Rear)
@@ -51,6 +55,9 @@ static uint8_t car_running = 0;         // 小车运行状态（1=运行，0=停
 static uint8_t car_speed_percent = 0;   // 当前整体速度百分比（0~100）
 static uint16_t car_target_station = 0; // 目标站点编号
 static uint8_t car_target_reached = 0;  // 到站锁存，避免重复触发
+static uint8_t obstacle_locked = 0;     // 避障锁（1=有障碍锁定）
+static track_ir_ctrl_output_t track_output = {0};
+static uint8_t track_line_mask = 0;
 
 #define CAR_STATION_MIN ((uint16_t)STATION_1)
 #define CAR_STATION_MAX ((uint16_t)STATION_2)
@@ -67,19 +74,10 @@ void CarCtrl_InitClosedLoop(void)
         // 恢复较高的积分限幅(2500)。要想达到稳态，单靠积分项必须能填补：3599(满载) - 1200(死区) = 2399 的差距。
         pid_init(&motor_pid[i], MOTOR_KP, MOTOR_KI, MOTOR_KD, (float)MOTOR_PWM_MAX_ARR, 2500.0f);
     }
-}
 
-/**
- * @brief 百分比转PWM值
- * @param percent 速度百分比（0~100）
- * @return int16_t PWM占空比
- */
-static int16_t CarCtrl_PercentToPwm(uint8_t percent)
-{
-  if (percent > 100U) {
-    percent = 100U;
-  }
-  return (int16_t)((CAR_PWM_MAX * percent) / 100U);
+  track_ir_algo_init();
+  obstacle_locked = 0U;
+  BEEP_Off();
 }
 
 /**
@@ -133,29 +131,46 @@ static void CarCtrl_CheckTargetStation(void)
   }
 }
 
-/**
- * @brief 按当前 car_speed_percent 设置所有电机速度
- */
-static void CarCtrl_ApplySpeed(void)
-{
-  int16_t pwm = CarCtrl_PercentToPwm(car_speed_percent);
-
-  motor_set_speed(MOTOR_1, pwm);
-  motor_set_speed(MOTOR_2, pwm);
-  motor_set_speed(MOTOR_3, pwm);
-  motor_set_speed(MOTOR_4, pwm);
-}
-
 /**
  * @brief 执行一步 PID 闭环计算并更新电机 (建议 10ms-20ms 调用一次)
  */
 void CarCtrl_UpdateClosedLoop(void)
 {
+  float dist;
+
     if (!car_running) {
+    obstacle_locked = 0U;
+    BEEP_Off();
+    track_ir_algo_reset();
         CarCtrl_StopAll();
         return;
     }
 
+  dist = sr04_get_distance();
+  if ((dist > 0.0f) && (dist < OBSTACLE_STOP_DISTANCE_CM)) {
+    obstacle_locked = 1U;
+  }
+
+  if (obstacle_locked != 0U) {
+    if ((dist > OBSTACLE_RECOVER_DISTANCE_CM) || (dist <= 0.0f)) {
+      obstacle_locked = 0U;
+      BEEP_Off();
+      elog_i(Protocol_TAG, "障碍解除，恢复循迹");
+    } else {
+      target_v_x = 0.0f;
+      target_v_y = 0.0f;
+      target_v_w = 0.0f;
+      BEEP_On();
+      CarCtrl_StopAll();
+      return;
+    }
+  }
+
+  BEEP_Off();
+
+  track_line_mask = track_ir_get_line_mask_filtered();
+  track_ir_algo_step(track_line_mask, &track_output);
+
     /* 麦克纳姆轮运动解算 (RPM单位)
      * 根据你的电机位置规定：
      * LR: M1, LF: M2, RF: M3, RR: M4
@@ -164,6 +179,9 @@ void CarCtrl_UpdateClosedLoop(void)
     
     // 从前进速度分量计算基础速度
     target_v_x = (float)car_speed_percent * TARGET_MAX_RPM / 100.0f;
+    target_v_x = target_v_x * (float)track_output.speed_scale_percent / 100.0f;
+    target_v_y = 0.0f;
+    target_v_w = track_output.yaw_correction_rpm;
     
     // 麦轮全向解算公式：
     // LF = Vx + Vy - Vw
@@ -281,10 +299,6 @@ static void CarCtrl_HandleCommand(const char *cmd_payload)
       car_speed_percent = (uint8_t)speed;
       elog_i(Protocol_TAG, "设置整体速度: %u%%", car_speed_percent);
 
-      // 如果当前处于运行状态，立即生效
-      if (car_running != 0U) {
-        CarCtrl_ApplySpeed();
-      }
       Protocol_SendFeedback("SP", 1);
     } else {
       elog_w(Protocol_TAG, "速度参数解析失败: %s", cmd_payload);
@@ -327,7 +341,7 @@ static void CarCtrl_HandleCommand(const char *cmd_payload)
  * @param payload 有效载荷文本 (不含帧头 LOGI:, 分隔符 :, 校验位 CS 和 帧尾 #)
  */
 static void Protocol_SendPacket(const char *payload) {
-  char packet[64] = {0};
+  char packet[192] = {0};
   uint8_t cs = 0;
 
   // 1. 组装基础段: LOGI:PAYLOAD
@@ -351,18 +365,20 @@ static void Protocol_SendPacket(const char *payload) {
 void Protocol_SendStatusReport(void) {
   char payload[128] = {0};
   float dist = sr04_get_distance();
-  uint8_t trk_mask = track_ir_get_line_mask();
+  uint8_t trk_mask = track_line_mask;
   
   // STAT:SP:xxx,STA:xxx,RUN:x,DIS:xxx.x,TRK:xxxx,RPM:m1:m2:m3:m4
   // TRK 此时上报 4 位二进制掩码 (H4 H3 H2 H1)
   // RPM 为各个电机的实际 RPM，保留整数
   snprintf(payload, sizeof(payload), 
-           "STAT:SP:%03u,STA:%03u,RUN:%u,DIS:%.1f,TRK:%u%u%u%u,RPM:%d:%d:%d:%d",
+           "STAT:SP:%03u,STA:%03u,RUN:%u,DIS:%.1f,TRK:%u%u%u%u,DEV:%d,OBS:%u,RPM:%d:%d:%d:%d",
            car_speed_percent, car_target_station, car_running, dist,
            (trk_mask & TRACK_IR_H4_BIT) ? 1 : 0,
            (trk_mask & TRACK_IR_H3_BIT) ? 1 : 0,
            (trk_mask & TRACK_IR_H2_BIT) ? 1 : 0,
            (trk_mask & TRACK_IR_H1_BIT) ? 1 : 0,
+           (int)track_output.deviation,
+           obstacle_locked,
            (int)hall_get_speed(MOTOR_1),
            (int)hall_get_speed(MOTOR_2),
            (int)hall_get_speed(MOTOR_3),
@@ -500,11 +516,8 @@ void CarCtrl_Task(void *argument) {
       last_report_tick = now;
     }
 
-    /*
-     * 与 hall_update_speed() 的 100ms 采样周期对齐，避免 PID 使用过期速度反复修正。
-     * 如果后续把测速周期改成 20ms，这里也要同步改回 20ms。
-     */
-    osDelay(100);
+    /* 与测速任务保持同周期，确保 PID 输入输出时基一致。 */
+    osDelay(CARCTRL_LOOP_MS);
   }
   /* USER CODE END CarCtrl_Task */
 }