From ccfb03f3b76dd579d93532574b3573e7106859a4 Mon Sep 17 00:00:00 2001
From: TuxMonkey <nqx_2004@qq.com>
Date: Mon, 20 Apr 2026 15:01:50 +0800
Subject: [PATCH 01/10] add emghandler

---
 application/cmd/robot_cmd.c | 54 ++++++++++++++++++++-----------------
 1 file changed, 29 insertions(+), 25 deletions(-)

diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c
index 59f39bb..4b7e3a2 100644
--- a/application/cmd/robot_cmd.c
+++ b/application/cmd/robot_cmd.c
@@ -153,27 +153,27 @@ static void RemoteControlSet()
     }
 
     // 右侧开关状态为[下],底盘旋转
-    if (switch_is_down(rc_data[TEMP].rc.switch_right))
-    {
-        chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
-
-        if (abs(rc_data[TEMP].rc.rocker_r_) > 300)
-        {
-            chassis_cmd_send.chassis_mode = CHASSIS_ROTATE;
-            chassis_cmd_send.rotate_w = 2;
-        }
-        if (abs(rc_data[TEMP].rc.rocker_r1) > 300)
-        {
-            chassis_cmd_send.chassis_mode = CHASSIS_ROTATE_REVERSE;
-            chassis_cmd_send.rotate_w = -2;
-        }
-        chassis_cmd_send.vx = 0;
-    }
-    else
-    {
+    // if (switch_is_down(rc_data[TEMP].rc.switch_right))
+    // {
+    //     chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
+    //
+    //     if (abs(rc_data[TEMP].rc.rocker_r_) > 300)
+    //     {
+    //         chassis_cmd_send.chassis_mode = CHASSIS_ROTATE;
+    //         chassis_cmd_send.rotate_w = 2;
+    //     }
+    //     if (abs(rc_data[TEMP].rc.rocker_r1) > 300)
+    //     {
+    //         chassis_cmd_send.chassis_mode = CHASSIS_ROTATE_REVERSE;
+    //         chassis_cmd_send.rotate_w = -2;
+    //     }
+    //     chassis_cmd_send.vx = 0;
+    // }
+    // else
+    // {
         chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
         chassis_cmd_send.vx = 0.003f * (-(float)rc_data[TEMP].rc.rocker_r_ + (float)rc_data[TEMP].rc.rocker_r1);
-    }
+    // }
 
     // 左侧开关状态为[中]，腿长控制
     if (switch_is_mid(rc_data[TEMP].rc.switch_left))
@@ -323,22 +323,26 @@ static void MouseKeySet()
  */
 static void EmergencyHandler()
 {
-    if (switch_is_down(rc_data[TEMP].rc.switch_left) || switch_is_mid(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[下],遥控器控制
+    // 获取遥控器在线状态 (1:在线 0:离线)
+    uint8_t rc_online = RemoteControlIsOnline();
+
+    if (switch_is_down(rc_data[TEMP].rc.switch_left) || switch_is_mid(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[下]或[中]
     {
-        if ((rc_data[TEMP].rc.dial > 300 && switch_is_down(rc_data[TEMP].rc.switch_left)) || robot_state == ROBOT_STOP) // 还需添加重要应用和模块离线的判断
+        // 急停触发条件：遥控器离线 或者 右侧开关打到[下] 或者 robot_state 已经是 STOP
+        if (!rc_online || switch_is_down(rc_data[TEMP].rc.switch_right) || robot_state == ROBOT_STOP)
         {
             robot_state = ROBOT_STOP;
             gimbal_cmd_send.gimbal_mode = GIMBAL_ZERO_FORCE;
-            chassis_cmd_send.chassis_mode = CHASSIS_ZERO_FORCE;
+            chassis_cmd_send.chassis_mode = CHASSIS_ZERO_FORCE; // 底盘电流归零，无力化
             shoot_cmd_send.shoot_mode = SHOOT_OFF;
             shoot_cmd_send.friction_mode = FRICTION_OFF;
             shoot_cmd_send.load_mode = LOAD_STOP;
 
-            gimbal_cmd_send.yaw = gimbal_fetch_data.gimbal_imu_data.YawTotalAngle; // 急停时设定值保持与实际值同步，避免恢复时疯转
+            gimbal_cmd_send.yaw = gimbal_fetch_data.gimbal_imu_data.YawTotalAngle; // 急停时设定值保持与实际值同步
             gimbal_cmd_send.pitch = gimbal_fetch_data.gimbal_imu_data.Pitch;
         }
-        // 遥控器右侧开关为[上],恢复正常运行
-        if (switch_is_up(rc_data[TEMP].rc.switch_right))
+        // 恢复正常运行条件：遥控器在线 且 右侧开关状态为[上]
+        else if (switch_is_up(rc_data[TEMP].rc.switch_right) || switch_is_mid(rc_data[TEMP].rc.switch_right))
         {
             robot_state = ROBOT_READY;
             shoot_cmd_send.shoot_mode = SHOOT_ON;

From 70ff2815f347b0254ab969006f5191624081e65d Mon Sep 17 00:00:00 2001
From: TuxMonkey <nqx_2004@qq.com>
Date: Mon, 20 Apr 2026 15:28:26 +0800
Subject: [PATCH 02/10] changed damiao controller

---
 application/robot_def.h         |   4 +-
 modules/motor/DMmotor/dmmotor.c | 130 ++++++++++++++++++++++----------
 modules/motor/DMmotor/dmmotor.h |  37 +++++----
 modules/motor/motor_def.h       |   2 +
 4 files changed, 115 insertions(+), 58 deletions(-)

diff --git a/application/robot_def.h b/application/robot_def.h
index 776457a..171c62a 100644
--- a/application/robot_def.h
+++ b/application/robot_def.h
@@ -17,8 +17,8 @@
 #include "stdint.h"
 
 /* 开发板类型定义,烧录时注意不要弄错对应功能;修改定义后需要重新编译,只能存在一个定义! */
-// #define ONE_BOARD // 单板控制整车
-#define CHASSIS_BOARD //底盘板
+#define ONE_BOARD // 单板控制整车
+// #define CHASSIS_BOARD //底盘板
 // #define GIMBAL_BOARD  //云台板
 
 #define VISION_USE_VCP  // 使用虚拟串口发送视觉数据
diff --git a/modules/motor/DMmotor/dmmotor.c b/modules/motor/DMmotor/dmmotor.c
index fad8221..0873988 100644
--- a/modules/motor/DMmotor/dmmotor.c
+++ b/modules/motor/DMmotor/dmmotor.c
@@ -1,5 +1,4 @@
 #include "dmmotor.h"
-#include "memory.h"
 #include "general_def.h"
 #include "user_lib.h"
 #include "cmsis_os.h"
@@ -8,16 +7,19 @@
 #include "stdlib.h"
 #include "bsp_log.h"
 
+#define DM_SEND_DELAY 0.2
+
 static uint8_t idx;
 static DMMotorInstance *dm_motor_instance[DM_MOTOR_CNT];
 static osThreadId dm_task_handle[DM_MOTOR_CNT];
-/* 两个用于将uint值和float值进行映射的函数,在设定发送值和解析反馈值时使用 */
+
 static uint16_t float_to_uint(float x, float x_min, float x_max, uint8_t bits)
 {
     float span = x_max - x_min;
     float offset = x_min;
     return (uint16_t)((x - offset) * ((float)((1 << bits) - 1)) / span);
 }
+
 static float uint_to_float(int x_int, float x_min, float x_max, int bits)
 {
     float span = x_max - x_min;
@@ -25,53 +27,64 @@ static float uint_to_float(int x_int, float x_min, float x_max, int bits)
     return ((float)x_int) * span / ((float)((1 << bits) - 1)) + offset;
 }
 
-static void DMMotorSetMode(DMMotor_Mode_e cmd, DMMotorInstance *motor)
+void DMMotorSetMode(DMMotor_Mode_e cmd, DMMotorInstance *motor)
 {
-    memset(motor->motor_can_instace->tx_buff, 0xff, 7);  // 发送电机指令的时候前面7bytes都是0xff
-    motor->motor_can_instace->tx_buff[7] = (uint8_t)cmd; // 最后一位是命令id
+    memset(motor->motor_can_instace->tx_buff, 0xff, 7);
+    motor->motor_can_instace->tx_buff[7] = (uint8_t)cmd;
     CANTransmit(motor->motor_can_instace, 1);
+	DWT_Delay(DM_SEND_DELAY);
 }
 
 static void DMMotorDecode(CANInstance *motor_can)
 {
-    uint16_t tmp; // 用于暂存解析值,稍后转换成float数据,避免多次创建临时变量
+    uint16_t tmp;
     uint8_t *rxbuff = motor_can->rx_buff;
     DMMotorInstance *motor = (DMMotorInstance *)motor_can->id;
-    DM_Motor_Measure_s *measure = &(motor->measure); // 将can实例中保存的id转换成电机实例的指针
+    DM_Motor_Measure_s *measure = &(motor->measure);
 
     DaemonReload(motor->motor_daemon);
-
+    measure->id = rxbuff[0];
+    measure->state = (rxbuff[0] >> 4) & 0xf;
     measure->last_position = measure->position;
     tmp = (uint16_t)((rxbuff[1] << 8) | rxbuff[2]);
     measure->position = uint_to_float(tmp, DM_P_MIN, DM_P_MAX, 16);
-
+    measure->angle_single_round = measure->position / (2 * PI) * 360.0f;
+    if (measure->position - measure->last_position > PI)
+        measure->total_round--;
+    else if (measure->position - measure->last_position < -PI)
+        measure->total_round++;
+    measure->total_angle = measure->total_round * 360.0f + measure->position / (2 * PI) * 360.0f;
     tmp = (uint16_t)((rxbuff[3] << 4) | rxbuff[4] >> 4);
     measure->velocity = uint_to_float(tmp, DM_V_MIN, DM_V_MAX, 12);
-
     tmp = (uint16_t)(((rxbuff[4] & 0x0f) << 8) | rxbuff[5]);
     measure->torque = uint_to_float(tmp, DM_T_MIN, DM_T_MAX, 12);
-
     measure->T_Mos = (float)rxbuff[6];
     measure->T_Rotor = (float)rxbuff[7];
 }
 
 static void DMMotorLostCallback(void *motor_ptr)
 {
+    DMMotorInstance *motor = (DMMotorInstance *)motor_ptr;
+    DMMotorEnable(motor);
+    DMMotorSetMode(DM_CMD_MOTOR_MODE, motor);
 }
+
 void DMMotorCaliEncoder(DMMotorInstance *motor)
 {
     DMMotorSetMode(DM_CMD_ZERO_POSITION, motor);
-    DWT_Delay(0.1);
 }
+
 DMMotorInstance *DMMotorInit(Motor_Init_Config_s *config)
 {
     DMMotorInstance *motor = (DMMotorInstance *)malloc(sizeof(DMMotorInstance));
     memset(motor, 0, sizeof(DMMotorInstance));
-    
+
+	motor->motor_type = config->motor_type;
     motor->motor_settings = config->controller_setting_init_config;
     PIDInit(&motor->current_PID, &config->controller_param_init_config.current_PID);
     PIDInit(&motor->speed_PID, &config->controller_param_init_config.speed_PID);
     PIDInit(&motor->angle_PID, &config->controller_param_init_config.angle_PID);
+    PIDInit(&motor->torque_PID, &config->controller_param_init_config.torque_PID);
     motor->other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr;
     motor->other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr;
 
@@ -84,13 +97,8 @@ DMMotorInstance *DMMotorInit(Motor_Init_Config_s *config)
         .owner_id = motor,
         .reload_count = 10,
     };
-    motor->motor_daemon = DaemonRegister(&conf);
 
-    DMMotorEnable(motor);
-    DMMotorSetMode(DM_CMD_MOTOR_MODE, motor);
-    DWT_Delay(0.1);
-    DMMotorCaliEncoder(motor);
-    DWT_Delay(0.1);
+    motor->motor_daemon = DaemonRegister(&conf);
     dm_motor_instance[idx++] = motor;
     return motor;
 }
@@ -105,7 +113,7 @@ void DMMotorEnable(DMMotorInstance *motor)
     motor->stop_flag = MOTOR_ENALBED;
 }
 
-void DMMotorStop(DMMotorInstance *motor)//不使用使能模式是因为需要收到反馈
+void DMMotorStop(DMMotorInstance *motor)
 {
     motor->stop_flag = MOTOR_STOP;
 }
@@ -115,33 +123,60 @@ void DMMotorOuterLoop(DMMotorInstance *motor, Closeloop_Type_e type)
     motor->motor_settings.outer_loop_type = type;
 }
 
-
-//@Todo: 目前只实现了力控，更多位控PID等请自行添加
 void DMMotorTask(void const *argument)
 {
-    float  pid_ref, set;
+    float pid_ref, set;
     DMMotorInstance *motor = (DMMotorInstance *)argument;
-   //DM_Motor_Measure_s *measure = &motor->measure;
-    Motor_Control_Setting_s *setting = &motor->motor_settings;
-    //CANInstance *motor_can = motor->motor_can_instace;
-    //uint16_t tmp;
+    DM_Motor_Measure_s *measure;
+    Motor_Control_Setting_s *setting;
     DMMotor_Send_s motor_send_mailbox;
+	portTickType currentTime;
+	const portTickType xFrequency = pdMS_TO_TICKS(4);
     while (1)
     {
-        pid_ref = motor->pid_ref;
-        
-        set = pid_ref;
+		currentTime = xTaskGetTickCount();
+		set = motor->pid_ref;
+
         if (setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE)
             set *= -1;
-       
-        LIMIT_MIN_MAX(set, DM_T_MIN, DM_T_MAX);
+
+		measure = &motor->measure;
+		setting = &motor->motor_settings;
+		if ((setting->close_loop_type & ANGLE_LOOP) && (setting->outer_loop_type & ANGLE_LOOP))
+		{
+			if (setting->angle_feedback_source == OTHER_FEED)
+			{
+				set = PIDCalculate(&motor->angle_PID, *motor->other_angle_feedback_ptr, set);
+			}
+			else if (setting->angle_feedback_source == MOTOR_FEED)
+			{
+				set = PIDCalculate(&motor->angle_PID, measure->position, set);;
+			}
+		}
+		if ((setting->close_loop_type & SPEED_LOOP) && (setting->outer_loop_type & (SPEED_LOOP | ANGLE_LOOP)))
+		{
+			if (setting->speed_feedback_source == OTHER_FEED)
+			{
+				set = PIDCalculate(&motor->speed_PID, *motor->other_speed_feedback_ptr, set);
+			}
+			else if (setting->speed_feedback_source == MOTOR_FEED)
+			{
+				set = PIDCalculate(&motor->speed_PID, measure->velocity, set);
+			}
+		}
+		if ((setting->close_loop_type & TORQUE_LOOP) && (setting->outer_loop_type & (TORQUE_LOOP | SPEED_LOOP | ANGLE_LOOP)))
+		{
+			set = PIDCalculate(&motor->torque_PID, 0, set);
+		}
+        pid_ref = set;
+        LIMIT_MIN_MAX(pid_ref, DM_T_MIN, DM_T_MAX);
         motor_send_mailbox.position_des = float_to_uint(0, DM_P_MIN, DM_P_MAX, 16);
         motor_send_mailbox.velocity_des = float_to_uint(0, DM_V_MIN, DM_V_MAX, 12);
         motor_send_mailbox.torque_des = float_to_uint(pid_ref, DM_T_MIN, DM_T_MAX, 12);
         motor_send_mailbox.Kp = 0;
         motor_send_mailbox.Kd = 0;
 
-        if(motor->stop_flag == MOTOR_STOP)
+        if (motor->stop_flag == MOTOR_STOP)
             motor_send_mailbox.torque_des = float_to_uint(0, DM_T_MIN, DM_T_MAX, 12);
 
         motor->motor_can_instace->tx_buff[0] = (uint8_t)(motor_send_mailbox.position_des >> 8);
@@ -153,23 +188,38 @@ void DMMotorTask(void const *argument)
         motor->motor_can_instace->tx_buff[6] = (uint8_t)(((motor_send_mailbox.Kd & 0xF) << 4) | (motor_send_mailbox.torque_des >> 8));
         motor->motor_can_instace->tx_buff[7] = (uint8_t)(motor_send_mailbox.torque_des);
 
-        CANTransmit(motor->motor_can_instace, 1);
+		CANTransmit(motor->motor_can_instace, 1);
 
-        osDelay(2);
+    	osDelayUntil(&currentTime, 4); // 4ms 的绝对延时
     }
 }
-void DMMotorControlInit()
+
+// /**
+//   * @brief Keil的runtime中的stdlib.h没有提供__itoa()，此处为该函数的手动实现
+//   * @param n 源数据
+//   * @param s 目标字符串
+//   * @param b 目标进制 有效范围2-36
+//   * @attention 没加特判，注意整数溢出/字符串数组越界/进制范围限制
+//   **/
+// void __itoa(int n,char *s,int b)
+// {
+// 	char *p = s; int sg = n < 0; n = sg ? -n : n;
+// 	do { *p++ = "0123456789ABCDEFGHIJKLMNOPQRSTUVWXYZ"[n % b];} while(n /= b);
+// 	sg ? (*p++ = '-') : 0;
+// 	*p = '\0';
+// 	for(char *h = s,*t = p-1; h < t; h++, t--)(*h ^= *t,*t ^= *h,*h ^= *t);
+// }
+
+void DMMotorControlInit(void)
 {
-    char dm_task_name[5] = "dm";
-    // 遍历所有电机实例,创建任务
     if (!idx)
         return;
     for (size_t i = 0; i < idx; i++)
     {
-        char dm_id_buff[2] = {0};
+		char dm_task_name[5] = "dm", dm_id_buff[2] = {0};
         __itoa(i, dm_id_buff, 10);
         strcat(dm_task_name, dm_id_buff);
         osThreadDef(dm_task_name, DMMotorTask, osPriorityNormal, 0, 128);
         dm_task_handle[i] = osThreadCreate(osThread(dm_task_name), dm_motor_instance[i]);
     }
-}
\ No newline at end of file
+}
diff --git a/modules/motor/DMmotor/dmmotor.h b/modules/motor/DMmotor/dmmotor.h
index 8f03e28..d4e77a9 100644
--- a/modules/motor/DMmotor/dmmotor.h
+++ b/modules/motor/DMmotor/dmmotor.h
@@ -6,7 +6,7 @@
 #include "motor_def.h"
 #include "daemon.h"
 
-#define DM_MOTOR_CNT 4
+#define DM_MOTOR_CNT 5
 
 #define DM_P_MIN  (-12.5f)
 #define DM_P_MAX  12.5f
@@ -15,18 +15,22 @@
 #define DM_T_MIN  (-18.0f)
 #define DM_T_MAX   18.0f
 
-typedef struct 
+#define DM_REDUCE 9
+
+typedef struct
 {
     uint8_t id;
     uint8_t state;
     float velocity;
     float last_position;
     float position;
+    float angle_single_round;
+    float total_angle;
     float torque;
     float T_Mos;
     float T_Rotor;
     int32_t total_round;
-}DM_Motor_Measure_s;
+} DM_Motor_Measure_s;
 
 typedef struct
 {
@@ -35,12 +39,14 @@ typedef struct
     uint16_t torque_des;
     uint16_t Kp;
     uint16_t Kd;
-}DMMotor_Send_s;
-typedef struct 
+} DMMotor_Send_s;
+typedef struct
 {
+    Motor_Type_e motor_type;
     DM_Motor_Measure_s measure;
     Motor_Control_Setting_s motor_settings;
     PIDInstance current_PID;
+    PIDInstance torque_PID;
     PIDInstance speed_PID;
     PIDInstance angle_PID;
     float *other_angle_feedback_ptr;
@@ -50,27 +56,26 @@ typedef struct
     float pid_ref;
     Motor_Working_Type_e stop_flag;
     CANInstance *motor_can_instace;
-    DaemonInstance* motor_daemon;
+    DaemonInstance *motor_daemon;
     uint32_t lost_cnt;
-}DMMotorInstance;
+} DMMotorInstance;
 
 typedef enum
 {
-    DM_CMD_MOTOR_MODE = 0xfc,   // 使能,会响应指令
-    DM_CMD_RESET_MODE = 0xfd,   // 停止
+    DM_CMD_MOTOR_MODE = 0xfc,    // 使能,会响应指令
+    DM_CMD_RESET_MODE = 0xfd,    // 停止
     DM_CMD_ZERO_POSITION = 0xfe, // 将当前的位置设置为编码器零位
-    DM_CMD_CLEAR_ERROR = 0xfb // 清除电机过热错误
-}DMMotor_Mode_e;
+    DM_CMD_CLEAR_ERROR = 0xfb    // 清除电机过热错误
+} DMMotor_Mode_e;
 
 DMMotorInstance *DMMotorInit(Motor_Init_Config_s *config);
 
 void DMMotorSetRef(DMMotorInstance *motor, float ref);
-
-void DMMotorOuterLoop(DMMotorInstance *motor,Closeloop_Type_e closeloop_type);
-
+void DMMotorOuterLoop(DMMotorInstance *motor, Closeloop_Type_e closeloop_type);
 void DMMotorEnable(DMMotorInstance *motor);
-
 void DMMotorStop(DMMotorInstance *motor);
 void DMMotorCaliEncoder(DMMotorInstance *motor);
-void DMMotorControlInit();
+DMMotorInstance *DMMotorInit(Motor_Init_Config_s *config);
+void DMMotorControlInit(void);
+void DMMotorSetMode(DMMotor_Mode_e cmd, DMMotorInstance *motor);
 #endif // !DMMOTOR
\ No newline at end of file
diff --git a/modules/motor/motor_def.h b/modules/motor/motor_def.h
index 8bd3cd2..eb60c69 100644
--- a/modules/motor/motor_def.h
+++ b/modules/motor/motor_def.h
@@ -27,6 +27,7 @@ typedef enum
     CURRENT_LOOP = 0b0001,
     SPEED_LOOP = 0b0010,
     ANGLE_LOOP = 0b0100,
+    TORQUE_LOOP = 0b1000,
 
     // only for checking
     SPEED_AND_CURRENT_LOOP = 0b0011,
@@ -125,6 +126,7 @@ typedef struct
     PID_Init_Config_s current_PID;
     PID_Init_Config_s speed_PID;
     PID_Init_Config_s angle_PID;
+    PID_Init_Config_s torque_PID;
 } Motor_Controller_Init_s;
 
 /* 用于初始化CAN电机的结构体,各类电机通用 */

From 8a13444e48560c3546ccb0ebec954cbb5cb6fbfb Mon Sep 17 00:00:00 2001
From: TuxMonkey <8196772+tuxmonkey@user.noreply.gitee.com>
Date: Tue, 14 Jul 2026 01:07:48 +0800
Subject: [PATCH 03/10] Codex generated changes: 1.stool-mode 2.INS direction
 changes not tested,not ok

---
 application/chassis/balance.c     | 419 +++++++++++++++++-------------
 application/chassis/lqr_calc.h    |  28 +-
 application/cmd/robot_cmd.c       |   4 +
 application/robot_def.h           |   7 +-
 modules/algorithm/QuaternionEKF.c |   4 +-
 modules/motor/DMmotor/dmmotor.c   |   8 +-
 modules/referee/referee_task.c    |  14 +-
 7 files changed, 286 insertions(+), 198 deletions(-)

diff --git a/application/chassis/balance.c b/application/chassis/balance.c
index 4c49b83..a3f0074 100644
--- a/application/chassis/balance.c
+++ b/application/chassis/balance.c
@@ -21,6 +21,22 @@
 #include "speed_estimation.h"
 #include "fly_detection.h"
 #include "buzzer.h"
+#include "math.h"
+#include "string.h"
+
+#define STOOL_SPEED_REF 1.5f
+#define STOOL_YAW_RATE_REF 3.5f
+#define STOOL_ACC_REF 4.0f
+#define STOOL_BRAKE_ACC_REF 8.0f
+#define STOOL_PITCH_K 55.0f
+#define STOOL_PITCH_W_K 7.0f
+#define STOOL_VEL_K 10.0f
+#define STOOL_STOP_DIST_K 8.0f
+#define STOOL_VEL_LPF_RC 0.03f
+#define STOOL_WHEEL_TORQUE_LIMIT 25.0f
+#define STOOL_RC_DEADBAND 40
+#define CHASSIS_UNDERVOLTAGE_LIMIT 15.0f
+#define REFEREE_VOLTAGE_VALID_MIN 5.0f
 // 计时变量
 static uint32_t balance_dwt_cnt;
 static float del_t;
@@ -29,8 +45,6 @@ static float del_t;
 static INS_t *Chassis_IMU_data;
 static RC_ctrl_t *rc_data; // 底盘单独调试用
 static Chassis_Ctrl_Cmd_s chassis_cmd_recv;
-static Chassis_Upload_Data_s chassis_feedback_data; // 底盘反馈数据
-static Chassis_Can_Comm chassis_can_recv;
 // 四个关节电机和两个驱动轮电机
 static DMMotorInstance *lf, *lb, *rf, *rb, *joint[4]; // 指针数组方便传参和调试
 static LKMotorInstance *l_driven, *r_driven, *driven[2];
@@ -51,10 +65,112 @@ static Robot_Status_e chassis_status;
 static referee_info_t *referee_data;       // 用于获取裁判系统的数据
 static Referee_Interactive_info_t ui_data; // UI数据，将底盘中的数据传入此结构体的对应变量中，UI会自动检测是否变化，对应显示UI
 
-static CANCommInstance *cmd_can_comm; // 底盘CAN通信实例
-
 static SuperCapInstance *cap; // 超级电容
-static uint16_t DataSend2Cap[4] = {0, 0, 0, 0};
+
+static uint8_t stool_drive_key_pressed;
+static float stool_vel;
+static float stool_dist;
+static float stool_stop_dist;
+static float stool_target_wz;
+static chassis_mode_e last_chassis_mode = CHASSIS_ZERO_FORCE;
+
+static void ClearMotionTargets(void)
+{
+    chassis.target_v = 0.0f;
+    chassis.target_dist = chassis.dist;
+    stool_target_wz = 0.0f;
+}
+
+static void ClearControlOutput(void)
+{
+    l_side.F_leg = 0.0f;
+    l_side.T_hip = 0.0f;
+    l_side.T_back = 0.0f;
+    l_side.T_front = 0.0f;
+    l_side.T_wheel = 0.0f;
+
+    r_side.F_leg = 0.0f;
+    r_side.T_hip = 0.0f;
+    r_side.T_back = 0.0f;
+    r_side.T_front = 0.0f;
+    r_side.T_wheel = 0.0f;
+}
+
+static void ClearJointOutput(void)
+{
+    l_side.F_leg = 0.0f;
+    l_side.T_hip = 0.0f;
+    l_side.T_back = 0.0f;
+    l_side.T_front = 0.0f;
+
+    r_side.F_leg = 0.0f;
+    r_side.T_hip = 0.0f;
+    r_side.T_back = 0.0f;
+    r_side.T_front = 0.0f;
+}
+
+static void ApproachFloat(float *value, float target, float step)
+{
+    if (*value < target)
+    {
+        *value += step;
+        if (*value > target)
+            *value = target;
+    }
+    else if (*value > target)
+    {
+        *value -= step;
+        if (*value < target)
+            *value = target;
+    }
+}
+
+static float RCChannelToRatio(int16_t ch)
+{
+    if (ch > -STOOL_RC_DEADBAND && ch < STOOL_RC_DEADBAND)
+        return 0.0f;
+    return float_constrain((float)ch / 660.0f, -1.0f, 1.0f);
+}
+
+static void ResetStoolRuntime(void)
+{
+    stool_drive_key_pressed = 0;
+    stool_vel = 0.0f;
+    stool_dist = chassis.dist;
+    stool_stop_dist = stool_dist;
+    ClearMotionTargets();
+    ClearControlOutput();
+}
+
+static void DisableAllJointMotor(void)
+{
+    for (uint8_t i = 0; i < JOINT_CNT; i++)
+    {
+        DMMotorSetRef(joint[i], 0.0f);
+        DMMotorOuterLoop(joint[i], OPEN_LOOP);
+        DMMotorStop(joint[i]);
+    }
+}
+
+static void RefreshJointMotorDisable(void)
+{
+    DisableAllJointMotor();
+}
+
+static void EnableDrivenMotor(void)
+{
+    for (uint8_t i = 0; i < DRIVEN_CNT; i++)
+        LKMotorEnable(driven[i]);
+}
+
+static void StopDrivenMotor(void)
+{
+    for (uint8_t i = 0; i < DRIVEN_CNT; i++)
+    {
+        LKMotorSetRef(driven[i], 0.0f);
+        LKMotorStop(driven[i]);
+    }
+}
 
 void BalanceInit()
 {
@@ -68,18 +184,6 @@ void BalanceInit()
             .rx_id = 0x301, // 超级电容默认发送id,注意tx和rx在其他人看来是反的
         }};
     cap = SuperCapInit(&cap_conf); // ww超级电容初始化
-    CANComm_Init_Config_s comm_conf = {
-        .can_config = {
-            .can_handle = &hcan2,
-            .tx_id = 0x311,
-            .rx_id = 0x312,
-        },
-        .daemon_count = 100,
-        .recv_data_len = sizeof(Chassis_Ctrl_Cmd_s),
-        .send_data_len = sizeof(Chassis_Upload_Data_s),
-    };
-    cmd_can_comm = CANCommInit(&comm_conf);
-
     // 关节电机
     Motor_Init_Config_s joint_conf = {
         // 写一个,剩下的修改方向和id即可
@@ -200,28 +304,14 @@ void BalanceInit()
     // 状态初始化
     l_side.target_len = r_side.target_len = 0.12;
     chassis.vel_cov = 100; // 速度协方差初始化
-    chassis_status = ROBOT_READY;
-    DWT_GetDeltaT(&balance_dwt_cnt);
-}
-
-static void EnableAllMotor() /* 打开所有电机 */
-{
-    for (uint8_t i = 0; i < JOINT_CNT; i++) // 打开关节电机
-        DMMotorEnable(joint[i]);
-    for (uint8_t i = 0; i < DRIVEN_CNT; i++) // 打开驱动电机
-        LKMotorEnable(driven[i]);
-}
-
-// 检查关节电机是否离线
-static uint8_t JointMotorIsLost()
-{
+    chassis_cmd_recv.chassis_mode = CHASSIS_ZERO_FORCE;
+    chassis_status = ROBOT_STOP;
+    ResetStoolRuntime();
+    DisableAllJointMotor();
     for (uint8_t i = 0; i < JOINT_CNT; i++)
-    {
-        if (joint[i]->motor_daemon->temp_count == 0)
-            return 1;
-    }
-
-    return 0;
+        DMMotorSetMode(DM_CMD_RESET_MODE, joint[i]);
+    StopDrivenMotor();
+    DWT_GetDeltaT(&balance_dwt_cnt);
 }
 
 // 检查驱动轮电机是否离线
@@ -236,147 +326,100 @@ static uint8_t DrivenMotorIsLost()
     return 0;
 }
 
-/* 切换底盘遥控器控制和云台双板控制 */
+/* Chassis-board local RC control. Right switch down is hard emergency stop. */
 static void ControlSwitch()
 {
-    float chassis_vol = referee_data->PowerHeatData.chassis_voltage * 0.001;
+    memset(&chassis_cmd_recv, 0, sizeof(chassis_cmd_recv));
+    chassis_cmd_recv.chassis_mode = CHASSIS_ZERO_FORCE;
+    chassis_cmd_recv.direction = CHASSIS_ALIGN;
+    chassis_cmd_recv.friction_mode = FRICTION_OFF;
+    chassis_cmd_recv.loader_mode = LOAD_STOP;
+    chassis_cmd_recv.vision_mode = UNLOCK;
+    chassis_cmd_recv.ui_mode = UI_KEEP;
 
-    // 根据裁判系统底盘输出电压设定底盘状态
-    if (chassis_vol < 15.0f || JointMotorIsLost() || DrivenMotorIsLost())
+    if (!RemoteControlIsOnline())
     {
-        chassis_cmd_recv.chassis_mode = CHASSIS_ZERO_FORCE; // 皆离线,急停
         return;
     }
 
-    // // 右侧拨杆向下,进入遥控器底盘控制,此时不响应云台控制指令
-    // if (switch_is_down(rc_data->rc.switch_right) && RemoteControlIsOnline())
-    // {
-    //     if (switch_is_up(rc_data->rc.switch_left))
-    //     {
-    //         chassis_cmd_recv.chassis_mode = CHASSIS_RESET;
-    //         chassis_cmd_recv.vx = 0.5 * (float)rc_data[TEMP].rc.rocker_r1; // speed x, unit m/s
-    //         chassis_cmd_recv.rotate_w = 0.5 * (float)rc_data[TEMP].rc.rocker_r_;
-    //     }
-    //     else
-    //     {
-    //         chassis_cmd_recv.chassis_mode = CHASSIS_FREE_DEBUG;              // 自由转动&前后
-    //         chassis_cmd_recv.vx = 0.003 * (float)rc_data[TEMP].rc.rocker_r1; // speed x, unit m/s
-    //         chassis_cmd_recv.delta_leglen = -0.0000005f * (float)rc_data[TEMP].rc.dial;
-    //         chassis_cmd_recv.offset_angle -= 0.000005 * (float)rc_data[TEMP].rc.rocker_r_;
-    //     }
-    // }
-    // else
-    // {
-    //     chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(cmd_can_comm);
-    // }
-    chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(cmd_can_comm);
-    // if (abs(l_side.theta) > (30.0f * DEGREE_2_RAD) || abs(r_side.theta) > (30.0f * DEGREE_2_RAD))
-    // {
-    //     chassis_cmd_recv.chassis_mode = CHASSIS_RESET;
-    // }
+    if (switch_is_down(rc_data[TEMP].rc.switch_right))
+        return;
+
+    float chassis_vol = referee_data->PowerHeatData.chassis_voltage * 0.001f;
+    if (chassis_vol > REFEREE_VOLTAGE_VALID_MIN &&
+        chassis_vol < CHASSIS_UNDERVOLTAGE_LIMIT)
+        return;
+
+    if (DrivenMotorIsLost())
+        return;
+
+    if (!switch_is_up(rc_data[TEMP].rc.switch_right))
+        return;
+
+    chassis_cmd_recv.chassis_mode = CHASSIS_STOOL_MODE;
+    chassis_cmd_recv.vx = RCChannelToRatio(rc_data[TEMP].rc.rocker_r1) * STOOL_SPEED_REF;
+    chassis_cmd_recv.offset_angle = -RCChannelToRatio(rc_data[TEMP].rc.rocker_r_) * STOOL_YAW_RATE_REF;
 }
 
-/* 腿缩回复位,只允许驱动轮电机移动 */
+/* Joint motors must remain disabled on this demo chassis. */
 static void ResetChassis()
 {
-    EnableAllMotor(); // 打开全部电机,关节复位到起始角度,驱动电机响应速度输入以从墙角或固连中脱身
-
-    // 目标速度置0
-    chassis.target_v = 0;
-    // 复位时清空距离和腿长积累量,保证顺利站起
-    chassis.dist = chassis.target_dist = 0;
-    l_side.target_len = r_side.target_len = 0.12;
-    // 角度输入为当前角度
-    // chassis_cmd_recv.offset_angle = chassis.target_yaw = chassis.yaw;
-
-    // 撞墙时前后移动保证能重新站立,执行速度输入
-    LKMotorSetRef(l_driven, chassis_cmd_recv.vx + (float)chassis_cmd_recv.rotate_w);
-    LKMotorSetRef(r_driven, -chassis_cmd_recv.vx + (float)chassis_cmd_recv.rotate_w);
-
-    // 若关节完成复位,进入ready态
-    if (abs(lf->measure.total_round) < 0.05 &&
-        abs(lb->measure.total_round) < 0.05 &&
-        abs(rf->measure.total_round) < 0.05 &&
-        abs(rb->measure.total_round) < 0.05)
-    {
-        chassis_status = ROBOT_READY; // 底盘已经准备好重新站立
-    }
-    else if (abs(lf->measure.total_round) <= 0.03 &&
-             abs(lb->measure.total_round) <= 0.03 &&
-             abs(rf->measure.total_round) <= 0.03 &&
-             abs(rb->measure.total_round) <= 0.03)
-    {                                 // 双阈值保证关节能够复位而不会进入死区
-        chassis_status = ROBOT_READY; // 底盘已经准备好重新站立
-
-        for (uint8_t i = 0; i < JOINT_CNT; i++)
-            DMMotorOuterLoop(joint[i], OPEN_LOOP); // 改回直接开环扭矩输入,让电调对扭矩闭环
-
-        return; // 退出函数不再执行关节指令
-    }
-    else
-        chassis_status = ROBOT_STOP;
-
-    // 还在复位中,关节改为位置环,执行复位
-    for (uint8_t i = 0; i < JOINT_CNT; i++)
-    {
-        DMMotorOuterLoop(joint[i], ANGLE_LOOP);
-        DMMotorSetRef(joint[i], 0);
-    }
+    ClearMotionTargets();
+    ClearControlOutput();
+    l_side.target_len = r_side.target_len = 0.12f;
+    chassis.dist = chassis.target_dist = 0.0f;
+    chassis_status = ROBOT_STOP;
+    RefreshJointMotorDisable();
+    StopDrivenMotor();
 }
 
 // 工作状态设定
 static void WokingStateSet()
 {
-    if (chassis_cmd_recv.chassis_mode == CHASSIS_RESET) // 复位模式
+    RefreshJointMotorDisable();
+
+    if (last_chassis_mode != chassis_cmd_recv.chassis_mode)
+    {
+        if (chassis_cmd_recv.chassis_mode == CHASSIS_STOOL_MODE)
+            ResetStoolRuntime();
+        else
+            ClearControlOutput();
+        last_chassis_mode = chassis_cmd_recv.chassis_mode;
+    }
+
+    if (chassis_cmd_recv.chassis_mode == CHASSIS_RESET ||
+        chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE)
     {
         ResetChassis();
         return;
     }
-    else if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE) // 未收到遥控器和云台指令底盘进入急停
-    {
-        // 目标速度置0
-        chassis.target_v = 0;
-        // 清空腿长和距离
-        l_side.target_len = r_side.target_len = 0.12;
-        chassis.dist = chassis.target_dist = 0;
-        // 角度输入为当前角度
-        // chassis_cmd_recv.offset_angle = chassis.target_yaw = chassis.yaw;
 
-        for (uint8_t i = 0; i < JOINT_CNT; i++)
-            DMMotorStop(joint[i]);
-        for (uint8_t i = 0; i < DRIVEN_CNT; i++)
-            LKMotorStop(driven[i]);
-        return; // 关闭所有电机,发送的指令为零
+    if (chassis_cmd_recv.chassis_mode != CHASSIS_STOOL_MODE)
+    {
+        ResetChassis();
+        return;
     }
 
-    // 运动模式
-    EnableAllMotor();
-    // 保证关节电机为开环扭矩控制
-    for (uint8_t i = 0; i < JOINT_CNT; i++)
-        DMMotorOuterLoop(joint[i], OPEN_LOOP);
+    EnableDrivenMotor();
+    chassis_status = ROBOT_READY;
+    l_side.target_len = r_side.target_len = 0.12f;
+    stool_target_wz = chassis_cmd_recv.offset_angle;
 
-    // 设置目标速度/腿长/距离
-    l_side.target_len += 0.00001f*(float)chassis_cmd_recv.delta_leglen;
-    r_side.target_len += 0.00001f*(float)chassis_cmd_recv.delta_leglen;
-    // 腿长限幅
-    VAL_LIMIT(l_side.target_len, 0.12, 0.30);
-    VAL_LIMIT(r_side.target_len, 0.12, 0.30);
-
-    // 加速度限幅,防止键盘控制摔倒
-    chassis.target_v += sign(chassis_cmd_recv.vx - chassis.target_v) * MAX_ACC_REF * del_t;
-    // VAL_LIMIT(chassis.target_v, 0.002, 2.0);
-    // 角度输入
-    if (chassis_cmd_recv.chassis_mode == CHASSIS_FREE_DEBUG)
+    if (fabsf(chassis_cmd_recv.vx) > 0.001f)
     {
-        chassis.target_yaw = chassis_cmd_recv.offset_angle;
+        ApproachFloat(&chassis.target_v, chassis_cmd_recv.vx, STOOL_ACC_REF * del_t);
+        stool_stop_dist = stool_dist;
+        stool_drive_key_pressed = 1;
     }
-    chassis.target_yaw = chassis.yaw + chassis_cmd_recv.offset_angle * DEGREE_2_RAD;
-
-    // TODO 转向速度限幅
-
-    // TODO 最大dist误差限幅
-
-    // TODO 最大速度误差限幅
+    else
+    {
+        if (stool_drive_key_pressed)
+            stool_stop_dist = stool_dist;
+        ApproachFloat(&chassis.target_v, 0.0f, STOOL_BRAKE_ACC_REF * del_t);
+        stool_drive_key_pressed = 0;
+    }
+    VAL_LIMIT(chassis.target_v, -STOOL_SPEED_REF, STOOL_SPEED_REF);
+    chassis.target_dist = chassis.dist;
 }
 
 /**
@@ -410,8 +453,40 @@ static void ParamAssemble()
     r_side.w_ecd = -r_driven->measure.speed_rads;
 }
 
+static void StoolModeBalanceControl(void)
+{
+    float raw_vel = WHEEL_RADIUS * (l_side.w_ecd + r_side.w_ecd) * 0.5f;
+    float vel_lpf_alpha = del_t / (STOOL_VEL_LPF_RC + del_t);
+    VAL_LIMIT(vel_lpf_alpha, 0.0f, 1.0f);
+
+    stool_vel += vel_lpf_alpha * (raw_vel - stool_vel);
+    stool_dist += stool_vel * del_t;
+
+    if (fabsf(chassis.target_v) > 0.001f)
+        stool_stop_dist = stool_dist;
+
+    float stop_dist_error = stool_dist - stool_stop_dist;
+    float wheel_torque = -STOOL_PITCH_K * chassis.pitch -
+                         STOOL_PITCH_W_K * chassis.pitch_w +
+                         STOOL_VEL_K * (stool_vel - chassis.target_v) +
+                         STOOL_STOP_DIST_K * stop_dist_error;
+
+    VAL_LIMIT(wheel_torque, -STOOL_WHEEL_TORQUE_LIMIT, STOOL_WHEEL_TORQUE_LIMIT);
+
+    l_side.T_wheel = wheel_torque;
+    r_side.T_wheel = wheel_torque;
+}
+
 static void SynthesizeMotion() /* 腿部控制:抗劈叉; 轮子控制:转向 */
 {
+    if (chassis_cmd_recv.chassis_mode == CHASSIS_STOOL_MODE)
+    {
+        PIDCalculate(&steer_v_pid, chassis.wz, stool_target_wz);
+        l_side.T_wheel -= steer_v_pid.Output;
+        r_side.T_wheel += steer_v_pid.Output;
+        return;
+    }
+
     if (chassis_cmd_recv.chassis_mode == CHASSIS_FREE_DEBUG ||
         chassis_cmd_recv.chassis_mode == CHASSIS_FOLLOW_GIMBAL_YAW) // 底盘跟随
     {
@@ -452,10 +527,7 @@ static void LegControl() /* 腿长控制和Roll补偿 */
 
 static void WattLimitSet() /* 设定运动模态的输出 */
 {
-    DMMotorSetRef(lf, 1.0f * -l_side.T_front); // 根据扭矩常数计算得到的系数 todo 需修改
-    DMMotorSetRef(lb, 1.0f * -l_side.T_back);
-    DMMotorSetRef(rf, 1.0f * r_side.T_front);
-    DMMotorSetRef(rb, 1.0f * r_side.T_back);
+    RefreshJointMotorDisable();
 
     LKMotorSetRef(l_driven, 195.3125 * l_side.T_wheel);
     LKMotorSetRef(r_driven, 195.3125 * -r_side.T_wheel);
@@ -464,16 +536,6 @@ static void WattLimitSet() /* 设定运动模态的输出 */
 // 裁判系统,双板通信,电容功率控制等
 static void CommNPower()
 {
-    //static uint8_t supercap_send_cnt = 0;
-    // CANCommSend(cmd_can_comm, (void *)&chassis_feedback_data);
-    // supercap_send_cnt++;
-    // if (supercap_send_cnt % 5 == 0)
-    // {
-    //     DataSend2Cap[0] = referee_data->PowerHeatData.buffer_energy; // 200hz发送
-    //     DataSend2Cap[1] = referee_data->GameRobotState.chassis_power_limit;
-    //     SuperCapSend(cap, (uint8_t *)&DataSend2Cap);
-    //     supercap_send_cnt = 0;
-    // }
     /* 更新ui数据 */
     ui_data.direction = chassis_cmd_recv.direction;
     ui_data.friction_mode = chassis_cmd_recv.friction_mode;
@@ -497,6 +559,21 @@ void BalanceTask()
     CommNPower();
     // 参数组装
     ParamAssemble();
+
+    if (chassis_status == ROBOT_STOP ||
+        chassis_cmd_recv.chassis_mode == CHASSIS_RESET ||
+        chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE)
+        return;
+
+    if (chassis_cmd_recv.chassis_mode == CHASSIS_STOOL_MODE)
+    {
+        StoolModeBalanceControl();
+        SynthesizeMotion();
+        ClearJointOutput();
+        WattLimitSet();
+        return;
+    }
+
     // 将五连杆映射成单杆
     Link2Leg(&l_side, &chassis);
     Link2Leg(&r_side, &chassis);
@@ -516,12 +593,6 @@ void BalanceTask()
     NormalForceSolve(&l_side, Chassis_IMU_data);
     NormalForceSolve(&r_side, Chassis_IMU_data);
 
-    // stop表示复位尚未完成,reset表明还未切换到其他模式,故都不执行运动模态的代码
-    if (chassis_status == ROBOT_STOP ||
-        chassis_cmd_recv.chassis_mode == CHASSIS_RESET ||
-        chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE)
-        return; // 复位模态或急停,直接退出
-
     // 运动模态,电机输出映射和限幅
     WattLimitSet();
-}
\ No newline at end of file
+}
diff --git a/application/chassis/lqr_calc.h b/application/chassis/lqr_calc.h
index ce05c24..93efa28 100644
--- a/application/chassis/lqr_calc.h
+++ b/application/chassis/lqr_calc.h
@@ -9,18 +9,20 @@
  */
 static void CalcLQR(LinkNPodParam *p, ChassisParam *chassis)
 {
-    static float k[12][3] = {91.443258,-113.149301,-8.780431,
-    2.186397,-6.908826,-0.171192,
-    47.943203,-39.423913,-13.297140,
-    30.671413,-28.308799,-9.363969,
-    231.778706,-224.742110,68.974495,
-    18.910995,-20.079072,7.490403,
-    50.114072,-58.670210,23.856863,
-    3.273746,-3.321150,1.267603,
-    140.969395,-137.481822,42.507159,
-    93.859327,-90.754295,27.811839,
-    -283.581279,232.017305,87.865897,
-    -28.193619,23.802666,3.963794,};
+    static float k[12][3] = {
+        {91.443258f, -113.149301f, -8.780431f},
+        {2.186397f, -6.908826f, -0.171192f},
+        {47.943203f, -39.423913f, -13.297140f},
+        {30.671413f, -28.308799f, -9.363969f},
+        {231.778706f, -224.742110f, 68.974495f},
+        {18.910995f, -20.079072f, 7.490403f},
+        {50.114072f, -58.670210f, 23.856863f},
+        {3.273746f, -3.321150f, 1.267603f},
+        {140.969395f, -137.481822f, 42.507159f},
+        {93.859327f, -90.754295f, 27.811839f},
+        {-283.581279f, 232.017305f, 87.865897f},
+        {-28.193619f, 23.802666f, 3.963794f},
+    };
     float T[2] = {0};       // 0 T_wheel  1 T_hip
     float l = p->leg_len;
     float lsqr = l * l;
@@ -48,4 +50,4 @@ static void CalcLQR(LinkNPodParam *p, ChassisParam *chassis)
     
     p->T_wheel = T[0];
     p->T_hip = T[1];
-}
\ No newline at end of file
+}
diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c
index 4b7e3a2..16d1351 100644
--- a/application/cmd/robot_cmd.c
+++ b/application/cmd/robot_cmd.c
@@ -16,6 +16,8 @@
 #include "bsp_log.h"
 #include "string.h"
 
+#if defined(ONE_BOARD) || defined(GIMBAL_BOARD)
+
 static Publisher_t *chassis_cmd_pub;             // 底盘控制消息发布者
 static Subscriber_t *chassis_feed_sub;           // 底盘反馈信息订阅者
 static Chassis_Ctrl_Cmd_s chassis_cmd_send;      // 发送给底盘应用的信息,包括控制信息和UI绘制相关
@@ -415,3 +417,5 @@ void RobotCMDTask()
     PubPushMessage(shoot_cmd_pub, (void *)&shoot_cmd_send);
     PubPushMessage(gimbal_cmd_pub, (void *)&gimbal_cmd_send);
 }
+
+#endif
diff --git a/application/robot_def.h b/application/robot_def.h
index 171c62a..0fbcc81 100644
--- a/application/robot_def.h
+++ b/application/robot_def.h
@@ -17,8 +17,8 @@
 #include "stdint.h"
 
 /* 开发板类型定义,烧录时注意不要弄错对应功能;修改定义后需要重新编译,只能存在一个定义! */
-#define ONE_BOARD // 单板控制整车
-// #define CHASSIS_BOARD //底盘板
+// #define ONE_BOARD // 单板控制整车
+#define CHASSIS_BOARD //底盘板
 // #define GIMBAL_BOARD  //云台板
 
 #define VISION_USE_VCP  // 使用虚拟串口发送视觉数据
@@ -92,6 +92,7 @@ typedef enum
     CHASSIS_RESET,             // 底盘重置,双腿缩回
     CHASSIS_FREE_DEBUG,        // 底盘单独调试模式
     CHASSIS_ROTATE_REVERSE,
+    CHASSIS_STOOL_MODE,        // 小板凳模式
 } chassis_mode_e;
 
 
@@ -254,4 +255,4 @@ typedef struct
 
 #pragma pack() // 开启字节对齐,结束前面的#pragma pack(1)
 
-#endif // !ROBOT_DEF_H
\ No newline at end of file
+#endif // !ROBOT_DEF_H
diff --git a/modules/algorithm/QuaternionEKF.c b/modules/algorithm/QuaternionEKF.c
index 27c06e5..54f593e 100644
--- a/modules/algorithm/QuaternionEKF.c
+++ b/modules/algorithm/QuaternionEKF.c
@@ -199,8 +199,8 @@ void IMU_QuaternionEKF_Update(float gx, float gy, float gz, float ax, float ay,
 
     // 利用四元数反解欧拉角
     QEKF_INS.Yaw = atan2f(2.0f * (QEKF_INS.q[0] * QEKF_INS.q[3] + QEKF_INS.q[1] * QEKF_INS.q[2]), 2.0f * (QEKF_INS.q[0] * QEKF_INS.q[0] + QEKF_INS.q[1] * QEKF_INS.q[1]) - 1.0f) * 57.295779513f;
-    QEKF_INS.Pitch = atan2f(2.0f * (QEKF_INS.q[0] * QEKF_INS.q[1] + QEKF_INS.q[2] * QEKF_INS.q[3]), 2.0f * (QEKF_INS.q[0] * QEKF_INS.q[0] + QEKF_INS.q[3] * QEKF_INS.q[3]) - 1.0f) * 57.295779513f;
-    QEKF_INS.Roll = asinf(-2.0f * (QEKF_INS.q[1] * QEKF_INS.q[3] - QEKF_INS.q[0] * QEKF_INS.q[2])) * 57.295779513f;
+    QEKF_INS.Pitch = asinf(-2.0f * (QEKF_INS.q[1] * QEKF_INS.q[3] - QEKF_INS.q[0] * QEKF_INS.q[2])) * 57.295779513f;
+    QEKF_INS.Roll = atan2f(2.0f * (QEKF_INS.q[0] * QEKF_INS.q[1] + QEKF_INS.q[2] * QEKF_INS.q[3]), 2.0f * (QEKF_INS.q[0] * QEKF_INS.q[0] + QEKF_INS.q[3] * QEKF_INS.q[3]) - 1.0f) * 57.295779513f;
 
     // get Yaw total, yaw数据可能会超过360,处理一下方便其他功能使用(如小陀螺)
     if (QEKF_INS.Yaw - QEKF_INS.YawAngleLast > 180.0f)
diff --git a/modules/motor/DMmotor/dmmotor.c b/modules/motor/DMmotor/dmmotor.c
index 0873988..11d104f 100644
--- a/modules/motor/DMmotor/dmmotor.c
+++ b/modules/motor/DMmotor/dmmotor.c
@@ -65,8 +65,7 @@ static void DMMotorDecode(CANInstance *motor_can)
 static void DMMotorLostCallback(void *motor_ptr)
 {
     DMMotorInstance *motor = (DMMotorInstance *)motor_ptr;
-    DMMotorEnable(motor);
-    DMMotorSetMode(DM_CMD_MOTOR_MODE, motor);
+    DMMotorStop(motor);
 }
 
 void DMMotorCaliEncoder(DMMotorInstance *motor)
@@ -131,17 +130,16 @@ void DMMotorTask(void const *argument)
     Motor_Control_Setting_s *setting;
     DMMotor_Send_s motor_send_mailbox;
 	portTickType currentTime;
-	const portTickType xFrequency = pdMS_TO_TICKS(4);
     while (1)
     {
 		currentTime = xTaskGetTickCount();
 		set = motor->pid_ref;
+		measure = &motor->measure;
+		setting = &motor->motor_settings;
 
         if (setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE)
             set *= -1;
 
-		measure = &motor->measure;
-		setting = &motor->motor_settings;
 		if ((setting->close_loop_type & ANGLE_LOOP) && (setting->outer_loop_type & ANGLE_LOOP))
 		{
 			if (setting->angle_feedback_source == OTHER_FEED)
diff --git a/modules/referee/referee_task.c b/modules/referee/referee_task.c
index 5ee621a..8d786e4 100644
--- a/modules/referee/referee_task.c
+++ b/modules/referee/referee_task.c
@@ -38,7 +38,7 @@ static void DeterminRobotID()
 
 static void MyUIRefresh(referee_info_t *referee_recv_info, Referee_Interactive_info_t *_Interactive_data);
 static void UIChangeCheck(Referee_Interactive_info_t *_Interactive_data); // 模式切换检测
-static void RobotModeTest(Referee_Interactive_info_t *_Interactive_data); // 测试用函数，实现模式自动变化
+static void RobotModeTest(Referee_Interactive_info_t *_Interactive_data) __attribute__((unused)); // 测试用函数，实现模式自动变化
 
 referee_info_t *UITaskInit(UART_HandleTypeDef *referee_usart_handle, Referee_Interactive_info_t *UI_data)
 {
@@ -225,6 +225,15 @@ static void MyUIRefresh(referee_info_t *referee_recv_info, Referee_Interactive_i
         case CHASSIS_FOLLOW_GIMBAL_YAW:
             UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "follow   ");
             break;
+        case CHASSIS_STOOL_MODE:
+            UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "stool    ");
+            break;
+        case CHASSIS_FREE_DEBUG:
+            UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "debug    ");
+            break;
+        case CHASSIS_ROTATE_REVERSE:
+            UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "revrot   ");
+            break;
         }
         UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[0]);
         _Interactive_data->Referee_Interactive_Flag.chassis_flag = 0;
@@ -263,6 +272,9 @@ static void MyUIRefresh(referee_info_t *referee_recv_info, Referee_Interactive_i
     {
         switch (_Interactive_data->loader_mode)
         {
+        case LOAD_REVERSE:
+            UICharDraw(&UI_State_dyn[4], "sd4", UI_Graph_Change, 8, UI_Color_Purplish_red, 15, 2, 270, 600, "rev   ");
+            break;
         case LOAD_1_BULLET:
             UICharDraw(&UI_State_dyn[4], "sd4", UI_Graph_Change, 8, UI_Color_Cyan, 15, 2, 270, 600, "1 bull");
             break;

From b3da53745d1692a3998d604b40d849adcab4dd51 Mon Sep 17 00:00:00 2001
From: TuxMonkey <8196772+tuxmonkey@user.noreply.gitee.com>
Date: Wed, 15 Jul 2026 13:40:54 +0800
Subject: [PATCH 04/10] =?UTF-8?q?=E8=83=BD=E5=8A=A8=E4=BA=86=EF=BC=81?=
 =?UTF-8?q?=EF=BC=88=E5=B0=8F=E6=9D=BF=E5=87=B3=EF=BC=89=E4=BD=86=E6=98=AF?=
 =?UTF-8?q?=E8=BF=98=E6=98=AF=E5=BE=88=E6=8A=96?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 application/chassis/balance.c     | 11 ++--
 application/cmd/robot_cmd.c       |  6 +--
 bsp/can/bsp_can.c                 | 35 +++++++++----
 modules/algorithm/QuaternionEKF.c |  2 +-
 modules/motor/LKmotor/LK9025.c    | 85 +++++++++++++++----------------
 5 files changed, 74 insertions(+), 65 deletions(-)

diff --git a/application/chassis/balance.c b/application/chassis/balance.c
index a3f0074..3f2933e 100644
--- a/application/chassis/balance.c
+++ b/application/chassis/balance.c
@@ -28,10 +28,10 @@
 #define STOOL_YAW_RATE_REF 3.5f
 #define STOOL_ACC_REF 4.0f
 #define STOOL_BRAKE_ACC_REF 8.0f
-#define STOOL_PITCH_K 55.0f
-#define STOOL_PITCH_W_K 7.0f
-#define STOOL_VEL_K 10.0f
-#define STOOL_STOP_DIST_K 8.0f
+#define STOOL_PITCH_K 36.0f
+#define STOOL_PITCH_W_K 10.0f
+#define STOOL_VEL_K 6.0f
+#define STOOL_STOP_DIST_K 3.0f
 #define STOOL_VEL_LPF_RC 0.03f
 #define STOOL_WHEEL_TORQUE_LIMIT 25.0f
 #define STOOL_RC_DEADBAND 40
@@ -353,9 +353,6 @@ static void ControlSwitch()
     if (DrivenMotorIsLost())
         return;
 
-    if (!switch_is_up(rc_data[TEMP].rc.switch_right))
-        return;
-
     chassis_cmd_recv.chassis_mode = CHASSIS_STOOL_MODE;
     chassis_cmd_recv.vx = RCChannelToRatio(rc_data[TEMP].rc.rocker_r1) * STOOL_SPEED_REF;
     chassis_cmd_recv.offset_angle = -RCChannelToRatio(rc_data[TEMP].rc.rocker_r_) * STOOL_YAW_RATE_REF;
diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c
index 16d1351..46c5731 100644
--- a/application/cmd/robot_cmd.c
+++ b/application/cmd/robot_cmd.c
@@ -330,8 +330,8 @@ static void EmergencyHandler()
 
     if (switch_is_down(rc_data[TEMP].rc.switch_left) || switch_is_mid(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[下]或[中]
     {
-        // 急停触发条件：遥控器离线 或者 右侧开关打到[下] 或者 robot_state 已经是 STOP
-        if (!rc_online || switch_is_down(rc_data[TEMP].rc.switch_right) || robot_state == ROBOT_STOP)
+        // 急停触发条件：遥控器离线或者右侧开关打到[下]
+        if (!rc_online || switch_is_down(rc_data[TEMP].rc.switch_right))
         {
             robot_state = ROBOT_STOP;
             gimbal_cmd_send.gimbal_mode = GIMBAL_ZERO_FORCE;
@@ -343,7 +343,7 @@ static void EmergencyHandler()
             gimbal_cmd_send.yaw = gimbal_fetch_data.gimbal_imu_data.YawTotalAngle; // 急停时设定值保持与实际值同步
             gimbal_cmd_send.pitch = gimbal_fetch_data.gimbal_imu_data.Pitch;
         }
-        // 恢复正常运行条件：遥控器在线 且 右侧开关状态为[上]
+        // 恢复正常运行条件：遥控器在线且右侧开关状态为[上]或[中]
         else if (switch_is_up(rc_data[TEMP].rc.switch_right) || switch_is_mid(rc_data[TEMP].rc.switch_right))
         {
             robot_state = ROBOT_READY;
diff --git a/bsp/can/bsp_can.c b/bsp/can/bsp_can.c
index 9e32a13..43591b6 100644
--- a/bsp/can/bsp_can.c
+++ b/bsp/can/bsp_can.c
@@ -11,6 +11,11 @@
 static CANInstance *can_instance[CAN_MX_REGISTER_CNT] = {NULL};
 static uint8_t idx; // 全局CAN实例索引,每次有新的模块注册会自增
 
+static uint8_t CANBusIndex(CAN_HandleTypeDef *hcan)
+{
+    return hcan == &hcan1 ? 0u : 1u;
+}
+
 /* ----------------two static function called by CANRegister()-------------------- */
 
 /**
@@ -28,18 +33,26 @@ static uint8_t idx; // 全局CAN实例索引,每次有新的模块注册会自
  */
 static void CANAddFilter(CANInstance *_instance)
 {
-    CAN_FilterTypeDef can_filter_conf;
-    static uint8_t can1_filter_idx = 0, can2_filter_idx = 14; // 0-13给can1用,14-27给can2用
+    static uint8_t all_pass_filter_configured[DEVICE_CAN_CNT] = {0};
+    uint8_t bus = CANBusIndex(_instance->can_handle);
 
-    can_filter_conf.FilterMode = CAN_FILTERMODE_IDLIST;                                                       // 使用id list模式,即只有将rxid添加到过滤器中才会接收到,其他报文会被过滤
-    can_filter_conf.FilterScale = CAN_FILTERSCALE_16BIT;                                                      // 使用16位id模式,即只有低16位有效
-    can_filter_conf.FilterFIFOAssignment = (_instance->tx_id & 1) ? CAN_RX_FIFO0 : CAN_RX_FIFO1;              // 奇数id的模块会被分配到FIFO0,偶数id的模块会被分配到FIFO1
-    can_filter_conf.SlaveStartFilterBank = 14;                                                                // 从第14个过滤器开始配置从机过滤器(在STM32的BxCAN控制器中CAN2是CAN1的从机)
-    can_filter_conf.FilterIdLow = _instance->rx_id << 5;                                                      // 过滤器寄存器的低16位,因为使用STDID,所以只有低11位有效,高5位要填0
-    can_filter_conf.FilterBank = _instance->can_handle == &hcan1 ? (can1_filter_idx++) : (can2_filter_idx++); // 根据can_handle判断是CAN1还是CAN2,然后自增
-    can_filter_conf.FilterActivation = CAN_FILTER_ENABLE;                                                     // 启用过滤器
+    if (all_pass_filter_configured[bus])
+        return;
 
-    HAL_CAN_ConfigFilter(_instance->can_handle, &can_filter_conf);
+    CAN_FilterTypeDef accept_all_filter = {0};
+    accept_all_filter.FilterActivation = CAN_FILTER_ENABLE;
+    accept_all_filter.FilterMode = CAN_FILTERMODE_IDMASK;
+    accept_all_filter.FilterScale = CAN_FILTERSCALE_32BIT;
+    accept_all_filter.FilterIdHigh = 0x0000;
+    accept_all_filter.FilterIdLow = 0x0000;
+    accept_all_filter.FilterMaskIdHigh = 0x0000;
+    accept_all_filter.FilterMaskIdLow = 0x0000;
+    accept_all_filter.FilterBank = _instance->can_handle == &hcan1 ? 0 : 14;
+    accept_all_filter.FilterFIFOAssignment = CAN_RX_FIFO0;
+    accept_all_filter.SlaveStartFilterBank = 14;
+    HAL_CAN_ConfigFilter(_instance->can_handle, &accept_all_filter);
+    all_pass_filter_configured[bus] = 1;
+    return;
 }
 
 /**
@@ -163,7 +176,7 @@ static void CANFIFOxCallback(CAN_HandleTypeDef *_hcan, uint32_t fifox)
                     memcpy(can_instance[i]->rx_buff, can_rx_buff, rxconf.DLC); // 消息拷贝到对应实例
                     can_instance[i]->can_module_callback(can_instance[i]);     // 触发回调进行数据解析和处理
                 }
-                return;
+                break;
             }
         }
     }
diff --git a/modules/algorithm/QuaternionEKF.c b/modules/algorithm/QuaternionEKF.c
index 54f593e..7fd7d3b 100644
--- a/modules/algorithm/QuaternionEKF.c
+++ b/modules/algorithm/QuaternionEKF.c
@@ -199,7 +199,7 @@ void IMU_QuaternionEKF_Update(float gx, float gy, float gz, float ax, float ay,
 
     // 利用四元数反解欧拉角
     QEKF_INS.Yaw = atan2f(2.0f * (QEKF_INS.q[0] * QEKF_INS.q[3] + QEKF_INS.q[1] * QEKF_INS.q[2]), 2.0f * (QEKF_INS.q[0] * QEKF_INS.q[0] + QEKF_INS.q[1] * QEKF_INS.q[1]) - 1.0f) * 57.295779513f;
-    QEKF_INS.Pitch = asinf(-2.0f * (QEKF_INS.q[1] * QEKF_INS.q[3] - QEKF_INS.q[0] * QEKF_INS.q[2])) * 57.295779513f;
+    QEKF_INS.Pitch = -asinf(-2.0f * (QEKF_INS.q[1] * QEKF_INS.q[3] - QEKF_INS.q[0] * QEKF_INS.q[2])) * 57.295779513f;
     QEKF_INS.Roll = atan2f(2.0f * (QEKF_INS.q[0] * QEKF_INS.q[1] + QEKF_INS.q[2] * QEKF_INS.q[3]), 2.0f * (QEKF_INS.q[0] * QEKF_INS.q[0] + QEKF_INS.q[3] * QEKF_INS.q[3]) - 1.0f) * 57.295779513f;
 
     // get Yaw total, yaw数据可能会超过360,处理一下方便其他功能使用(如小陀螺)
diff --git a/modules/motor/LKmotor/LK9025.c b/modules/motor/LKmotor/LK9025.c
index 6f159db..f2f4d28 100644
--- a/modules/motor/LKmotor/LK9025.c
+++ b/modules/motor/LKmotor/LK9025.c
@@ -1,5 +1,6 @@
 #include "LK9025.h"
 #include "stdlib.h"
+#include "string.h"
 #include "general_def.h"
 #include "daemon.h"
 #include "bsp_dwt.h"
@@ -7,33 +8,30 @@
 
 static uint8_t idx;
 static LKMotorInstance *lkmotor_instance[LK_MOTOR_MX_CNT] = {NULL};
-static CANInstance *sender_instance; // 多电机发送时使用的caninstance(当前保存的是注册的第一个电机的caninstance)
-// 后续考虑兼容单电机和多电机指令.
 
-/**
- * @brief 电机反馈报文解析
- *
- * @param _instance 发生中断的caninstance
- */
+static uint32_t LKMotorSingleCanID(uint32_t id)
+{
+    return id < 0x140 ? 0x140 + id : id;
+}
+
 static void LKMotorDecode(CANInstance *_instance)
 {
-    LKMotorInstance *motor = (LKMotorInstance *)_instance->id; // 通过caninstance保存的father id获取对应的motorinstance
+    LKMotorInstance *motor = (LKMotorInstance *)_instance->id;
     LKMotor_Measure_t *measure = &motor->measure;
     uint8_t *rx_buff = _instance->rx_buff;
 
-    DaemonReload(motor->daemon); // 喂狗
+    DaemonReload(motor->daemon);
     measure->feed_dt = DWT_GetDeltaT(&measure->feed_dwt_cnt);
 
     measure->last_ecd = measure->ecd;
     measure->ecd = (uint16_t)((rx_buff[7] << 8) | rx_buff[6]);
-
     measure->angle_single_round = ECD_ANGLE_COEF_LK * measure->ecd;
 
-    measure->speed_rads = (1 - SPEED_SMOOTH_COEF) * measure->speed_rads +
+    measure->speed_rads = (1.0f - SPEED_SMOOTH_COEF) * measure->speed_rads +
                           DEGREE_2_RAD * SPEED_SMOOTH_COEF * (float)((int16_t)(rx_buff[5] << 8 | rx_buff[4]));
 
-    measure->real_current = (1 - CURRENT_SMOOTH_COEF) * measure->real_current +
-                            CURRENT_SMOOTH_COEF * (float)((int16_t)(rx_buff[3] << 8 | rx_buff[2]));
+    measure->real_current = (int16_t)((1.0f - CURRENT_SMOOTH_COEF) * measure->real_current +
+                                      CURRENT_SMOOTH_COEF * (float)((int16_t)(rx_buff[3] << 8 | rx_buff[2])));
 
     measure->temperature = rx_buff[1];
 
@@ -41,7 +39,7 @@ static void LKMotorDecode(CANInstance *_instance)
         measure->total_round--;
     else if (measure->ecd - measure->last_ecd < -32768)
         measure->total_round++;
-    measure->total_angle = measure->total_round * 360 + measure->angle_single_round;
+    measure->total_angle = measure->total_round * 360.0f + measure->angle_single_round;
 }
 
 static void LKMotorLostCallback(void *motor_ptr)
@@ -53,7 +51,6 @@ static void LKMotorLostCallback(void *motor_ptr)
 LKMotorInstance *LKMotorInit(Motor_Init_Config_s *config)
 {
     LKMotorInstance *motor = (LKMotorInstance *)malloc(sizeof(LKMotorInstance));
-    motor = (LKMotorInstance *)malloc(sizeof(LKMotorInstance));
     memset(motor, 0, sizeof(LKMotorInstance));
 
     motor->motor_settings = config->controller_setting_init_config;
@@ -65,16 +62,10 @@ LKMotorInstance *LKMotorInit(Motor_Init_Config_s *config)
 
     config->can_init_config.id = motor;
     config->can_init_config.can_module_callback = LKMotorDecode;
-    config->can_init_config.rx_id = 0x140 + config->can_init_config.tx_id;
-    config->can_init_config.tx_id = config->can_init_config.tx_id + 0x280 - 1; // 这样在发送写入buffer的时候更方便,因为下标从0开始,LK多电机发送id为0x280
+    config->can_init_config.tx_id = LKMotorSingleCanID(config->can_init_config.tx_id);
+    config->can_init_config.rx_id = config->can_init_config.tx_id;
     motor->motor_can_ins = CANRegister(&config->can_init_config);
 
-    if (idx == 0) // 用第一个电机的can instance发送数据
-    {
-        sender_instance = motor->motor_can_ins;
-        sender_instance->tx_id = 0x280; //  修改tx_id为0x280,用于多电机发送,不用管其他LKMotorInstance的tx_id,它们仅作初始化用
-    }
-
     LKMotorEnable(motor);
     DWT_GetDeltaT(&motor->measure.feed_dwt_cnt);
     lkmotor_instance[idx++] = motor;
@@ -82,14 +73,29 @@ LKMotorInstance *LKMotorInit(Motor_Init_Config_s *config)
     Daemon_Init_Config_s daemon_config = {
         .callback = LKMotorLostCallback,
         .owner_id = motor,
-        .reload_count = 50, // 50ms
+        .reload_count = 50,
     };
     motor->daemon = DaemonRegister(&daemon_config);
 
     return motor;
 }
 
-/* 第一个电机的can instance用于发送数据,向其tx_buff填充数据 */
+static void LKMotorSendTorque(LKMotorInstance *motor, int16_t set)
+{
+    uint16_t raw_set = (uint16_t)set;
+    uint8_t *tx_buff = motor->motor_can_ins->tx_buff;
+
+    tx_buff[0] = 0xA1;
+    tx_buff[1] = 0x00;
+    tx_buff[2] = 0x00;
+    tx_buff[3] = 0x00;
+    tx_buff[4] = (uint8_t)(raw_set & 0xff);
+    tx_buff[5] = (uint8_t)(raw_set >> 8);
+    tx_buff[6] = 0x00;
+    tx_buff[7] = 0x00;
+    CANTransmit(motor->motor_can_ins, 0.2);
+}
+
 void LKMotorControl()
 {
     float pid_measure, pid_ref;
@@ -105,48 +111,41 @@ void LKMotorControl()
         setting = &motor->motor_settings;
         pid_ref = motor->pid_ref;
         if (setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE)
-            pid_ref *= -1;
+            pid_ref *= -1.0f;
 
         if ((setting->close_loop_type & ANGLE_LOOP) && setting->outer_loop_type == ANGLE_LOOP)
         {
             if (setting->angle_feedback_source == OTHER_FEED)
                 pid_measure = *motor->other_angle_feedback_ptr;
             else
-                pid_measure = measure->real_current;
+                pid_measure = measure->total_angle;
             pid_ref = PIDCalculate(&motor->angle_PID, pid_measure, pid_ref);
             if (setting->feedforward_flag & SPEED_FEEDFORWARD)
                 pid_ref += *motor->speed_feedforward_ptr;
         }
 
-        if ((setting->close_loop_type & SPEED_LOOP) && setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP))
+        if ((setting->close_loop_type & SPEED_LOOP) && (setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP)))
         {
-            if (setting->angle_feedback_source == OTHER_FEED)
+            if (setting->speed_feedback_source == OTHER_FEED)
                 pid_measure = *motor->other_speed_feedback_ptr;
             else
                 pid_measure = measure->speed_rads;
-            pid_ref = PIDCalculate(&motor->angle_PID, pid_measure, pid_ref);
+            pid_ref = PIDCalculate(&motor->speed_PID, pid_measure, pid_ref);
             if (setting->feedforward_flag & CURRENT_FEEDFORWARD)
                 pid_ref += *motor->current_feedforward_ptr;
         }
 
         if (setting->close_loop_type & CURRENT_LOOP)
-        {
             pid_ref = PIDCalculate(&motor->current_PID, measure->real_current, pid_ref);
-        }
 
-        set = (int16_t)pid_ref;
+        if (pid_ref > I_MAX)
+            pid_ref = I_MAX;
+        else if (pid_ref < I_MIN)
+            pid_ref = I_MIN;
 
-        // 这里随便写的,为了兼容多电机命令.后续应该将tx_id以更好的方式表达电机id,单独使用一个CANInstance,而不是用第一个电机的CANInstance
-        memcpy(sender_instance->tx_buff + (motor->motor_can_ins->tx_id - 0x280) * 2, &set, sizeof(uint16_t));
-
-        if (motor->stop_flag == MOTOR_STOP)
-        { // 若该电机处于停止状态,直接将发送buff置零
-            memset(sender_instance->tx_buff + (motor->motor_can_ins->tx_id - 0x280) * 2, 0, sizeof(uint16_t));
-        }
+        set = motor->stop_flag == MOTOR_STOP ? 0 : (int16_t)pid_ref;
+        LKMotorSendTorque(motor, set);
     }
-
-    if (idx) // 如果有电机注册了
-        CANTransmit(sender_instance, 0.2);
 }
 
 void LKMotorStop(LKMotorInstance *motor)

From 76f8c67a352241b23b9afe01c278ae3adb22ea55 Mon Sep 17 00:00:00 2001
From: TuxMonkey <8196772+tuxmonkey@user.noreply.gitee.com>
Date: Wed, 15 Jul 2026 14:06:27 +0800
Subject: [PATCH 05/10] =?UTF-8?q?=E8=BD=AE=E7=94=B5=E6=9C=BAID=E6=9E=81?=
 =?UTF-8?q?=E6=80=A7=20IMU=E6=AD=A3=E8=B4=9F=E4=BF=AE=E6=94=B9?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 application/chassis/balance.c     | 12 ++++++------
 modules/algorithm/QuaternionEKF.c |  2 +-
 2 files changed, 7 insertions(+), 7 deletions(-)

diff --git a/application/chassis/balance.c b/application/chassis/balance.c
index 3f2933e..d58294c 100644
--- a/application/chassis/balance.c
+++ b/application/chassis/balance.c
@@ -28,10 +28,10 @@
 #define STOOL_YAW_RATE_REF 3.5f
 #define STOOL_ACC_REF 4.0f
 #define STOOL_BRAKE_ACC_REF 8.0f
-#define STOOL_PITCH_K 36.0f
-#define STOOL_PITCH_W_K 10.0f
-#define STOOL_VEL_K 6.0f
-#define STOOL_STOP_DIST_K 3.0f
+#define STOOL_PITCH_K 30.0f
+#define STOOL_PITCH_W_K 8.5f
+#define STOOL_VEL_K 4.5f
+#define STOOL_STOP_DIST_K 2.0f
 #define STOOL_VEL_LPF_RC 0.03f
 #define STOOL_WHEEL_TORQUE_LIMIT 25.0f
 #define STOOL_RC_DEADBAND 40
@@ -236,9 +236,9 @@ void BalanceInit()
 
     };
     driven_conf.can_init_config.tx_id = 1;
-    driven[RD] = r_driven = LKMotorInit(&driven_conf);
-    driven_conf.can_init_config.tx_id = 2;
     driven[LD] = l_driven = LKMotorInit(&driven_conf);
+    driven_conf.can_init_config.tx_id = 2;
+    driven[RD] = r_driven = LKMotorInit(&driven_conf);
 
     // 腿长控制
     PID_Init_Config_s leg_length_pid_conf = {
diff --git a/modules/algorithm/QuaternionEKF.c b/modules/algorithm/QuaternionEKF.c
index 7fd7d3b..54f593e 100644
--- a/modules/algorithm/QuaternionEKF.c
+++ b/modules/algorithm/QuaternionEKF.c
@@ -199,7 +199,7 @@ void IMU_QuaternionEKF_Update(float gx, float gy, float gz, float ax, float ay,
 
     // 利用四元数反解欧拉角
     QEKF_INS.Yaw = atan2f(2.0f * (QEKF_INS.q[0] * QEKF_INS.q[3] + QEKF_INS.q[1] * QEKF_INS.q[2]), 2.0f * (QEKF_INS.q[0] * QEKF_INS.q[0] + QEKF_INS.q[1] * QEKF_INS.q[1]) - 1.0f) * 57.295779513f;
-    QEKF_INS.Pitch = -asinf(-2.0f * (QEKF_INS.q[1] * QEKF_INS.q[3] - QEKF_INS.q[0] * QEKF_INS.q[2])) * 57.295779513f;
+    QEKF_INS.Pitch = asinf(-2.0f * (QEKF_INS.q[1] * QEKF_INS.q[3] - QEKF_INS.q[0] * QEKF_INS.q[2])) * 57.295779513f;
     QEKF_INS.Roll = atan2f(2.0f * (QEKF_INS.q[0] * QEKF_INS.q[1] + QEKF_INS.q[2] * QEKF_INS.q[3]), 2.0f * (QEKF_INS.q[0] * QEKF_INS.q[0] + QEKF_INS.q[3] * QEKF_INS.q[3]) - 1.0f) * 57.295779513f;
 
     // get Yaw total, yaw数据可能会超过360,处理一下方便其他功能使用(如小陀螺)

From 709e66dcd4057604b52da4ba4452f709022c9cde Mon Sep 17 00:00:00 2001
From: TuxMonkey <8196772+tuxmonkey@user.noreply.gitee.com>
Date: Wed, 15 Jul 2026 14:14:15 +0800
Subject: [PATCH 06/10] =?UTF-8?q?=E8=BD=AE=E7=94=B5=E6=9C=BAID=E6=9E=81?=
 =?UTF-8?q?=E6=80=A7=20IMU=E6=AD=A3=E8=B4=9F=E4=BF=AE=E6=94=B9?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 application/chassis/balance.c | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/application/chassis/balance.c b/application/chassis/balance.c
index d58294c..cbba77b 100644
--- a/application/chassis/balance.c
+++ b/application/chassis/balance.c
@@ -28,10 +28,10 @@
 #define STOOL_YAW_RATE_REF 3.5f
 #define STOOL_ACC_REF 4.0f
 #define STOOL_BRAKE_ACC_REF 8.0f
-#define STOOL_PITCH_K 30.0f
-#define STOOL_PITCH_W_K 8.5f
-#define STOOL_VEL_K 4.5f
-#define STOOL_STOP_DIST_K 2.0f
+#define STOOL_PITCH_K 24.0f
+#define STOOL_PITCH_W_K 6.8f
+#define STOOL_VEL_K 3.2f
+#define STOOL_STOP_DIST_K 1.2f
 #define STOOL_VEL_LPF_RC 0.03f
 #define STOOL_WHEEL_TORQUE_LIMIT 25.0f
 #define STOOL_RC_DEADBAND 40

From 12be1e96de79ac8f374a2f070ab0daeab5c86e54 Mon Sep 17 00:00:00 2001
From: TuxMonkey <8196772+tuxmonkey@user.noreply.gitee.com>
Date: Wed, 15 Jul 2026 14:29:29 +0800
Subject: [PATCH 07/10] =?UTF-8?q?=E6=88=91=E8=8D=89=E5=8F=AF=E4=BB=A5?=
 =?UTF-8?q?=E4=BA=86=EF=BC=8C=E4=BD=86=E6=98=AF=E6=9C=89=E7=82=B9=E4=B8=8D?=
 =?UTF-8?q?=E5=B9=B3?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 application/chassis/balance.c | 8 +++++---
 1 file changed, 5 insertions(+), 3 deletions(-)

diff --git a/application/chassis/balance.c b/application/chassis/balance.c
index cbba77b..097df29 100644
--- a/application/chassis/balance.c
+++ b/application/chassis/balance.c
@@ -31,7 +31,8 @@
 #define STOOL_PITCH_K 24.0f
 #define STOOL_PITCH_W_K 6.8f
 #define STOOL_VEL_K 3.2f
-#define STOOL_STOP_DIST_K 1.2f
+#define STOOL_STOP_DIST_K 0.0f
+#define STOOL_PITCH_REF -0.05f
 #define STOOL_VEL_LPF_RC 0.03f
 #define STOOL_WHEEL_TORQUE_LIMIT 25.0f
 #define STOOL_RC_DEADBAND 40
@@ -430,7 +431,7 @@ static void ParamAssemble()
 {
     // 机体参数,视为平面刚体
     chassis.pitch = Chassis_IMU_data->Pitch * DEGREE_2_RAD;
-    chassis.pitch_w = Chassis_IMU_data->Gyro[0];
+    chassis.pitch_w = Chassis_IMU_data->Gyro[1];
     chassis.yaw = Chassis_IMU_data->YawTotalAngle * DEGREE_2_RAD;
     chassis.wz = Chassis_IMU_data->Gyro[2];
     chassis.roll = Chassis_IMU_data->Roll * DEGREE_2_RAD;
@@ -463,7 +464,8 @@ static void StoolModeBalanceControl(void)
         stool_stop_dist = stool_dist;
 
     float stop_dist_error = stool_dist - stool_stop_dist;
-    float wheel_torque = -STOOL_PITCH_K * chassis.pitch -
+    float pitch_error = chassis.pitch - STOOL_PITCH_REF;
+    float wheel_torque = -STOOL_PITCH_K * pitch_error -
                          STOOL_PITCH_W_K * chassis.pitch_w +
                          STOOL_VEL_K * (stool_vel - chassis.target_v) +
                          STOOL_STOP_DIST_K * stop_dist_error;

From eb858afc171b58c53abb0bd16f8082382959c1fc Mon Sep 17 00:00:00 2001
From: TuxMonkey <8196772+tuxmonkey@user.noreply.gitee.com>
Date: Wed, 15 Jul 2026 14:43:13 +0800
Subject: [PATCH 08/10] =?UTF-8?q?=E5=8A=A0=E4=BA=86=E4=B8=AAREF=20Pitch?=
 =?UTF-8?q?=E8=A1=A5=E5=81=BF=E4=B8=80=E4=B8=8B=20=E7=9C=8B=E7=9D=80?=
 =?UTF-8?q?=E8=BF=98=E4=B8=8D=E9=94=99?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 application/chassis/balance.c | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/application/chassis/balance.c b/application/chassis/balance.c
index 097df29..7a02874 100644
--- a/application/chassis/balance.c
+++ b/application/chassis/balance.c
@@ -28,11 +28,11 @@
 #define STOOL_YAW_RATE_REF 3.5f
 #define STOOL_ACC_REF 4.0f
 #define STOOL_BRAKE_ACC_REF 8.0f
-#define STOOL_PITCH_K 24.0f
-#define STOOL_PITCH_W_K 6.8f
-#define STOOL_VEL_K 3.2f
-#define STOOL_STOP_DIST_K 0.0f
-#define STOOL_PITCH_REF -0.05f
+#define STOOL_PITCH_K 50.0f
+#define STOOL_PITCH_W_K 12.5f
+#define STOOL_VEL_K 6.2f
+#define STOOL_STOP_DIST_K 3.0f
+#define STOOL_PITCH_REF -0.10f
 #define STOOL_VEL_LPF_RC 0.03f
 #define STOOL_WHEEL_TORQUE_LIMIT 25.0f
 #define STOOL_RC_DEADBAND 40

From 5693c30faf3d96797f564a6632e1f4b07dc8b87d Mon Sep 17 00:00:00 2001
From: TuxMonkey <8196772+tuxmonkey@user.noreply.gitee.com>
Date: Wed, 15 Jul 2026 16:54:22 +0800
Subject: [PATCH 09/10] =?UTF-8?q?=E5=B0=8F=E6=9D=BF=E5=87=B3=20=E5=AD=98?=
 =?UTF-8?q?=E6=A1=A3=E3=80=82?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 application/chassis/balance.c | 11 ++++++-----
 1 file changed, 6 insertions(+), 5 deletions(-)

diff --git a/application/chassis/balance.c b/application/chassis/balance.c
index 7a02874..3ef761f 100644
--- a/application/chassis/balance.c
+++ b/application/chassis/balance.c
@@ -24,10 +24,10 @@
 #include "math.h"
 #include "string.h"
 
-#define STOOL_SPEED_REF 1.5f
-#define STOOL_YAW_RATE_REF 3.5f
-#define STOOL_ACC_REF 4.0f
-#define STOOL_BRAKE_ACC_REF 8.0f
+#define STOOL_SPEED_REF 1.8f
+#define STOOL_YAW_RATE_REF 2.4f
+#define STOOL_ACC_REF 6.5f
+#define STOOL_BRAKE_ACC_REF 16.0f
 #define STOOL_PITCH_K 50.0f
 #define STOOL_PITCH_W_K 12.5f
 #define STOOL_VEL_K 6.2f
@@ -343,7 +343,8 @@ static void ControlSwitch()
         return;
     }
 
-    if (switch_is_down(rc_data[TEMP].rc.switch_right))
+    uint8_t switch_right = rc_data[TEMP].rc.switch_right;
+    if (!switch_is_up(switch_right) && !switch_is_mid(switch_right))
         return;
 
     float chassis_vol = referee_data->PowerHeatData.chassis_voltage * 0.001f;

From 14a3ab4cd04c013a813b0b35dc6cf411f1720a50 Mon Sep 17 00:00:00 2001
From: TuxMonkey <8196772+tuxmonkey@user.noreply.gitee.com>
Date: Wed, 15 Jul 2026 19:30:07 +0800
Subject: [PATCH 10/10] =?UTF-8?q?=E5=B0=8F=E6=9D=BF=E5=87=B3=20=E5=AD=98?=
 =?UTF-8?q?=E6=A1=A3?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 application/chassis/balance.c | 36 +++++++++++++++++++++++++----------
 1 file changed, 26 insertions(+), 10 deletions(-)

diff --git a/application/chassis/balance.c b/application/chassis/balance.c
index 3ef761f..611276b 100644
--- a/application/chassis/balance.c
+++ b/application/chassis/balance.c
@@ -24,14 +24,16 @@
 #include "math.h"
 #include "string.h"
 
-#define STOOL_SPEED_REF 1.8f
-#define STOOL_YAW_RATE_REF 2.4f
-#define STOOL_ACC_REF 6.5f
-#define STOOL_BRAKE_ACC_REF 16.0f
-#define STOOL_PITCH_K 50.0f
-#define STOOL_PITCH_W_K 12.5f
-#define STOOL_VEL_K 6.2f
-#define STOOL_STOP_DIST_K 3.0f
+#define STOOL_SPEED_REF 1.5f
+#define STOOL_YAW_RATE_REF 3.5f
+#define STOOL_SPIN_YAW_RATE_REF 10.0f
+#define STOOL_SPIN_YAW_ACC_REF 5.0f
+#define STOOL_ACC_REF 4.0f
+#define STOOL_BRAKE_ACC_REF 8.0f
+#define STOOL_PITCH_K 55.0f
+#define STOOL_PITCH_W_K 7.0f
+#define STOOL_VEL_K 10.0f
+#define STOOL_STOP_DIST_K 8.0f
 #define STOOL_PITCH_REF -0.10f
 #define STOOL_VEL_LPF_RC 0.03f
 #define STOOL_WHEEL_TORQUE_LIMIT 25.0f
@@ -73,6 +75,7 @@ static float stool_vel;
 static float stool_dist;
 static float stool_stop_dist;
 static float stool_target_wz;
+static uint8_t stool_spin_enabled;
 static chassis_mode_e last_chassis_mode = CHASSIS_ZERO_FORCE;
 
 static void ClearMotionTargets(void)
@@ -80,6 +83,7 @@ static void ClearMotionTargets(void)
     chassis.target_v = 0.0f;
     chassis.target_dist = chassis.dist;
     stool_target_wz = 0.0f;
+    stool_spin_enabled = 0;
 }
 
 static void ClearControlOutput(void)
@@ -337,6 +341,7 @@ static void ControlSwitch()
     chassis_cmd_recv.loader_mode = LOAD_STOP;
     chassis_cmd_recv.vision_mode = UNLOCK;
     chassis_cmd_recv.ui_mode = UI_KEEP;
+    stool_spin_enabled = 0;
 
     if (!RemoteControlIsOnline())
     {
@@ -357,7 +362,15 @@ static void ControlSwitch()
 
     chassis_cmd_recv.chassis_mode = CHASSIS_STOOL_MODE;
     chassis_cmd_recv.vx = RCChannelToRatio(rc_data[TEMP].rc.rocker_r1) * STOOL_SPEED_REF;
-    chassis_cmd_recv.offset_angle = -RCChannelToRatio(rc_data[TEMP].rc.rocker_r_) * STOOL_YAW_RATE_REF;
+    if (switch_is_up(switch_right))
+    {
+        chassis_cmd_recv.offset_angle = -STOOL_SPIN_YAW_RATE_REF;
+        stool_spin_enabled = 1;
+    }
+    else
+    {
+        chassis_cmd_recv.offset_angle = -RCChannelToRatio(rc_data[TEMP].rc.rocker_r_) * STOOL_YAW_RATE_REF;
+    }
 }
 
 /* Joint motors must remain disabled on this demo chassis. */
@@ -402,7 +415,10 @@ static void WokingStateSet()
     EnableDrivenMotor();
     chassis_status = ROBOT_READY;
     l_side.target_len = r_side.target_len = 0.12f;
-    stool_target_wz = chassis_cmd_recv.offset_angle;
+    if (stool_spin_enabled || fabsf(stool_target_wz) > STOOL_YAW_RATE_REF)
+        ApproachFloat(&stool_target_wz, chassis_cmd_recv.offset_angle, STOOL_SPIN_YAW_ACC_REF * del_t);
+    else
+        stool_target_wz = chassis_cmd_recv.offset_angle;
 
     if (fabsf(chassis_cmd_recv.vx) > 0.001f)
     {