feat: 添加 PID 控制器及霍尔传感器转速获取功能

CMakeLists.txt

@@ -55,6 +55,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
     ${CMAKE_CURRENT_SOURCE_DIR}/Core/Bsp/protocol.c
     ${CMAKE_CURRENT_SOURCE_DIR}/Core/Bsp/bsp_motor.c
     ${CMAKE_CURRENT_SOURCE_DIR}/Core/Bsp/bsp_hall.c
+    ${CMAKE_CURRENT_SOURCE_DIR}/Core/Bsp/bsp_pid.c
 )
 
 # Add include paths

Core/Bsp/bsp_hall.c

@@ -52,6 +52,17 @@ void hall_pulse_callback(uint16_t GPIO_Pin)
 //     return 0;
 // }
 
+/**
+ * @brief 获取当前实时转速 (RPM)
+ */
+float hall_get_speed(motor_id_t motor_id)
+{
+    if (motor_id < MOTOR_COUNT) {
+        return sensors[motor_id].speed_rpm;
+    }
+    return 0.0f;
+}
+
 // /**
 //  * @brief 重置霍尔传感器计数值
 //  */

Core/Bsp/bsp_hall.h

@@ -6,6 +6,7 @@
 #include "bsp_motor.h"  // 确保 motor_id_t 类型可见
 #include "elog.h"
 #include "cmsis_os.h"
+
 /**
  * @brief 霍尔周期/脉冲计数结构体
  */
@@ -14,6 +15,9 @@ typedef struct {
     float speed_rpm;           // 计算得到的转速 (RPM)
 } hall_sensor_t;
 
+/* 获取实时 RPM 接口 */
+float hall_get_speed(motor_id_t motor_id);
+
 /* 初始化霍尔传感器 (GPIO 外部中断模式) */
 void hall_init(void);
 

Core/Bsp/bsp_pid.c

@@ -0,0 +1,68 @@
+#include "bsp_pid.h"
+
+/**
+ * @brief 初始化 PID 控制器参数
+ * @param pid 指向 PID 结构体的指针
+ * @param kp 比例增益
+ * @param ki 积分增益
+ * @param kd 微分增益
+ * @param out_max 输出最大限幅
+ * @param int_max 积分最大限幅 (防止积分饱和)
+ */
+void pid_init(PID_TypeDef *pid, float kp, float ki, float kd, float out_max, float int_max) {
+    pid->target = 0.0f;
+    pid->actual = 0.0f;
+    pid->err = 0.0f;
+    pid->err_last = 0.0f;
+    pid->Kp = kp;
+    pid->Ki = ki;
+    pid->Kd = kd;
+    pid->integral = 0.0f;
+    pid->integral_max = int_max;
+    pid->output = 0.0f;
+    pid->output_max = out_max;
+}
+
+/**
+ * @brief 计算 PID 闭环输出
+ * @param pid 指向 PID 结构体的指针
+ * @param target 目标 RPM
+ * @param actual 实际反馈 RPM
+ * @return float 计算得出的 PWM 输出值
+ */
+float pid_calculate(PID_TypeDef *pid, float target, float actual) {
+    pid->target = target;
+    pid->actual = actual;
+    pid->err = target - actual;
+
+    /* 积分累加 */
+    pid->integral += pid->err;
+
+    /* 积分限幅 (Anti-Windup) */
+    if (pid->integral > pid->integral_max) pid->integral = pid->integral_max;
+    if (pid->integral < -pid->integral_max) pid->integral = -pid->integral_max;
+
+    /* PID 公式：Output = Kp*err + Ki*integral + Kd*(err - err_last) */
+    pid->output = (pid->Kp * pid->err) + 
+                  (pid->Ki * pid->integral) + 
+                  (pid->Kd * (pid->err - pid->err_last));
+
+    /* 记录误差用于下次计算 */
+    pid->err_last = pid->err;
+
+    /* 输出限幅 */
+    if (pid->output > pid->output_max) pid->output = pid->output_max;
+    if (pid->output < -pid->output_max) pid->output = -pid->output_max;
+
+    return pid->output;
+}
+
+/**
+ * @brief 重置 PID 内部状态 (通常在电机停止时调用)
+ */
+void pid_reset(PID_TypeDef *pid) {
+    pid->integral = 0.0f;
+    pid->err = 0.0f;
+    pid->err_last = 0.0f;
+    pid->output = 0.0f;
+}

Core/Bsp/bsp_pid.h

@@ -0,0 +1,30 @@
+#ifndef __BSP_PID_H
+#define __BSP_PID_H
+
+#include "main.h"
+
+/**
+ * @brief PID 参数结构体定义
+ */
+typedef struct {
+    float target;       // 目标值 (目标 RPM)
+    float actual;       // 实际值 (反馈 RPM)
+    float err;          // 当前误差
+    float err_last;     // 上次误差
+    float Kp, Ki, Kd;   // PID 增益
+    float integral;     // 误差积分累加器
+    float integral_max; // 积分限幅 (防止积分饱和)
+    float output;       // PID 计算输出值 (PWM)
+    float output_max;   // 输出限幅 (如 3599)
+} PID_TypeDef;
+
+/* 初始化 PID 控制器 */
+void pid_init(PID_TypeDef *pid, float kp, float ki, float kd, float out_max, float int_max);
+
+/* 计算 PID 输出 */
+float pid_calculate(PID_TypeDef *pid, float target, float actual);
+
+/* 重置 PID 内部状态 */
+void pid_reset(PID_TypeDef *pid);
+
+#endif /* __BSP_PID_H */

Core/Bsp/protocol.c

@@ -9,6 +9,8 @@
 
 #include "protocol.h"
 #include "bsp_motor.h"
+#include "bsp_hall.h"
+#include "bsp_pid.h"
 #include "bsp_uart.h"
 #include "checksum.h"
 #include "cmsis_os.h"
@@ -16,17 +18,49 @@
 #include <stdio.h>
 #include <string.h>
 
-
 /* 定义日志 TAG */
 #define Protocol_TAG "Protocol"
 
 /* 小车控制状态相关定义 */
 #define CAR_PWM_MAX 3599U // PWM最大值，对应100%
 
+/* PID 闭环参数，针对低速高摩擦环境优化 */
+#define MOTOR_KP 6.5f 
+#define MOTOR_KI 2.5f          // 微增 KI，消除最后 2-3 RPM 的静差
+#define MOTOR_KD 0.1f
+#define TARGET_MAX_RPM 50.0f   // 100% 对应 50 RPM
+#define MOTOR_PWM_DEADZONE 1200 // 保持死区，克服静摩擦
+#define MOTOR_PWM_MAX_ARR 3599 // 定时器重装载值
+
+/* 电机位置定义 (基于用户规定) */
+#define MOTOR_LR MOTOR_1 // 左后 (Left Rear)
+#define MOTOR_LF MOTOR_2 // 左前 (Left Front)
+#define MOTOR_RF MOTOR_3 // 右前 (Right Front)
+#define MOTOR_RR MOTOR_4 // 右后 (Right Rear)
+
+/* 运动解算临时变量 (RPM 单位) */
+static float target_v_x = 0;   // 前进后退分量 (-TARGET_MAX_RPM ~ TARGET_MAX_RPM)
+static float target_v_y = 0;   // 左右横移分量
+static float target_v_w = 0;   // 原地旋转分量
+
 static uint8_t car_running = 0;         // 小车运行状态（1=运行，0=停止）
 static uint8_t car_speed_percent = 0;   // 当前整体速度百分比（0~100）
 static uint16_t car_target_station = 0; // 目标站点编号
 
+/* 4个电机的 PID 控制器 */
+static PID_TypeDef motor_pid[MOTOR_COUNT];
+
+/**
+ * @brief 初始化闭环控制系统
+ */
+void CarCtrl_InitClosedLoop(void)
+{
+    for(int i=0; i<MOTOR_COUNT; i++) {
+        // 恢复较高的积分限幅(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）
@@ -45,10 +79,10 @@ static int16_t CarCtrl_PercentToPwm(uint8_t percent)
  */
 static void CarCtrl_StopAll(void)
 {
-  motor_stop(MOTOR_1);
-  motor_stop(MOTOR_2);
-  motor_stop(MOTOR_3);
-  motor_stop(MOTOR_4);
+  for(int i=0; i<MOTOR_COUNT; i++) {
+    motor_stop(i);
+    pid_reset(&motor_pid[i]);
+  }
 }
 
 /**
@@ -64,6 +98,70 @@ static void CarCtrl_ApplySpeed(void)
   motor_set_speed(MOTOR_4, pwm);
 }
 
+/**
+ * @brief 执行一步 PID 闭环计算并更新电机 (建议 10ms-20ms 调用一次)
+ */
+void CarCtrl_UpdateClosedLoop(void)
+{
+    if (!car_running) {
+        CarCtrl_StopAll();
+        return;
+    }
+
+    /* 麦克纳姆轮运动解算 (RPM单位)
+     * 根据你的电机位置规定：
+     * LR: M1, LF: M2, RF: M3, RR: M4
+     */
+    float target_rpms[MOTOR_COUNT];
+    
+    // 从前进速度分量计算基础速度
+    target_v_x = (float)car_speed_percent * TARGET_MAX_RPM / 100.0f;
+    
+    // 麦轮全向解算公式：
+    // LF = Vx + Vy - Vw
+    // RF = Vx - Vy + Vw
+    // LR = Vx - Vy - Vw
+    // RR = Vx + Vy + Vw
+    target_rpms[MOTOR_LF] = target_v_x + target_v_y - target_v_w; // 左前
+    target_rpms[MOTOR_RF] = target_v_x - target_v_y + target_v_w; // 右前
+    target_rpms[MOTOR_LR] = target_v_x - target_v_y - target_v_w; // 左后
+    target_rpms[MOTOR_RR] = target_v_x + target_v_y + target_v_w; // 右后
+
+    for(int i=0; i<MOTOR_COUNT; i++) {
+        float actual_rpm = hall_get_speed(i);
+        
+        // 限幅目标值，防止叠加后超出物理极限
+        if (target_rpms[i] > TARGET_MAX_RPM) target_rpms[i] = TARGET_MAX_RPM;
+        if (target_rpms[i] < -TARGET_MAX_RPM) target_rpms[i] = -TARGET_MAX_RPM;
+
+        float pid_out = pid_calculate(&motor_pid[i], target_rpms[i], actual_rpm);
+        
+        // 优化死区逻辑：当目标速度不为0时，叠加死区偏移
+        float final_pwm = 0;
+        if (target_rpms[i] > 1.0f || target_rpms[i] < -1.0f) {
+            if (pid_out >= 0) {
+                final_pwm = pid_out + MOTOR_PWM_DEADZONE;
+            } else {
+                final_pwm = pid_out - MOTOR_PWM_DEADZONE;
+            }
+            // 最终限幅，防止总 PWM 超过 ARR (3599)
+            if (final_pwm > 3599) final_pwm = 3599;
+            if (final_pwm < -3599) final_pwm = -3599;
+        }
+
+        motor_set_speed(i, (int16_t)final_pwm);
+        
+        // 修改打印逻辑：使用位置别名让日志更易读
+        const char* motor_names[] = {"LR", "LF", "RF", "RR"};
+        static uint32_t last_log_time = 0;
+        if (HAL_GetTick() - last_log_time > 500) { 
+            elog_d(Protocol_TAG, "M[%s] T:%.1f A:%.1f Pw:%d", 
+                   motor_names[i], target_rpms[i], actual_rpm, (int16_t)final_pwm);
+            if (i == MOTOR_COUNT - 1) last_log_time = HAL_GetTick();
+        }
+    }
+}
+
 /**
  * @brief 解析并执行协议命令
  * @param cmd_payload 队列传入的命令字符串（如 "ST:RUN"、"SP:080"、"GS:005"）
@@ -235,16 +333,23 @@ void Protocol_HandleMessage(uint8_t *data, uint16_t len) {
 void CarCtrl_Task(void *argument) {
   /* USER CODE BEGIN CarCtrl_Task */
   char cmd_payload[16] = {0};
+  
+  /* 初始化闭环 PID 控制器 */
+  CarCtrl_InitClosedLoop();
+
   /* Infinite loop */
   for (;;) {
-    /* 从消息队列中获取数据，阻塞等待 */
-    if (osMessageQueueGet(CmdQueueHandle, cmd_payload, NULL, osWaitForever) ==
-        osOK) {
-      elog_i(CarCtrlTask_TAG, "CarCtrl: Received command (ASCII: %s) from queue",
-             cmd_payload);
+    /* 1. 处理控制指令 (非阻塞获取，如果没有指令则继续执行闭环) */
+    if (osMessageQueueGet(CmdQueueHandle, cmd_payload, NULL, 0) == osOK) {
+      elog_d(CarCtrlTask_TAG, "CarCtrl: Command %s", cmd_payload);
       CarCtrl_HandleCommand(cmd_payload);
     }
-    // osDelay(1); // osMessageQueueGet 已经是阻塞的，不需要额外的 osDelay
+
+    /* 2. 执行 PID 闭环控制更新 */
+    CarCtrl_UpdateClosedLoop();
+
+    /* 闭环频率建议：20ms 次 (50Hz) */
+    osDelay(20);
   }
   /* USER CODE END CarCtrl_Task */
 }