feat: add steering wheel4 support

CMakeLists.txt

@@ -85,7 +85,7 @@ set(ENABLE_CONTROLLERS ON) # 启用 controller
 
 set(ENABLE_DJI_DRIVER ON)
 set(ENABLE_TB6612_DRIVER OFF)
-set(ENABLE_VESC_DRIVER OFF)
+set(MotorIF_UseVESC ON)
 set(ENABLE_DM_DRIVER OFF)
 add_subdirectory(Modules/motor_drivers/UserCode)
 

UserCode/CMakeLists.txt

@@ -37,6 +37,7 @@ set(ChassisIF_ChassisType "" CACHE STRING "select chassis type (required)")
 set_property(CACHE ChassisIF_ChassisType PROPERTY STRINGS
         MECANUM4
         OMNI4
+        STEERING4
 )
 
 if (ChassisIF_ChassisType STREQUAL "MECANUM4")
@@ -46,7 +47,11 @@ if (ChassisIF_ChassisType STREQUAL "MECANUM4")
 elseif (ChassisIF_ChassisType STREQUAL "OMNI4")
     list(APPEND ALL_SOURCES drivers/chassis_omni4.c)
     list(APPEND ALL_HEADERS "drivers/chassis_omni4.h")
-    list(APPEND DRIVER_MACROS CHASSIS_OMNI)
+    list(APPEND DRIVER_MACROS CHASSIS_OMNI4)
+elseif (ChassisIF_ChassisType STREQUAL "STEERING4")
+    list(APPEND ALL_SOURCES drivers/chassis_steering4.c)
+    list(APPEND ALL_HEADERS "drivers/chassis_steering4.h")
+    list(APPEND DRIVER_MACROS CHASSIS_STEERING4)
 else ()
     message(FATAL_ERROR "invalid ChassisIF_ChassisType: ${ChassisIF_ChassisType}")
 endif ()

UserCode/app/app.c

@@ -16,17 +16,35 @@
 #include "drivers/HWT101CT.h"
 #include "interfaces/chassis_if.h"
 #include "interfaces/motor_if.h"
+#include "drivers/steering_wheel.h"
+
+SteeringWheel_t wheel;
+Motor_VelCtrl_t driver_motor_ctrl;
+DJI_t           steerer_motor;
+VESC_t          driver_motor;
+Motor_VelCtrl_t steerer_motor_vel_ctrl;
+Motor_PosCtrl_t steerer_motor_pos_ctrl;
+GPIO_t          photogate = { .port = GPIOE, .pin = GPIO_PIN_12 };
+GPIO_PinState   gate;
 
 void TIM_Callback_1kHz(TIM_HandleTypeDef* htim)
 {
-    APP_Chassis_Update_1kHz();
+    // APP_Chassis_Update_1kHz();
+    //
+    // APP_Device_Update();
+    SteeringWheel_ControlUpdate(&wheel);
 
-    APP_Device_Update();
+    DJI_SendSetIqCommand(&hcan1, IQ_CMD_GROUP_1_4);
 }
 
 void TIM_Callback_200Hz(TIM_HandleTypeDef* htim)
 {
-    APP_Chassis_Update_200Hz();
+    // APP_Chassis_Update_200Hz();
+}
+
+void HAL_GPIO_EXTI_Callback(uint16_t pin)
+{
+    GPIO_EXTI_Callback(pin);
 }
 
 /**
@@ -37,22 +55,103 @@ void TIM_Callback_200Hz(TIM_HandleTypeDef* htim)
 void Init(void* argument)
 {
     /* 初始化代码 */
-    APP_Device_Init();
+    DJI_CAN_FilterInit(&hcan1, 0);
+    VESC_CAN_FilterInit(&hcan1, 1);
+    CAN_RegisterCallback(&hcan1, DJI_CAN_BaseReceiveCallback);
+    CAN_RegisterCallback(&hcan1, VESC_CAN_BaseReceiveCallback);
+
+    HAL_CAN_RegisterCallback(&hcan1, HAL_CAN_RX_FIFO0_MSG_PENDING_CB_ID, CAN_Fifo0ReceiveCallback);
+    CAN_Start(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING);
+
+    DJI_Init(&steerer_motor,
+             &(DJI_Config_t) { .auto_zero      = true,
+                               .hcan           = &hcan1,
+                               .id1            = 4,
+                               .motor_type     = M2006_C610,
+                               .reduction_rate = 2.0f,
+                               .reverse        = true });
+    VESC_Init(&driver_motor,
+              &(VESC_Config_t) {
+                      .auto_zero  = true,
+                      .hcan       = &hcan1,
+                      .id         = 23,
+                      .electrodes = 14,
+              });
+
+    Motor_VelCtrl_Init(
+            &steerer_motor_vel_ctrl,
+            &(Motor_VelCtrlConfig_t) {
+                    .motor      = &steerer_motor,
+                    .motor_type = MOTOR_TYPE_DJI,
+                    .pid = { .Kp = 60.0f, .Ki = 0.2f, .Kd = 0.0f, .abs_output_max = 4000.0f },
+            });
+
+    Motor_PosCtrl_Init(&steerer_motor_pos_ctrl,
+                       &(Motor_PosCtrlConfig_t) {
+                               .motor        = &steerer_motor,
+                               .motor_type   = MOTOR_TYPE_DJI,
+                               .velocity_pid = { .Kp             = 60.0f,
+                                                 .Ki             = 0.2f,
+                                                 .Kd             = 0.0f,
+                                                 .abs_output_max = 4000.0f },
+                           .position_pid = {
+                                   .Kp = 2.8f,
+                                   .Ki = 0.06f,
+                                   .Kd = 0.0f,
+                                   .abs_output_max = 250.0f,
+                           },
+                           .pos_vel_freq_ratio = 10,
+                       });
+    Motor_VelCtrl_Init(&driver_motor_ctrl,
+                       &(Motor_VelCtrlConfig_t) {
+                               .motor_type = MOTOR_TYPE_VESC,
+                               .motor      = &driver_motor,
+                       });
 
-    APP_Chassis_InitBeforeUpdate();
+    SteeringWheel_Init(&wheel,
+                       &(SteeringWheel_Config_t) {
+                               .drive_motor  = &driver_motor_ctrl,
+                               .steer_motor  = &steerer_motor_pos_ctrl,
+                               .steer_offset = 10.0f,
+                               .calib        = {
+                               .photogate = {
+                                   .port = GPIOE,
+                                   .pin = GPIO_PIN_12,
+                               },
+                               .photogate_triggered_state = GPIO_PIN_RESET,
+                               .steer_motor = &steerer_motor_vel_ctrl},
+                       });
 
-    // 注册定时器
     HAL_TIM_RegisterCallback(&htim6, HAL_TIM_PERIOD_ELAPSED_CB_ID, TIM_Callback_1kHz);
     HAL_TIM_Base_Start_IT(&htim6);
-    HAL_TIM_RegisterCallback(&htim13, HAL_TIM_PERIOD_ELAPSED_CB_ID, TIM_Callback_200Hz);
-    HAL_TIM_Base_Start_IT(&htim13);
 
-    // 一秒钟时间等待各种东西更新
+    __MOTOR_CTRL_ENABLE(&driver_motor_ctrl);
+
     osDelay(1000);
 
-    APP_Chassis_Init();
+    SteeringWheel_StartCalibration(&wheel);
+
+    while (wheel.calib.state != STEER_CALIB_DONE)
+        osDelay(1);
+    osDelay(2000);
+    SteeringWheel_SetVelocity(&wheel,
+                              &(SteeringWheel_Velocity_t) {
+                                      .angle = 90.0f,
+                                      .speed = 1.0f,
+                              });
+    osDelay(5000);
+    SteeringWheel_SetVelocity(&wheel,
+                              &(SteeringWheel_Velocity_t) { .angle = -45.0f, .speed = 1.0f });
+    osDelay(5000);
+    SteeringWheel_SetVelocity(&wheel,
+                              &(SteeringWheel_Velocity_t) { .angle = -90.0f, .speed = -1.0f });
+    osDelay(5000);
+    SteeringWheel_SetVelocity(&wheel, &(SteeringWheel_Velocity_t) { .angle = 0.0f, .speed = 2.0f });
 
-    osEventFlagsSet(systemEventHandle, SYSTEM_INITIALIZED);
+    for (;;)
+    {
+        osDelay(1);
+    }
 
     /* 初始化完成后退出线程 */
     osThreadExit();

UserCode/drivers/chassis_steering4.c

@@ -0,0 +1,176 @@
+/**
+ * @file    chassis_steering4.c
+ * @author  syhanjin
+ * @date    2025-10-03
+ */
+#include "chassis_steering4.h"
+
+#include <string.h>
+
+struct
+{
+    float kx, ky;
+} wheel_position[STEERING4_WHEEL_MAX] = { { 1, 1 }, { -1, 1 }, { -1, -1 }, { 1, -1 } };
+
+static void get_wheel_position(const Steering4_t*          chassis,
+                               const Steering4_WheelType_t wheel_type,
+                               float*                      xi,
+                               float*                      yi)
+{
+    *xi = wheel_position[wheel_type].kx * chassis->half_width;
+    *yi = wheel_position[wheel_type].ky * chassis->half_height;
+}
+
+/**
+ * 初始化四轮舵轮底盘
+ * @param chassis 底盘
+ * @param config 底盘配置
+ */
+void Steering4_Init(Steering4_t* chassis, const Steering4_Config_t* config)
+{
+    memset(chassis, 0, sizeof(Steering4_t));
+
+    chassis->half_width  = config->width * 0.5f;
+    chassis->half_height = config->height * 0.5f;
+    chassis->inv_l2      = 1.0f / (chassis->half_width * chassis->half_width +
+                              chassis->half_height * chassis->half_height);
+
+    chassis->radius = config->radius * 1e-3f; // mm to m
+    chassis->k_rpm  = 1.0f / (chassis->radius * 3.14159265358979323846f * 2) * 60.0f;
+
+    SteeringWheel_Init(&chassis->wheel[STEERING4_WHEEL_FR], &config->wheel_front_right);
+    SteeringWheel_Init(&chassis->wheel[STEERING4_WHEEL_FL], &config->wheel_front_left);
+    SteeringWheel_Init(&chassis->wheel[STEERING4_WHEEL_RL], &config->wheel_rear_left);
+    SteeringWheel_Init(&chassis->wheel[STEERING4_WHEEL_RR], &config->wheel_rear_right);
+}
+
+/**
+ * 底盘控制更新函数
+ *
+ * 本函数自动处理控制逻辑，并依序调用每个轮子的 PID 更新函数
+ *
+ * @note 推荐控制调用频率 1kHz，调用频率将会影响轮子的 PID 参数
+ * @param chassis 底盘
+ */
+void Steering4_Update(Steering4_t* chassis)
+{
+    if (!chassis->is_calibrated)
+    { // 检查校准状态
+        bool is_calibrated = true;
+        for (size_t i = 0; i < STEERING4_WHEEL_MAX; i++)
+            if (chassis->wheel[i].calib.state != STEER_CALIB_DONE)
+            {
+                is_calibrated = false;
+                break;
+            }
+        chassis->is_calibrated = is_calibrated;
+    }
+
+    // 更新控制
+    for (size_t i = 0; i < STEERING4_WHEEL_MAX; i++)
+    {
+        SteeringWheel_ControlUpdate(&chassis->wheel[i]);
+    }
+}
+
+/**
+ * 开启底盘舵轮校准
+ *
+ * @note 校准过程中舵轮会正转寻找光电门，定位零点
+ * @param chassis 底盘
+ */
+void Steering4_StartCalibration(Steering4_t* chassis)
+{
+    for (size_t i = 0; i < STEERING4_WHEEL_MAX; i++)
+    {
+        SteeringWheel_StartCalibration(&chassis->wheel[i]);
+    }
+}
+
+/**
+ * 设置底盘速度
+ *
+ * @param chassis 底盘
+ * @param vx
+ * @param vy
+ * @param wz
+ */
+void Steering4_ApplyVelocity(Steering4_t* chassis, const float vx, const float vy, const float wz)
+{
+    if (!chassis->is_calibrated)
+        // 必须先校准才能设置速度
+        return;
+
+    for (size_t i = 0; i < STEERING4_WHEEL_MAX; i++)
+    {
+        float xi = 0, yi = 0;
+        get_wheel_position(chassis, i, &xi, &yi);
+        const float vxi       = vx - yi * wz;
+        const float vyi       = vy + xi * wz;
+        const float speed_rpm = chassis->k_rpm * sqrtf(vxi * vxi + vyi * vyi);
+        if (fabsf(speed_rpm) < 1e-6f)
+        { // 速度为零
+            SteeringWheel_SetVelocity(&chassis->wheel[i],
+                                      &(SteeringWheel_Velocity_t) {
+                                              SteeringWheel_GetSteerAngle(&chassis->wheel[i]), 0 });
+        }
+        else
+        {
+            const float angle = RAD2DEG(atan2f(vyi, vxi));
+            SteeringWheel_SetVelocity(&chassis->wheel[i],
+                                      &(SteeringWheel_Velocity_t) { angle, speed_rpm });
+        }
+    }
+}
+
+static void steering4_velocity_forward(const Steering4_t* chassis, float* vx, float* vy, float* wz)
+{
+    *vx = 0, *vy = 0, *wz = 0;
+    for (size_t i = 0; i < STEERING4_WHEEL_MAX; i++)
+    {
+        float xi = 0, yi = 0;
+        get_wheel_position(chassis, i, &xi, &yi);
+        const float steer_angle  = SteeringWheel_GetSteerAngle(&chassis->wheel[i]);
+        const float driver_speed = SteeringWheel_GetDriveSpeed(&chassis->wheel[i]) / chassis->k_rpm;
+        const float steer_angle_rad = DEG2RAD(steer_angle);
+        const float sin_theta       = sinf(steer_angle_rad);
+        const float cos_theta       = cosf(steer_angle_rad);
+        *vx += driver_speed * cos_theta;
+        *vy += driver_speed * sin_theta;
+        *wz += -yi * cos_theta + xi * sin_theta;
+    }
+    *vx *= 0.25f;
+    *vy *= 0.25f;
+    *wz *= chassis->inv_l2;
+}
+
+float Steering4Forward_GetYaw(const Steering4_t* chassis)
+{
+    return 0;
+}
+float Steering4Forward_GetX(const Steering4_t* chassis)
+{
+    return 0;
+}
+float Steering4Forward_GetY(const Steering4_t* chassis)
+{
+    return 0;
+}
+float Steering4Forward_GetWz(const Steering4_t* chassis)
+{
+    float vx = 0, vy = 0, wz = 0;
+    steering4_velocity_forward(chassis, &vx, &vy, &wz);
+    return wz;
+}
+float Steering4Forward_GetVx(const Steering4_t* chassis)
+{
+    float vx = 0, vy = 0, wz = 0;
+    steering4_velocity_forward(chassis, &vx, &vy, &wz);
+    return vx;
+}
+float Steering4Forward_GetVy(const Steering4_t* chassis)
+{
+    float vx = 0, vy = 0, wz = 0;
+    steering4_velocity_forward(chassis, &vx, &vy, &wz);
+    return vy;
+}