修can发送

添加cmd
改名字
2025-10-13 01:29:09 +08:00 · 2025-10-10 22:02:47 +08:00 · 2025-10-10 20:16:28 +08:00 · 2025-10-10 17:31:19 +08:00 · 2025-10-10 16:59:25 +08:00 · 2025-10-10 00:21:11 +08:00
29 changed files with 1580 additions and 1471 deletions
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@ -57,7 +57,6 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE

    # User/component sources
    User/component/ahrs.c
-    User/component/cmd.c
    User/component/crc16.c
    User/component/crc8.c
    User/component/filter.c
@ -84,6 +83,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE

    # User/module sources
    User/module/balance_chassis.c
+    User/module/cmd.c
    User/module/config.c
    User/module/gimbal.c
    User/module/shoot.c
@ -92,7 +92,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
    User/task/ai.c
    User/task/atti_esti.c
    User/task/blink.c
-    User/task/command.c
+    User/task/cmd.c
    User/task/ctrl_chassis.c
    User/task/ctrl_gimbal.c
    User/task/ctrl_shoot.c
--- a/Core/Src/can.c
+++ b/Core/Src/can.c
@ -47,7 +47,7 @@ void MX_CAN1_Init(void)
  hcan1.Init.TimeTriggeredMode = DISABLE;
  hcan1.Init.AutoBusOff = DISABLE;
  hcan1.Init.AutoWakeUp = DISABLE;
-  hcan1.Init.AutoRetransmission = DISABLE;
+  hcan1.Init.AutoRetransmission = ENABLE;
  hcan1.Init.ReceiveFifoLocked = DISABLE;
  hcan1.Init.TransmitFifoPriority = ENABLE;
  if (HAL_CAN_Init(&hcan1) != HAL_OK)
@ -79,7 +79,7 @@ void MX_CAN2_Init(void)
  hcan2.Init.TimeTriggeredMode = DISABLE;
  hcan2.Init.AutoBusOff = DISABLE;
  hcan2.Init.AutoWakeUp = DISABLE;
-  hcan2.Init.AutoRetransmission = DISABLE;
+  hcan2.Init.AutoRetransmission = ENABLE;
  hcan2.Init.ReceiveFifoLocked = DISABLE;
  hcan2.Init.TransmitFifoPriority = ENABLE;
  if (HAL_CAN_Init(&hcan2) != HAL_OK)
--- a/Core/Src/stm32f4xx_it.c
+++ b/Core/Src/stm32f4xx_it.c
--- a/DevC.ioc
+++ b/DevC.ioc
@ -21,7 +21,8 @@ CAN1.BS2=CAN_BS2_7TQ
 CAN1.CalculateBaudRate=1000000
 CAN1.CalculateTimeBit=1000
 CAN1.CalculateTimeQuantum=71.42857142857143
-CAN1.IPParameters=CalculateTimeQuantum,BS1,BS2,Prescaler,TXFP,ABOM,CalculateTimeBit,CalculateBaudRate
+CAN1.IPParameters=CalculateTimeQuantum,BS1,BS2,Prescaler,TXFP,ABOM,CalculateTimeBit,CalculateBaudRate,NART
+CAN1.NART=ENABLE
 CAN1.Prescaler=3
 CAN1.TXFP=ENABLE
 CAN2.BS1=CAN_BS1_6TQ
@ -29,7 +30,8 @@ CAN2.BS2=CAN_BS2_7TQ
 CAN2.CalculateBaudRate=1000000
 CAN2.CalculateTimeBit=1000
 CAN2.CalculateTimeQuantum=71.42857142857143
-CAN2.IPParameters=CalculateTimeQuantum,BS1,BS2,Prescaler,TXFP,CalculateTimeBit,CalculateBaudRate
+CAN2.IPParameters=CalculateTimeQuantum,BS1,BS2,Prescaler,TXFP,CalculateTimeBit,CalculateBaudRate,NART
+CAN2.NART=ENABLE
 CAN2.Prescaler=3
 CAN2.TXFP=ENABLE
 Dma.I2C2_TX.2.Direction=DMA_MEMORY_TO_PERIPH
--- a/User/component/cmd.c
+++ b/User/component/cmd.c
@ -1,387 +0,0 @@
-/*
-  控制命令
-*/
-
-#include "cmd.h"
-
-#include <string.h>
-
-/* USER INCLUDE BEGIN */
-
-/* USER INCLUDE END */
-
-/* USER DEFINE BEGIN */
-
-/* USER DEFINE END */
-
-/**
- * @brief 行为转换为对应按键
- *
- * @param cmd 主结构体
- * @param behavior 行为
- * @return uint16_t 行为对应的按键
- */
-static inline CMD_KeyValue_t CMD_BehaviorToKey(CMD_t *cmd,
-                                               CMD_Behavior_t behavior) {
-  return cmd->param->map.key_map[behavior].key;
-}
-
-static inline CMD_ActiveType_t CMD_BehaviorToActive(CMD_t *cmd,
-                                                    CMD_Behavior_t behavior) {
-  return cmd->param->map.key_map[behavior].active;
-}
-
-/**
- * @brief 检查按键是否按下
- *
- * @param rc 遥控器数据
- * @param key 按键名称
- * @param stateful 是否为状态切换按键
- * @return true 按下
- * @return false 未按下
- */
-static bool CMD_KeyPressedRc(const CMD_RC_t *rc, CMD_KeyValue_t key) {
-  /* 按下按键为鼠标左、右键 */
-  if (key == CMD_L_CLICK) {
-    return rc->mouse.l_click;
-  }
-  if (key == CMD_R_CLICK) {
-    return rc->mouse.r_click;
-  }
-  return rc->key & (1u << key);
-}
-
-static bool CMD_BehaviorOccurredRc(const CMD_RC_t *rc, CMD_t *cmd,
-                                   CMD_Behavior_t behavior) {
-  CMD_KeyValue_t key = CMD_BehaviorToKey(cmd, behavior);
-  CMD_ActiveType_t active = CMD_BehaviorToActive(cmd, behavior);
-
-  bool now_key_pressed, last_key_pressed;
-
-  /* 按下按键为鼠标左、右键 */
-  if (key == CMD_L_CLICK) {
-    now_key_pressed = rc->mouse.l_click;
-    last_key_pressed = cmd->mouse_last.l_click;
-  } else if (key == CMD_R_CLICK) {
-    now_key_pressed = rc->mouse.r_click;
-    last_key_pressed = cmd->mouse_last.r_click;
-  } else {
-    now_key_pressed = rc->key & (1u << key);
-    last_key_pressed = cmd->key_last & (1u << key);
-  }
-
-  switch (active) {
-    case CMD_ACTIVE_PRESSING:
-      return now_key_pressed && !last_key_pressed;
-    case CMD_ACTIVE_RASING:
-      return !now_key_pressed && last_key_pressed;
-    case CMD_ACTIVE_PRESSED:
-      return now_key_pressed;
-  }
-}
-
-/**
- * @brief 解析pc行为逻辑
- *
- * @param rc 遥控器数据
- * @param cmd 主结构体
- * @param dt_sec 两次解析的间隔
- */
-static void CMD_PcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
-  cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
-
-  /* 云台设置为鼠标控制欧拉角的变化，底盘的控制向量设置为零 */
-  cmd->gimbal.delta_eulr.yaw =
-      (float)rc->mouse.x * dt_sec * cmd->param->sens_mouse;
-  cmd->gimbal.delta_eulr.pit =
-      (float)(-rc->mouse.y) * dt_sec * cmd->param->sens_mouse;
-  cmd->chassis.ctrl_vec.vx = cmd->chassis.ctrl_vec.vy = 0.0f;
-  cmd->shoot.reverse_trig = false;
-
-  /* 按键行为映射相关逻辑 */
-  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FORE)) {
-    cmd->chassis.ctrl_vec.vy += cmd->param->move.move_sense;
-  }
-  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_BACK)) {
-    cmd->chassis.ctrl_vec.vy -= cmd->param->move.move_sense;
-  }
-  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_LEFT)) {
-    cmd->chassis.ctrl_vec.vx -= cmd->param->move.move_sense;
-  }
-  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_RIGHT)) {
-    cmd->chassis.ctrl_vec.vx += cmd->param->move.move_sense;
-  }
-  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_ACCELERATE)) {
-    cmd->chassis.ctrl_vec.vx *= cmd->param->move.move_fast_sense;
-    cmd->chassis.ctrl_vec.vy *= cmd->param->move.move_fast_sense;
-  }
-  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_DECELEBRATE)) {
-    cmd->chassis.ctrl_vec.vx *= cmd->param->move.move_slow_sense;
-    cmd->chassis.ctrl_vec.vy *= cmd->param->move.move_slow_sense;
-  }
-  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FIRE)) {
-    /* 切换至开火模式，设置相应的射击频率和弹丸初速度 */
-    cmd->shoot.mode = SHOOT_MODE_LOADED;
-    cmd->shoot.fire = true;
-  } else {
-    /* 切换至准备模式，停止射击 */
-    cmd->shoot.mode = SHOOT_MODE_LOADED;
-    cmd->shoot.fire = false;
-  }
-  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FIRE_MODE)) {
-    /* 每按一次依次切换开火下一个模式 */
-    cmd->shoot.fire_mode++;
-    cmd->shoot.fire_mode %= FIRE_MODE_NUM;
-  }
-  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_ROTOR)) {
-    /* 切换到小陀螺模式 */
-    cmd->chassis.mode = CHASSIS_MODE_ROTOR;
-    cmd->chassis.mode_rotor = ROTOR_MODE_RAND;
-  }
-  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_OPENCOVER)) {
-    /* 每按一次开、关弹舱盖 */
-    cmd->shoot.cover_open = !cmd->shoot.cover_open;
-  }
-  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_BUFF)) {
-    if (cmd->ai_status == AI_STATUS_HITSWITCH) {
-      /* 停止ai的打符模式，停用host控制 */
-      CMD_RefereeAdd(&(cmd->referee), CMD_UI_HIT_SWITCH_STOP);
-      cmd->host_overwrite = false;
-      cmd->ai_status = AI_STATUS_STOP;
-    } else if (cmd->ai_status == AI_STATUS_AUTOAIM) {
-      /* 自瞄模式中切换失败提醒 */
-    } else {
-      /* ai切换至打符模式，启用host控制 */
-      CMD_RefereeAdd(&(cmd->referee), CMD_UI_HIT_SWITCH_START);
-      cmd->ai_status = AI_STATUS_HITSWITCH;
-      cmd->host_overwrite = true;
-    }
-  }
-  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_AUTOAIM)) {
-    if (cmd->ai_status == AI_STATUS_AUTOAIM) {
-      /* 停止ai的自瞄模式，停用host控制 */
-      cmd->host_overwrite = false;
-      cmd->ai_status = AI_STATUS_STOP;
-      CMD_RefereeAdd(&(cmd->referee), CMD_UI_AUTO_AIM_STOP);
-    } else {
-      /* ai切换至自瞄模式，启用host控制 */
-      cmd->ai_status = AI_STATUS_AUTOAIM;
-      cmd->host_overwrite = true;
-      CMD_RefereeAdd(&(cmd->referee), CMD_UI_AUTO_AIM_START);
-    }
-  } else {
-    cmd->host_overwrite = false;
-    // TODO: 修复逻辑
-  }
-  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_REVTRIG)) {
-    /* 按下拨弹反转 */
-    cmd->shoot.reverse_trig = true;
-  }
-  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FOLLOWGIMBAL35)) {
-    cmd->chassis.mode = CHASSIS_MODE_FOLLOW_GIMBAL_35;
-  }
-  /* 保存当前按下的键位状态 */
-  cmd->key_last = rc->key;
-  memcpy(&(cmd->mouse_last), &(rc->mouse), sizeof(cmd->mouse_last));
-}
-
-/**
- * @brief 解析rc行为逻辑
- *
- * @param rc 遥控器数据
- * @param cmd 主结构体
- * @param dt_sec 两次解析的间隔
- */
-static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
-  switch (rc->sw_l) {
-      /* 左拨杆相应行为选择和解析 */
-    case CMD_SW_UP:
-      cmd->chassis.mode = CHASSIS_MODE_BREAK;
-      break;
-
-    case CMD_SW_MID:
-      cmd->chassis.mode = CHASSIS_MODE_FOLLOW_GIMBAL;
-      break;
-
-    case CMD_SW_DOWN:
-      cmd->chassis.mode = CHASSIS_MODE_ROTOR;
-      cmd->chassis.mode_rotor = ROTOR_MODE_CW;
-      break;
-
-    case CMD_SW_ERR:
-      cmd->chassis.mode = CHASSIS_MODE_RELAX;
-      break;
-  }
-  switch (rc->sw_r) {
-      /* 右拨杆相应行为选择和解析*/ 
-    case CMD_SW_UP:
-      cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
-      cmd->shoot.mode = SHOOT_MODE_SAFE;
-      break;
-
-    case CMD_SW_MID:
-      cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
-      cmd->shoot.fire = false;
-      cmd->shoot.mode = SHOOT_MODE_LOADED;
-      break;
-
-    case CMD_SW_DOWN:
-      cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
-      cmd->shoot.mode = SHOOT_MODE_LOADED;
-      cmd->shoot.fire_mode = FIRE_MODE_SINGLE;
-      cmd->shoot.fire = true;
-      break;
-		/*
-		    case CMD_SW_UP:
-      cmd->gimbal.mode = GIMBAL_MODE_RELAX;
-      cmd->shoot.mode = SHOOT_MODE_SAFE;
-      break;
-
-    case CMD_SW_MID:
-      cmd->gimbal.mode = GIMBAL_MODE_RELAX;
-      cmd->shoot.fire = false;
-      cmd->shoot.mode = SHOOT_MODE_LOADED;
-      break;
-
-    case CMD_SW_DOWN:
-      cmd->gimbal.mode = GIMBAL_MODE_RELAX;
-      cmd->shoot.mode = SHOOT_MODE_LOADED;
-      cmd->shoot.fire_mode = FIRE_MODE_SINGLE;
-      cmd->shoot.fire = true;
-      break;
-		*/
-    case CMD_SW_ERR:
-      cmd->gimbal.mode = GIMBAL_MODE_RELAX;
-      cmd->shoot.mode = SHOOT_MODE_RELAX;
-  }
-  /* 将操纵杆的对应值转换为底盘的控制向量和云台变化的欧拉角 */
-  cmd->chassis.ctrl_vec.vx = rc->ch_l_x;
-  cmd->chassis.ctrl_vec.vy = rc->ch_l_y;
-  cmd->gimbal.delta_eulr.yaw = rc->ch_r_x * dt_sec * cmd->param->sens_rc;
-  cmd->gimbal.delta_eulr.pit = rc->ch_r_y * dt_sec * cmd->param->sens_rc;
-}
-
-/**
- * @brief rc失控时机器人恢复放松模式
- *
- * @param cmd 主结构体
- */
-static void CMD_RcLostLogic(CMD_t *cmd) {
-  /* 机器人底盘、云台、射击运行模式恢复至放松模式 */
-  cmd->chassis.mode = CHASSIS_MODE_RELAX;
-  cmd->gimbal.mode = GIMBAL_MODE_RELAX;
-  cmd->shoot.mode = SHOOT_MODE_RELAX;
-}
-
-/**
- * @brief 初始化命令解析
- *
- * @param cmd 主结构体
- * @param param 参数
- * @return int8_t 0对应没有错误
- */
-int8_t CMD_Init(CMD_t *cmd, const CMD_Params_t *param) {
-  /* 指针检测 */
-  if (cmd == NULL) return -1;
-  if (param == NULL) return -1;
-
-  /* 设置机器人的命令参数，初始化控制方式为rc控制 */
-  cmd->pc_ctrl = false;
-  cmd->param = param;
-
-  return 0;
-}
-
-/**
- * @brief 检查是否启用上位机控制指令覆盖
- *
- * @param cmd 主结构体
- * @return true 启用
- * @return false 不启用
- */
-inline bool CMD_CheckHostOverwrite(CMD_t *cmd) { return cmd->host_overwrite; }
-
-/**
- * @brief 解析命令
- *
- * @param rc 遥控器数据
- * @param cmd 命令
- * @param dt_sec 两次解析的间隔
- * @return int8_t 0对应没有错误
- */
-int8_t CMD_ParseRc(CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
-  /* 指针检测 */
-  if (rc == NULL) return -1;
-  if (cmd == NULL) return -1;
-
-  /* 在pc控制和rc控制间切换 */
-  if (CMD_KeyPressedRc(rc, CMD_KEY_SHIFT) &&
-      CMD_KeyPressedRc(rc, CMD_KEY_CTRL) && CMD_KeyPressedRc(rc, CMD_KEY_Q))
-    cmd->pc_ctrl = true;
-
-  if (CMD_KeyPressedRc(rc, CMD_KEY_SHIFT) &&
-      CMD_KeyPressedRc(rc, CMD_KEY_CTRL) && CMD_KeyPressedRc(rc, CMD_KEY_E))
-    cmd->pc_ctrl = false;
-  /*c当rc丢控时，恢复机器人至默认状态 */
-  if ((rc->sw_l == CMD_SW_ERR) || (rc->sw_r == CMD_SW_ERR)) {
-    CMD_RcLostLogic(cmd);
-  } else {
-    if (cmd->pc_ctrl) {
-      CMD_PcLogic(rc, cmd, dt_sec);
-    } else {
-      CMD_RcLogic(rc, cmd, dt_sec);
-    }
-  }
-  return 0;
-}
-
-/**
- * @brief 解析上位机命令
- *
- * @param host host数据
- * @param cmd 命令
- * @param dt_sec 两次解析的间隔
- * @return int8_t 0对应没有错误
- */
-int8_t CMD_ParseHost(const CMD_Host_t *host, CMD_t *cmd, float dt_sec) {
-  (void)dt_sec; /* 未使用dt_sec，消除警告 */
-  /* 指针检测 */
-  if (host == NULL) return -1;
-  if (cmd == NULL) return -1;
-
-  /* 云台欧拉角设置为host相应的变化的欧拉角 */
-  cmd->gimbal.delta_eulr.yaw = host->gimbal_delta.yaw;
-  cmd->gimbal.delta_eulr.pit = host->gimbal_delta.pit;
-
-  /* host射击命令，设置不同的射击频率和弹丸初速度 */
-  if (host->fire) {
-    cmd->shoot.mode = SHOOT_MODE_LOADED;
-    cmd->shoot.fire = true;
-  } else {
-    cmd->shoot.mode = SHOOT_MODE_SAFE;
-  }
-  return 0;
-}
-
-/**
- * @brief 添加向Referee发送的命令
- *
- * @param ref 命令队列
- * @param cmd 要添加的命令
- * @return int8_t 0对应没有错误
- */
-int8_t CMD_RefereeAdd(CMD_RefereeCmd_t *ref, CMD_UI_t cmd) {
-  /* 指针检测 */
-  if (ref == NULL) return -1;
-  /* 越界检测 */
-  if (ref->counter >= CMD_REFEREE_MAX_NUM || ref->counter < 0) return -1;
-
-  /* 添加机器人当前行为状态到画图的命令队列中 */
-  ref->cmd[ref->counter] = cmd;
-  ref->counter++;
-  return 0;
-}
-
-/* USER FUNCTION BEGIN */
-
-/* USER FUNCTION END */
--- a/User/component/cmd.h
+++ b/User/component/cmd.h
@ -1,318 +0,0 @@
-/*
-  控制命令
-*/
-
-#pragma once
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#include <stdbool.h>
-#include <stdint.h>
-
-#include "component/ahrs.h"
-
-/* USER INCLUDE BEGIN */
-
-/* USER INCLUDE END */
-
-#define CMD_REFEREE_MAX_NUM (3) /* Lines 16 omitted */
-
-/* USER DEFINE BEGIN */
-
-/* USER DEFINE END */
-
-/* 机器人型号 */
-typedef enum {
-  ROBOT_MODEL_INFANTRY = 0, /* 步兵机器人 */
-  ROBOT_MODEL_HERO,         /* 英雄机器人 */
-  ROBOT_MODEL_ENGINEER,     /* 工程机器人 */
-  ROBOT_MODEL_DRONE,        /* 空中机器人 */
-  ROBOT_MODEL_SENTRY,       /* 哨兵机器人 */
-  ROBOT_MODEL_NUM,          /* 型号数量 */
-} CMD_RobotModel_t;
-
-/* 底盘运行模式 */
-typedef enum {
-  CHASSIS_MODE_RELAX, /* 放松模式，电机不输出。一般情况底盘初始化之后的模式 */
-  CHASSIS_MODE_BREAK, /* 刹车模式，电机闭环控制保持静止。用于机器人停止状态 */
-  CHASSIS_MODE_FOLLOW_GIMBAL, /* 通过闭环控制使车头方向跟随云台 */
-  CHASSIS_MODE_FOLLOW_GIMBAL_35, /* 通过闭环控制使车头方向35度跟随云台 */
-  CHASSIS_MODE_ROTOR, /* 小陀螺模式，通过闭环控制使底盘不停旋转 */
-  CHASSIS_MODE_INDENPENDENT, /* 独立模式。底盘运行不受云台影响 */
-  CHASSIS_MODE_OPEN, /* 开环模式。底盘运行不受PID控制，直接输出到电机 */
-} CMD_ChassisMode_t;
-
-/* 云台运行模式 */
-typedef enum {
-  GIMBAL_MODE_RELAX, /* 放松模式，电机不输出。一般情况云台初始化之后的模式 */
-  GIMBAL_MODE_ABSOLUTE, /* 绝对坐标系控制，控制在空间内的绝对姿态 */
-  GIMBAL_MODE_RELATIVE, /* 相对坐标系控制，控制相对于底盘的姿态 */
-} CMD_GimbalMode_t;
-
-/* 射击运行模式 */
-typedef enum {
-  SHOOT_MODE_RELAX,  /* 放松模式，电机不输出 */
-  SHOOT_MODE_SAFE,   /* 保险模式，电机闭环控制保持静止 */
-  SHOOT_MODE_LOADED, /* 上膛模式，摩擦轮开启。随时准备开火 */
-} CMD_ShootMode_t;
-
-typedef enum {
-  FIRE_MODE_SINGLE, /* 单发开火模式  */
-  FIRE_MODE_BURST,  /* N连发开火模式 */
-  FIRE_MODE_CONT,   /* 持续开火模式 */
-  FIRE_MODE_NUM,
-} CMD_FireMode_t;
-
-/* 小陀螺转动模式 */
-typedef enum {
-  ROTOR_MODE_CW,   /* 顺时针转动 */
-  ROTOR_MODE_CCW,  /* 逆时针转动 */
-  ROTOR_MODE_RAND, /* 随机转动 */
-} CMD_RotorMode_t;
-
-/* 底盘控制命令 */
-typedef struct {
-  CMD_ChassisMode_t mode;     /* 底盘运行模式 */
-  CMD_RotorMode_t mode_rotor; /* 小陀螺转动模式 */
-  MoveVector_t ctrl_vec;      /* 底盘控制向量 */
-} CMD_ChassisCmd_t;
-
-/* 云台控制命令 */
-typedef struct {
-  CMD_GimbalMode_t mode;  /* 云台运行模式 */
-  AHRS_Eulr_t delta_eulr; /* 欧拉角变化角度 */
-} CMD_GimbalCmd_t;
-
-/* 射击控制命令 */
-typedef struct {
-  CMD_ShootMode_t mode;     /* 射击运行模式 */
-  CMD_FireMode_t fire_mode; /* 开火模式 */
-  bool fire;                /*开火*/
-  bool cover_open;          /* 弹舱盖开关 */
-  bool reverse_trig;        /* 拨弹电机状态 */
-} CMD_ShootCmd_t;
-
-/* 拨杆位置 */
-typedef enum {
-  CMD_SW_ERR = 0,
-  CMD_SW_UP = 1,
-  CMD_SW_MID = 3,
-  CMD_SW_DOWN = 2,
-} CMD_SwitchPos_t;
-
-/* 键盘按键值 */
-typedef enum {
-  CMD_KEY_W = 0,
-  CMD_KEY_S,
-  CMD_KEY_A,
-  CMD_KEY_D,
-  CMD_KEY_SHIFT,
-  CMD_KEY_CTRL,
-  CMD_KEY_Q,
-  CMD_KEY_E,
-  CMD_KEY_R,
-  CMD_KEY_F,
-  CMD_KEY_G,
-  CMD_KEY_Z,
-  CMD_KEY_X,
-  CMD_KEY_C,
-  CMD_KEY_V,
-  CMD_KEY_B,
-  CMD_L_CLICK,
-  CMD_R_CLICK,
-  CMD_KEY_NUM,
-} CMD_KeyValue_t;
-
-/* 行为值序列 */
-typedef enum {
-  CMD_BEHAVIOR_FORE = 0,       /* 向前 */
-  CMD_BEHAVIOR_BACK,           /* 向后 */
-  CMD_BEHAVIOR_LEFT,           /* 向左 */
-  CMD_BEHAVIOR_RIGHT,          /* 向右 */
-  CMD_BEHAVIOR_ACCELERATE,     /* 加速 */
-  CMD_BEHAVIOR_DECELEBRATE,    /* 减速 */
-  CMD_BEHAVIOR_FIRE,           /* 开火 */
-  CMD_BEHAVIOR_FIRE_MODE,      /* 切换开火模式 */
-  CMD_BEHAVIOR_BUFF,           /* 打符模式 */
-  CMD_BEHAVIOR_AUTOAIM,        /* 自瞄模式 */
-  CMD_BEHAVIOR_OPENCOVER,      /* 弹舱盖开关 */
-  CMD_BEHAVIOR_ROTOR,          /* 小陀螺模式 */
-  CMD_BEHAVIOR_REVTRIG,        /* 反转拨弹 */
-  CMD_BEHAVIOR_FOLLOWGIMBAL35, /* 跟随云台呈35度 */
-  CMD_BEHAVIOR_NUM,
-} CMD_Behavior_t;
-
-typedef enum {
-  CMD_ACTIVE_PRESSING, /* 按下时触发 */
-  CMD_ACTIVE_RASING,   /* 抬起时触发 */
-  CMD_ACTIVE_PRESSED,  /* 按住时触发 */
-} CMD_ActiveType_t;
-
-typedef struct {
-  CMD_ActiveType_t active;
-  CMD_KeyValue_t key;
-} CMD_KeyMapItem_t;
-
-/* 行为映射的对应按键数组 */
-typedef struct {
-  CMD_KeyMapItem_t key_map[CMD_BEHAVIOR_NUM];
-} CMD_KeyMap_Params_t;
-
-/* 位移灵敏度参数 */
-typedef struct {
-  float move_sense;      /* 移动灵敏度 */
-  float move_fast_sense; /* 加速灵敏度 */
-  float move_slow_sense; /* 减速灵敏度 */
-} CMD_Move_Params_t;
-
-typedef struct {
-  uint16_t width;
-  uint16_t height;
-} CMD_Screen_t;
-
-/* 命令参数 */
-typedef struct {
-  float sens_mouse;        /* 鼠标灵敏度 */
-  float sens_rc;           /* 遥控器摇杆灵敏度 */
-  CMD_KeyMap_Params_t map; /* 按键映射行为命令 */
-  CMD_Move_Params_t move;  /* 位移灵敏度参数 */
-  CMD_Screen_t screen;     /* 屏幕分辨率参数 */
-} CMD_Params_t;
-
-/* AI行为状态 */
-typedef enum {
-  AI_STATUS_STOP,      /* 停止状态 */
-  AI_STATUS_AUTOAIM,   /* 自瞄状态 */
-  AI_STATUS_HITSWITCH, /* 打符状态 */
-  AI_STATUS_AUTOMATIC  /* 自动状态 */
-} CMD_AI_Status_t;
-
-/* UI所用行为状态 */
-typedef enum {
-  CMD_UI_NOTHING,          /* 当前无状态 */
-  CMD_UI_AUTO_AIM_START,   /* 自瞄状态开启 */
-  CMD_UI_AUTO_AIM_STOP,    /* 自瞄状态关闭 */
-  CMD_UI_HIT_SWITCH_START, /* 打符状态开启 */
-  CMD_UI_HIT_SWITCH_STOP   /* 打符状态关闭 */
-} CMD_UI_t;
-
-/*裁判系统发送的命令*/
-typedef struct {
-  CMD_UI_t cmd[CMD_REFEREE_MAX_NUM]; /* 命令数组 */
-  uint8_t counter;                   /* 命令计数 */
-} CMD_RefereeCmd_t;
-
-typedef struct {
-  bool pc_ctrl;        /* 是否使用键鼠控制 */
-  bool host_overwrite; /* 是否Host控制 */
-  uint16_t key_last;   /* 上次按键键值 */
-
-  struct {
-    int16_t x;
-    int16_t y;
-    int16_t z;
-    bool l_click; /* 左键 */
-    bool r_click; /* 右键 */
-  } mouse_last;   /* 鼠标值 */
-
-  CMD_AI_Status_t ai_status; /* AI状态 */
-
-  const CMD_Params_t *param; /* 命令参数 */
-
-  CMD_ChassisCmd_t chassis; /* 底盘控制命令 */
-  CMD_GimbalCmd_t gimbal;   /* 云台控制命令 */
-  CMD_ShootCmd_t shoot;     /* 射击控制命令 */
-  CMD_RefereeCmd_t referee; /* 裁判系统发送命令 */
-} CMD_t;
-
-typedef struct {
-  float ch_l_x; /* 遥控器左侧摇杆横轴值，上为正 */
-  float ch_l_y; /* 遥控器左侧摇杆纵轴值，右为正 */
-  float ch_r_x; /* 遥控器右侧摇杆横轴值，上为正 */
-  float ch_r_y; /* 遥控器右侧摇杆纵轴值，右为正 */
-
-  float ch_res; /* 第五通道值 */
-
-  CMD_SwitchPos_t sw_r; /* 右侧拨杆位置 */
-  CMD_SwitchPos_t sw_l; /* 左侧拨杆位置 */
-
-  struct {
-    int16_t x;
-    int16_t y;
-    int16_t z;
-    bool l_click; /* 左键 */
-    bool r_click; /* 右键 */
-  } mouse;        /* 鼠标值 */
-
-  uint16_t key; /* 按键值 */
-
-  uint16_t res; /* 保留，未启用 */
-} CMD_RC_t;
-
-typedef struct {
-  AHRS_Eulr_t gimbal_delta; /* 欧拉角的变化量 */
-
-  struct {
-    float vx;         /* x轴移动速度 */
-    float vy;         /* y轴移动速度 */
-    float wz;         /* z轴转动速度 */
-  } chassis_move_vec; /* 底盘移动向量 */
-
-  bool fire; /* 开火状态 */
-} CMD_Host_t;
-
-/**
- * @brief 解析行为命令
- *
- * @param rc 遥控器数据
- * @param cmd 主结构体
- */
-int8_t CMD_Init(CMD_t *cmd, const CMD_Params_t *param);
-
-/**
- * @brief 检查是否启用上位机控制指令覆盖
- *
- * @param cmd 主结构体
- * @return true 启用
- * @return false 不启用
- */
-bool CMD_CheckHostOverwrite(CMD_t *cmd);
-
-/**
- * @brief 解析命令
- *
- * @param rc 遥控器数据
- * @param cmd 命令
- * @param dt_sec 两次解析的间隔
- * @return int8_t 0对应没有错误
- */
-int8_t CMD_ParseRc(CMD_RC_t *rc, CMD_t *cmd, float dt_sec);
-
-/**
- * @brief 解析上位机命令
- *
- * @param host host数据
- * @param cmd 命令
- * @param dt_sec 两次解析的间隔
- * @return int8_t 0对应没有错误
- */
-int8_t CMD_ParseHost(const CMD_Host_t *host, CMD_t *cmd, float dt_sec);
-
-/**
- * @brief 添加向Referee发送的命令
- *
- * @param ref 命令队列
- * @param cmd 要添加的命令
- * @return int8_t 0对应没有错误
- */
-int8_t CMD_RefereeAdd(CMD_RefereeCmd_t *ref, CMD_UI_t cmd);
-
-/* USER FUNCTION BEGIN */
-
-/* USER FUNCTION END */
-
-#ifdef __cplusplus
-}
-#endif
--- a/User/component/limiter.h
+++ b/User/component/limiter.h
@ -61,3 +61,4 @@ float PowerLimit_TargetPower(float power_limit, float power_buffer);
 */
 float HeatLimit_ShootFreq(float heat, float heat_limit, float cooling_rate,
                          float heat_increase, bool is_big);
+
--- a/User/component/speed_planner.c
+++ b/User/component/speed_planner.c
@ -1,135 +1,266 @@
-/**
- * @file speed_planner.c
- * @brief 斜坡速度规划器实现
- */
-
 #include "component/speed_planner.h"
 #include <math.h>
 #include <string.h>

-/**
- * @brief 限制函数
- */
-static inline float clamp(float value, float min, float max) {
-  if (value < min) return min;
+/* 浮点数比较容差 */
+#define FLOAT_EPSILON 1e-6f
+
+/* 限制函数 */
+static float SpeedPlanner_Limit(float value, float min, float max) {
  if (value > max) return max;
+  if (value < min) return min;
  return value;
 }

-/**
- * @brief 初始化速度规划器
- */
-int8_t SpeedPlanner_Init(SpeedPlanner_t *planner, const SpeedPlanner_Params_t *params) {
-  if (planner == NULL || params == NULL) {
+/* 符号函数 */
+static float SpeedPlanner_Sign(float value) {
+  if (value > FLOAT_EPSILON) return 1.0f;
+  if (value < -FLOAT_EPSILON) return -1.0f;
+  return 0.0f;
+}
+
+/* 获取速度符号，用于判断加速/减速方向 */
+__attribute__((unused)) static float SpeedPlanner_GetVelocitySign(float velocity) {
+  return SpeedPlanner_Sign(velocity);
+}
+
+int8_t SpeedPlanner_Init(SpeedPlanner_t *planner, SpeedPlanner_Param_t *param, float control_freq) {
+  if (planner == NULL || param == NULL || control_freq <= 0.0f) {
    return -1;
  }
  
-  memset(planner, 0, sizeof(SpeedPlanner_t));
-  planner->param = *params;
+  /* 参数检查 */
+  if (param->max_velocity <= 0.0f || param->max_acceleration <= 0.0f || 
+      param->max_deceleration <= 0.0f || param->position_error_limit <= 0.0f) {
+    return -1;
+  }
+  
+  /* 复制参数 */
+  planner->param = *param;
+  
+  /* 计算控制周期 */
+  planner->dt = 1.0f / control_freq;
+  
+  /* 初始化状态 */
+  SpeedPlanner_Reset(planner);
  
  return 0;
 }

-/**
- * @brief 重置速度规划器
- */
-void SpeedPlanner_Reset(SpeedPlanner_t *planner, float current_position, float current_velocity) {
+int8_t SpeedPlanner_Reset(SpeedPlanner_t *planner) {
  if (planner == NULL) {
-    return;
+    return -1;
  }
  
-  planner->current_position = current_position;
-  planner->current_velocity = current_velocity;
-  planner->target_position = current_position;
-  planner->planned_position = current_position;
-  planner->planned_velocity = current_velocity;
-  planner->target_velocity = current_velocity;
+  planner->state = SPEED_PLANNER_IDLE;
+  planner->current_velocity = 0.0f;
+  planner->current_position = 0.0f;
+  planner->target_velocity = 0.0f;
+  planner->target_position = 0.0f;
+  planner->planned_velocity = 0.0f;
+  planner->planned_acceleration = 0.0f;
+  planner->last_target_velocity = 0.0f;
+  planner->position_error = 0.0f;
+  planner->ramp_start_velocity = 0.0f;
+  planner->ramp_end_velocity = 0.0f;
+  planner->ramp_duration = 0.0f;
+  planner->ramp_time = 0.0f;
+  
+  return 0;
 }

-/**
- * @brief 更新速度规划器
- */
-float SpeedPlanner_Update(SpeedPlanner_t *planner, 
-                          float target_velocity,
-                          float current_position,
-                          float current_velocity,
-                          float dt) {
-  if (planner == NULL || dt <= 0.0f) {
-    return 0.0f;
-  }
-  
-  /* 更新当前状态 */
-  planner->current_position = current_position;
-  planner->current_velocity = current_velocity;
-  planner->target_velocity = target_velocity;
-  
-  /* 限制目标速度到最大速度范围 */
-  float limited_target_velocity = clamp(target_velocity, 
-                                        -planner->param.max_velocity, 
-                                        planner->param.max_velocity);
-  
-  /* 计算速度变化 */
-  float velocity_error = limited_target_velocity - planner->planned_velocity;
-  
-  /* 使用最大加速度限制速度变化率 */
-  float max_velocity_change = planner->param.max_acceleration * dt;
-  float velocity_change = clamp(velocity_error, -max_velocity_change, max_velocity_change);
-  
-  /* 更新规划速度 */
-  planner->planned_velocity += velocity_change;
-  
-  /* 限制规划速度 */
-  planner->planned_velocity = clamp(planner->planned_velocity,
-                                    -planner->param.max_velocity,
-                                    planner->param.max_velocity);
-  
-  /* 更新规划位置 */
-  planner->planned_position += planner->planned_velocity * dt;
-  
-  /* 防止位移起飞：限制规划位置与当前位置的误差 */
-  float position_error = planner->planned_position - current_position;
-  
-  if (fabsf(position_error) > planner->param.max_position_error) {
-    /* 位置误差过大，重新规划 */
-    if (position_error > 0.0f) {
-      planner->planned_position = current_position + planner->param.max_position_error;
-    } else {
-      planner->planned_position = current_position - planner->param.max_position_error;
-    }
-    
-    /* 根据位置误差调整速度，使位置逐渐收敛 */
-    /* 使用简单的比例控制，系数可以调整 */
-    float position_correction_gain = 2.0f; // 位置收敛增益
-    planner->planned_velocity = (planner->planned_position - current_position) * position_correction_gain;
-    
-    /* 再次限制速度 */
-    planner->planned_velocity = clamp(planner->planned_velocity,
-                                      -planner->param.max_velocity,
-                                      planner->param.max_velocity);
-  }
-  
-  /* 更新目标位置（用于外部参考） */
-  planner->target_position = planner->planned_position;
-  
-  return planner->planned_velocity;
-}
-
-/**
- * @brief 获取规划后的目标位置
- */
-float SpeedPlanner_GetPlannedPosition(const SpeedPlanner_t *planner) {
+int8_t SpeedPlanner_UpdateState(SpeedPlanner_t *planner, float current_velocity, float current_position) {
  if (planner == NULL) {
-    return 0.0f;
+    return -1;
  }
-  return planner->planned_position;
+  
+  planner->current_velocity = current_velocity;
+  planner->current_position = current_position;
+  planner->position_error = planner->target_position - planner->current_position;
+  
+  return 0;
 }

-/**
- * @brief 获取规划后的速度
- */
-float SpeedPlanner_GetPlannedVelocity(const SpeedPlanner_t *planner) {
+int8_t SpeedPlanner_SetTargetVelocity(SpeedPlanner_t *planner, float target_velocity) {
+  if (planner == NULL) {
+    return -1;
+  }
+  
+  /* 限制目标速度在允许范围内 */
+  planner->target_velocity = SpeedPlanner_Limit(target_velocity, 
+                                                -planner->param.max_velocity, 
+                                                planner->param.max_velocity);
+  
+  return 0;
+}
+
+int8_t SpeedPlanner_SetTargetPosition(SpeedPlanner_t *planner, float target_position) {
+  if (planner == NULL) {
+    return -1;
+  }
+  
+  planner->target_position = target_position;
+  planner->position_error = planner->target_position - planner->current_position;
+  
+  return 0;
+}
+
+/* 检查目标速度是否改变 */
+static int8_t SpeedPlanner_IsTargetChanged(SpeedPlanner_t *planner) {
+  return (fabsf(planner->target_velocity - planner->last_target_velocity) > planner->param.velocity_tolerance);
+}
+
+/* 开始新的斜坡规划 */
+static void SpeedPlanner_StartRamp(SpeedPlanner_t *planner) {
+  planner->ramp_start_velocity = planner->current_velocity;
+  planner->ramp_end_velocity = planner->target_velocity;
+  
+  /* 计算需要的加速度和时间 */
+  float velocity_diff = planner->ramp_end_velocity - planner->ramp_start_velocity;
+  float required_acceleration = fabsf(velocity_diff);
+  
+  /* 选择合适的加速度（加速或减速） */
+  float max_accel = (velocity_diff >= 0) ? planner->param.max_acceleration : planner->param.max_deceleration;
+  
+  if (required_acceleration > max_accel) {
+    required_acceleration = max_accel;
+  }
+  
+  /* 计算斜坡时间 */
+  if (required_acceleration > FLOAT_EPSILON) {
+    planner->ramp_duration = fabsf(velocity_diff) / required_acceleration;
+  } else {
+    planner->ramp_duration = 0.0f;
+  }
+  
+  planner->ramp_time = 0.0f;
+  
+  /* 设置状态 */
+  if (fabsf(velocity_diff) < planner->param.velocity_tolerance) {
+    planner->state = SPEED_PLANNER_FINISHED;
+  } else if (velocity_diff > 0) {
+    planner->state = SPEED_PLANNER_ACCELERATING;
+  } else {
+    planner->state = SPEED_PLANNER_DECELERATING;
+  }
+}
+
+/* 执行斜坡插值 */
+static float SpeedPlanner_RampInterpolation(SpeedPlanner_t *planner) {
+  if (planner->ramp_duration <= FLOAT_EPSILON) {
+    return planner->ramp_end_velocity;
+  }
+  
+  /* 线性插值 */
+  float progress = planner->ramp_time / planner->ramp_duration;
+  progress = SpeedPlanner_Limit(progress, 0.0f, 1.0f);
+  
+  return planner->ramp_start_velocity + 
+         (planner->ramp_end_velocity - planner->ramp_start_velocity) * progress;
+}
+
+int8_t SpeedPlanner_Update(SpeedPlanner_t *planner) {
+  if (planner == NULL) {
+    return -1;
+  }
+  
+  /* 检查位置误差是否超限 */
+  if (fabsf(planner->position_error) > planner->param.position_error_limit) {
+    /* 紧急停止：设置目标速度为0 */
+    planner->target_velocity = 0.0f;
+  }
+  
+  /* 检查目标是否改变 */
+  if (SpeedPlanner_IsTargetChanged(planner) || planner->state == SPEED_PLANNER_IDLE) {
+    SpeedPlanner_StartRamp(planner);
+    planner->last_target_velocity = planner->target_velocity;
+  }
+  
+  /* 根据当前状态执行规划 */
+  switch (planner->state) {
+    case SPEED_PLANNER_IDLE:
+      planner->planned_velocity = planner->current_velocity;
+      planner->planned_acceleration = 0.0f;
+      break;
+      
+    case SPEED_PLANNER_ACCELERATING:
+    case SPEED_PLANNER_DECELERATING:
+      /* 执行斜坡规划 */
+      planner->planned_velocity = SpeedPlanner_RampInterpolation(planner);
+      
+      /* 计算规划加速度 */
+      if (planner->dt > FLOAT_EPSILON) {
+        planner->planned_acceleration = (planner->planned_velocity - planner->current_velocity) / planner->dt;
+      } else {
+        planner->planned_acceleration = 0.0f;
+      }
+      
+      /* 限制加速度 */
+      float max_accel = (planner->state == SPEED_PLANNER_ACCELERATING) ? 
+                        planner->param.max_acceleration : planner->param.max_deceleration;
+      planner->planned_acceleration = SpeedPlanner_Limit(planner->planned_acceleration, -max_accel, max_accel);
+      
+      /* 更新斜坡时间 */
+      planner->ramp_time += planner->dt;
+      
+      /* 检查是否完成斜坡 */
+      if (planner->ramp_time >= planner->ramp_duration || 
+          fabsf(planner->planned_velocity - planner->ramp_end_velocity) < planner->param.velocity_tolerance) {
+        planner->state = SPEED_PLANNER_FINISHED;
+        planner->planned_velocity = planner->ramp_end_velocity;
+      }
+      break;
+      
+    case SPEED_PLANNER_CONSTANT:
+    case SPEED_PLANNER_FINISHED:
+      planner->planned_velocity = planner->target_velocity;
+      planner->planned_acceleration = 0.0f;
+      break;
+      
+    default:
+      planner->state = SPEED_PLANNER_IDLE;
+      break;
+  }
+  
+  /* 最终限制输出速度 */
+  planner->planned_velocity = SpeedPlanner_Limit(planner->planned_velocity, 
+                                                -planner->param.max_velocity, 
+                                                planner->param.max_velocity);
+  
+  return 0;
+}
+
+float SpeedPlanner_GetPlannedVelocity(SpeedPlanner_t *planner) {
  if (planner == NULL) {
    return 0.0f;
  }
  return planner->planned_velocity;
 }
+
+float SpeedPlanner_GetPlannedAcceleration(SpeedPlanner_t *planner) {
+  if (planner == NULL) {
+    return 0.0f;
+  }
+  return planner->planned_acceleration;
+}
+
+float SpeedPlanner_GetPositionError(SpeedPlanner_t *planner) {
+  if (planner == NULL) {
+    return 0.0f;
+  }
+  return planner->position_error;
+}
+
+SpeedPlanner_State_t SpeedPlanner_GetState(SpeedPlanner_t *planner) {
+  if (planner == NULL) {
+    return SPEED_PLANNER_IDLE;
+  }
+  return planner->state;
+}
+
+int8_t SpeedPlanner_IsEmergencyStop(SpeedPlanner_t *planner) {
+  if (planner == NULL) {
+    return 0;
+  }
+  return (fabsf(planner->position_error) > planner->param.position_error_limit) ? 1 : 0;
+}
--- a/User/component/speed_planner.h
+++ b/User/component/speed_planner.h
@ -1,76 +1,143 @@
-/**
- * @file speed_planner.h
- * @brief 斜坡速度规划器
- * @details 提供最大加速度和最大速度限制,防止目标位移起飞
- */
-
 #ifndef SPEED_PLANNER_H
 #define SPEED_PLANNER_H

 #include <stdint.h>

+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* 速度规划器状态枚举 */
+typedef enum {
+  SPEED_PLANNER_IDLE = 0,       /* 空闲状态 */
+  SPEED_PLANNER_ACCELERATING,   /* 加速阶段 */
+  SPEED_PLANNER_CONSTANT,       /* 匀速阶段 */
+  SPEED_PLANNER_DECELERATING,   /* 减速阶段 */
+  SPEED_PLANNER_FINISHED        /* 完成状态 */
+} SpeedPlanner_State_t;
+
 /* 速度规划器参数结构体 */
 typedef struct {
-  float max_velocity;     /* 最大速度 (m/s) */
-  float max_acceleration; /* 最大加速度 (m/s²) */
-  float max_position_error; /* 最大位置误差 (m), 防止位移起飞 */
-} SpeedPlanner_Params_t;
+  float max_velocity;           /* 最大速度 (m/s) */
+  float max_acceleration;       /* 最大加速度 (m/s²) */
+  float max_deceleration;       /* 最大减速度 (m/s²) */
+  float position_error_limit;   /* 位置误差限制 (m) */
+  float velocity_tolerance;     /* 速度容差，用于判断是否到达目标速度 (m/s) */
+} SpeedPlanner_Param_t;

-/* 速度规划器状态结构体 */
+/* 速度规划器结构体 */
 typedef struct {
-  float current_velocity;  /* 当前速度 (m/s) */
-  float target_velocity;   /* 目标速度 (m/s) */
-  float planned_velocity;  /* 规划后的速度 (m/s) */
-  float current_position;  /* 当前位置 (m) */
-  float target_position;   /* 目标位置 (m) */
-  float planned_position;  /* 规划后的位置 (m) */
+  /* 参数 */
+  SpeedPlanner_Param_t param;
  
-  SpeedPlanner_Params_t param; /* 参数 */
+  /* 状态变量 */
+  SpeedPlanner_State_t state;
+  float current_velocity;       /* 当前速度 (m/s) */
+  float current_position;       /* 当前位置 (m) */
+  float target_velocity;        /* 目标速度 (m/s) */
+  float target_position;        /* 目标位置 (m) */
+  
+  /* 规划变量 */
+  float planned_velocity;       /* 规划输出速度 (m/s) */
+  float planned_acceleration;   /* 规划输出加速度 (m/s²) */
+  
+  /* 内部状态 */
+  float last_target_velocity;   /* 上次目标速度，用于检测目标变化 */
+  float position_error;         /* 位置误差 (目标位置 - 当前位置) */
+  float dt;                     /* 控制周期 (s) */
+  
+  /* 斜坡规划状态 */
+  float ramp_start_velocity;    /* 斜坡起始速度 */
+  float ramp_end_velocity;      /* 斜坡结束速度 */
+  float ramp_duration;          /* 斜坡持续时间 */
+  float ramp_time;              /* 斜坡当前时间 */
 } SpeedPlanner_t;

 /**
 * @brief 初始化速度规划器
- * @param planner 规划器结构体指针
- * @param params 参数结构体指针
- * @return 0:成功, -1:失败
+ * @param planner 速度规划器指针
+ * @param param 参数结构体指针
+ * @param control_freq 控制频率 (Hz)
+ * @return 0: 成功, -1: 失败
 */
-int8_t SpeedPlanner_Init(SpeedPlanner_t *planner, const SpeedPlanner_Params_t *params);
+int8_t SpeedPlanner_Init(SpeedPlanner_t *planner, SpeedPlanner_Param_t *param, float control_freq);
+
+/**
+ * @brief 更新当前状态
+ * @param planner 速度规划器指针
+ * @param current_velocity 当前速度 (m/s)
+ * @param current_position 当前位置 (m)
+ * @return 0: 成功, -1: 失败
+ */
+int8_t SpeedPlanner_UpdateState(SpeedPlanner_t *planner, float current_velocity, float current_position);
+
+/**
+ * @brief 设置目标速度
+ * @param planner 速度规划器指针
+ * @param target_velocity 目标速度 (m/s)
+ * @return 0: 成功, -1: 失败
+ */
+int8_t SpeedPlanner_SetTargetVelocity(SpeedPlanner_t *planner, float target_velocity);
+
+/**
+ * @brief 设置目标位置
+ * @param planner 速度规划器指针
+ * @param target_position 目标位置 (m)
+ * @return 0: 成功, -1: 失败
+ */
+int8_t SpeedPlanner_SetTargetPosition(SpeedPlanner_t *planner, float target_position);
+
+/**
+ * @brief 执行速度规划计算
+ * @param planner 速度规划器指针
+ * @return 0: 成功, -1: 失败
+ */
+int8_t SpeedPlanner_Update(SpeedPlanner_t *planner);
+
+/**
+ * @brief 获取规划的速度输出
+ * @param planner 速度规划器指针
+ * @return 规划的速度 (m/s)
+ */
+float SpeedPlanner_GetPlannedVelocity(SpeedPlanner_t *planner);
+
+/**
+ * @brief 获取规划的加速度输出
+ * @param planner 速度规划器指针
+ * @return 规划的加速度 (m/s²)
+ */
+float SpeedPlanner_GetPlannedAcceleration(SpeedPlanner_t *planner);
+
+/**
+ * @brief 获取位置误差
+ * @param planner 速度规划器指针
+ * @return 位置误差 (m)
+ */
+float SpeedPlanner_GetPositionError(SpeedPlanner_t *planner);
+
+/**
+ * @brief 获取当前状态
+ * @param planner 速度规划器指针
+ * @return 当前状态
+ */
+SpeedPlanner_State_t SpeedPlanner_GetState(SpeedPlanner_t *planner);

 /**
 * @brief 重置速度规划器
- * @param planner 规划器结构体指针
- * @param current_position 当前位置
- * @param current_velocity 当前速度
+ * @param planner 速度规划器指针
+ * @return 0: 成功, -1: 失败
 */
-void SpeedPlanner_Reset(SpeedPlanner_t *planner, float current_position, float current_velocity);
+int8_t SpeedPlanner_Reset(SpeedPlanner_t *planner);

 /**
- * @brief 更新速度规划器
- * @param planner 规划器结构体指针
- * @param target_velocity 目标速度 (m/s)
- * @param current_position 当前位置 (m)
- * @param current_velocity 当前速度 (m/s)
- * @param dt 时间间隔 (s)
- * @return 规划后的速度 (m/s)
+ * @brief 检查是否需要紧急停止（位置误差过大）
+ * @param planner 速度规划器指针
+ * @return 1: 需要紧急停止, 0: 正常
 */
-float SpeedPlanner_Update(SpeedPlanner_t *planner, 
-                          float target_velocity,
-                          float current_position,
-                          float current_velocity,
-                          float dt);
+int8_t SpeedPlanner_IsEmergencyStop(SpeedPlanner_t *planner);

-/**
- * @brief 获取规划后的目标位置
- * @param planner 规划器结构体指针
- * @return 规划后的目标位置 (m)
- */
-float SpeedPlanner_GetPlannedPosition(const SpeedPlanner_t *planner);
-
-/**
- * @brief 获取规划后的速度
- * @param planner 规划器结构体指针
- * @return 规划后的速度 (m/s)
- */
-float SpeedPlanner_GetPlannedVelocity(const SpeedPlanner_t *planner);
+#ifdef __cplusplus
+}
+#endif

 #endif // SPEED_PLANNER_H
--- a/User/component/vmc.c
+++ b/User/component/vmc.c
@ -127,6 +127,7 @@ int8_t VMC_ForwardSolve(VMC_t *vmc, float phi1, float phi4, float body_pitch, fl
    // 计算角加速度
    leg->dd_theta = VMC_ComputeNumericDerivative(leg->d_theta, leg->last_d_theta, vmc->dt);
    
+      VMC_GroundContactDetection(vmc); // 更新地面接触状态
    return 0;
 }

@ -175,10 +176,10 @@ float VMC_GroundContactDetection(VMC_t *vmc) {
    // Fn = F0*cos(theta) + Tp*sin(theta)/L0 + mg
    leg->Fn = leg->F_virtual * cosf(leg->theta) + 
              leg->T_virtual * sinf(leg->theta) / leg->L0 + 
-              vmc->param.wheel_mass * 9.8f;  // 添加轮子重力
+              10.0f;  // 添加轮子重力，假设轮子质量约1kg
    
    // 地面接触判断
-    leg->is_ground_contact = (leg->Fn > 10.0f);  // 10N阈值
+    leg->is_ground_contact = (leg->Fn > 20.0f);  // 10N阈值
    
    return leg->Fn;
 }
--- a/User/module/balance_chassis.c
+++ b/User/module/balance_chassis.c
@ -42,6 +42,19 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) {
  if (mode == c->mode)
    return CHASSIS_OK; /* 模式未改变直接返回 */

+  // 特殊处理：从JUMP切换到WHELL_LEG_BALANCE时不重置
+  // 但从WHELL_LEG_BALANCE切换到JUMP时，需要重置以触发新的跳跃
+  if (c->mode == CHASSIS_MODE_JUMP && mode == CHASSIS_MODE_WHELL_LEG_BALANCE) {
+    c->mode = mode;
+    return CHASSIS_OK;
+  }
+  if (c->mode == CHASSIS_MODE_WHELL_LEG_BALANCE && mode == CHASSIS_MODE_JUMP) {
+    c->mode = mode;
+    c->state = 0; // 重置状态，确保每次切换模式时都重新初始化
+    c->jump_time=0;  // 重置跳跃时间，切换到JUMP模式时会触发新跳跃
+    return CHASSIS_OK;
+  }
+
  Chassis_MotorEnable(c);

  PID_Reset(&c->pid.leg_length[0]);
@ -51,13 +64,15 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) {
  PID_Reset(&c->pid.tp[0]);
  PID_Reset(&c->pid.tp[1]);

-  c->yaw_control.target_yaw = c->feedback.imu.euler.yaw;
+  c->yaw_control.target_yaw = c->feedback.yaw.rotor_abs_angle;

  // 清空位移
  c->chassis_state.position_x = 0.0f;
  c->chassis_state.velocity_x = 0.0f;
  c->chassis_state.last_velocity_x = 0.0f;
  c->chassis_state.target_x = 0.0f;
+  c->chassis_state.target_velocity_x = 0.0f;
+  c->chassis_state.last_target_velocity_x = 0.0f;

  LQR_Reset(&c->lqr[0]);
  LQR_Reset(&c->lqr[1]);
@ -69,9 +84,10 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) {
  LowPassFilter2p_Reset(&c->filter.wheel_out[0], 0.0f);
  LowPassFilter2p_Reset(&c->filter.wheel_out[1], 0.0f);

-
  c->mode = mode;
  c->state = 0; // 重置状态，确保每次切换模式时都重新初始化
+  c->jump_time=0;  // 重置跳跃时间，切换到JUMP模式时会触发新跳跃
+  c->wz_multi=0.0f;

  return CHASSIS_OK;
 }
@ -104,6 +120,8 @@ static void Chassis_UpdateChassisState(Chassis_t *c) {
  c->chassis_state.position_x += c->chassis_state.velocity_x * c->dt;
 }

+
+
 int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) {
  if (c == NULL || param == NULL || target_freq <= 0.0f) {
    return -1; // 参数错误
@ -157,6 +175,8 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) {
  c->chassis_state.velocity_x = 0.0f;
  c->chassis_state.last_velocity_x = 0.0f;
  c->chassis_state.target_x = 0.0f;
+  c->chassis_state.target_velocity_x = 0.0f;
+  c->chassis_state.last_target_velocity_x = 0.0f;

  /*初始化yaw控制状态*/
  c->yaw_control.target_yaw = 0.0f;
@ -249,14 +269,54 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
    Chassis_Output(c); // 统一输出

  } break;
-
-  case CHASSIS_MODE_WHELL_LEG_BALANCE:
+  case CHASSIS_MODE_JUMP:
+    // 跳跃模式状态机
+    // 状态转换: 0(准备)->1(0.3s)->2(0.2s)->3(0.3s)->0(完成)
+    // jump_time == 0: 未开始跳跃
+    // jump_time == 1: 已完成跳跃（不再触发）
+    // jump_time > 1: 跳跃进行中
+    
+    // 检测是否需要开始新的跳跃（state为0且jump_time为0）
+    if (c->state == 0 && c->jump_time == 0) {
+      // 开始跳跃，进入state 1
+      c->state = 1;
+      c->jump_time = BSP_TIME_Get_us();
+    }
+    
+    // 只有在跳跃进行中时才处理状态转换（jump_time > 1）
+    if (c->jump_time > 1) {
+      // 计算已经过的时间（微秒）
+      uint64_t elapsed_us = BSP_TIME_Get_us() - c->jump_time;
+      
+      // 状态转换逻辑
+      if (c->state == 1 && elapsed_us >= 300000) {
+        // state 1 保持0.3s后转到state 2
+        c->state = 2;
+        c->jump_time = BSP_TIME_Get_us(); // 重置计时
+      } else if (c->state == 2 && elapsed_us >= 160000) {
+        // state 2 保持0.2s后转到state 3
+        c->state = 3;
+        c->jump_time = BSP_TIME_Get_us(); // 重置计时
+      } else if (c->state == 3 && elapsed_us >= 160000) {
+        // state 3 保持0.3s后转到state 0
+        c->state = 0;
+        c->jump_time = 1; // 设置为1，表示已完成跳跃，不再触发
+      }
+    }
+    
    // 执行LQR控制（包含PID腿长控制）
    Chassis_LQRControl(c, c_cmd);
+    Chassis_Output(c); // 统一输出
+    break;
+  case CHASSIS_MODE_WHELL_LEG_BALANCE:
+  case CHASSIS_MODE_ROTOR:

+    // 执行LQR控制（包含PID腿长控制）
+    Chassis_LQRControl(c, c_cmd);
    Chassis_Output(c); // 统一输出
    break;

+    break;
  default:
    return CHASSIS_ERR_MODE;
  }
@ -296,15 +356,33 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
  /* 运动参数（参考C++版本的状态估计） */
  static float xhat = 0.0f, x_dot_hat = 0.0f;
  float target_dot_x = 0.0f;
+  float max_acceleration = 3.0f; // 最大加速度限制: 3 m/s²

  // 简化的速度估计（后续可以改进为C++版本的复杂滤波）
  x_dot_hat = c->chassis_state.velocity_x;
  xhat = c->chassis_state.position_x;

-  // 目标设定
-  target_dot_x = c_cmd->move_vec.vx * 2;
-  // target_dot_x = SpeedLimit_TargetSpeed(300.0f, c->chassis_state.velocity_x,
-  // target_dot_x, c->dt); // 速度限制器，假设最大加速度为1 m/s²
+  // 目标速度设定（原始期望速度）
+  float desired_velocity = c_cmd->move_vec.vx * 2;
+
+  // 加速度限制处理
+  float velocity_change =
+      desired_velocity - c->chassis_state.last_target_velocity_x;
+  float max_velocity_change = max_acceleration * c->dt; // 最大允许的速度变化
+
+  // 限制速度变化率（加速度限制）
+  if (velocity_change > max_velocity_change) {
+    velocity_change = max_velocity_change;
+  } else if (velocity_change < -max_velocity_change) {
+    velocity_change = -max_velocity_change;
+  }
+
+  // 应用加速度限制后的目标速度
+  target_dot_x = c->chassis_state.last_target_velocity_x + velocity_change;
+  c->chassis_state.target_velocity_x = target_dot_x;
+  c->chassis_state.last_target_velocity_x = target_dot_x;
+
+  // 更新目标位置
  c->chassis_state.target_x += target_dot_x * c->dt;

  /* 分别计算左右腿的LQR控制 */
@ -329,25 +407,53 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {

  /* 构建目标状态 */
  LQR_State_t target_state = {0};
-  target_state.theta = 0.0f;                  // 目标摆杆角度
+  target_state.theta = c->param->theta;                  // 目标摆杆角度
  target_state.d_theta = 0.0f;                // 目标摆杆角速度
-  target_state.phi = -0.1f;                    // 目标俯仰角
+  target_state.phi = 11.9601*pow((0.15f + c_cmd->height * 0.25f),3) + -11.8715*pow((0.15f + c_cmd->height * 0.25f),2) + 3.87083*(0.15f + c_cmd->height * 0.25f) + -0.414154;                   // 目标俯仰角
  target_state.d_phi = 0.0f;                  // 目标俯仰角速度
-  target_state.x = c->chassis_state.target_x; // 目标位置
+  target_state.x = c->chassis_state.target_x -2.07411f*(0.15f + c_cmd->height * 0.25f) + 0.41182f;
  target_state.d_x = target_dot_x;            // 目标速度

-  /* 更新LQR状态 */
-  LQR_SetTargetState(&c->lqr[0], &target_state);
-  LQR_SetTargetState(&c->lqr[1], &target_state);
-
-  LQR_Control(&c->lqr[0]);
-  LQR_Control(&c->lqr[1]);
-
+  if (c->mode != CHASSIS_MODE_ROTOR){
  c->yaw_control.current_yaw = c->feedback.yaw.rotor_abs_angle;
-  c->yaw_control.target_yaw = c->param->mech_zero_yaw;
+  c->yaw_control.target_yaw =
+      c->param->mech_zero_yaw + c_cmd->move_vec.vy * M_PI_2;
  c->yaw_control.yaw_force =
      PID_Calc(&c->pid.yaw, c->yaw_control.target_yaw,
               c->feedback.yaw.rotor_abs_angle, 0.0f, c->dt);
+  }else{
+    c->yaw_control.current_yaw = c->feedback.yaw.rotor_abs_angle;
+    c->yaw_control.target_yaw = c->yaw_control.current_yaw + 0.5f;
+    c->yaw_control.yaw_force =
+        PID_Calc(&c->pid.yaw, c->yaw_control.target_yaw,
+                 c->feedback.yaw.rotor_abs_angle, 0.0f, c->dt);
+  }
+
+  if (c->vmc_[0].leg.is_ground_contact) {
+      /* 更新LQR状态 */
+    LQR_SetTargetState(&c->lqr[0], &target_state);
+    LQR_Control(&c->lqr[0]);
+  } else {
+    target_state.theta = 0.15f; // 非接触时，腿摆角目标为0.1rad，防止腿完全悬空
+    LQR_SetTargetState(&c->lqr[0], &target_state);
+    c->lqr[0].control_output.T = 0.0f;
+    c->lqr[0].control_output.Tp =
+        c->lqr[0].K_matrix[1][0] * (-c->vmc_[0].leg.theta + 0.0f) +
+        c->lqr[0].K_matrix[1][1] * (-c->vmc_[0].leg.d_theta + 0.0f);
+    c->yaw_control.yaw_force = 0.0f; // 单腿离地时关闭偏航控制
+  }
+  if (c->vmc_[1].leg.is_ground_contact){
+    LQR_SetTargetState(&c->lqr[1], &target_state);
+    LQR_Control(&c->lqr[1]);
+  }else {
+    target_state.theta = 0.15f; // 非接触时，腿摆角目标为0.1rad，防止腿完全悬空
+    LQR_SetTargetState(&c->lqr[1], &target_state);
+    c->lqr[1].control_output.T = 0.0f;
+    c->lqr[1].control_output.Tp =
+        c->lqr[1].K_matrix[1][0] * (-c->vmc_[1].leg.theta + 0.0f) +
+        c->lqr[1].K_matrix[1][1] * (-c->vmc_[1].leg.d_theta + 0.0f);
+    c->yaw_control.yaw_force = 0.0f; // 单腿离地时关闭偏航控制
+  }

  /* 轮毂力矩输出（参考C++版本的减速比） */
  c->output.wheel[0] =
@ -372,8 +478,28 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
    roll_compensation_force = -20.0f;

  // 目标腿长设定（移除横滚角补偿）
-  target_L0[0] = 0.15f + c_cmd->height * 0.2f; // 左腿：基础腿长 + 高度调节
-  target_L0[1] = 0.15f + c_cmd->height * 0.2f; // 右腿：基础腿长 + 高度调节
+  switch (c->state) {
+  case 0: // 正常状态，根据高度指令调节腿长
+    target_L0[0] = 0.15f + c_cmd->height * 0.22f; // 左腿：基础腿长 + 高度调节
+    target_L0[1] = 0.15f + c_cmd->height * 0.22f; // 右腿：基础腿长 + 高度调节
+    break;
+  case 1: // 准备阶段，腿长收缩
+    target_L0[0] = 0.15f; // 左腿：收缩到基础腿长
+    target_L0[1] = 0.15f; // 右腿：收缩到基础腿长
+    break;
+  case 2: // 起跳阶段，腿长快速伸展
+    target_L0[0] = 0.55f; // 左腿：伸展到最大腿长
+    target_L0[1] = 0.55f; // 右腿：伸展到最大腿长
+    break;
+  case 3: // 着地阶段，腿长缓冲
+    target_L0[0] = 0.1f; // 左腿：缓冲腿长
+    target_L0[1] = 0.1f; // 右腿：缓冲腿长
+    break;
+  default:
+    target_L0[0] = 0.15f + c_cmd->height * 0.22f;
+    target_L0[1] = 0.15f + c_cmd->height * 0.22f;
+    break;
+  }

  // 获取腿长变化率
  VMC_GetVirtualLegState(&c->vmc_[0], NULL, NULL, &leg_d_length[0], NULL);
@ -401,15 +527,14 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
    // 横滚角补偿力（左腿减少支撑力）
    virtual_force[0] =
        (c->lqr[0].control_output.Tp) * sinf(c->vmc_[0].leg.theta) +
-        pid_output + 60.0f + roll_compensation_force;
+        pid_output + 55.0f + roll_compensation_force;

    // VMC逆解算（包含摆角补偿）
    if (VMC_InverseSolve(&c->vmc_[0], virtual_force[0],
                         c->lqr[0].control_output.Tp + Delta_Tp[0]) == 0) {
-    // if (VMC_InverseSolve(&c->vmc_[0], 0.0f,
-    //                      Delta_Tp[0]) == 0) {
      VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0],
                          &c->output.joint[1]);
+
    } else {
      // VMC失败，设为0力矩
      c->output.joint[0] = 0.0f;
@ -427,13 +552,11 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
    // 横滚角补偿力（右腿增加支撑力）
    virtual_force[1] =
        (c->lqr[1].control_output.Tp) * sinf(c->vmc_[1].leg.theta) +
-        pid_output + 65.0f - roll_compensation_force;
+        pid_output + 60.0f - roll_compensation_force;

    // VMC逆解算（包含摆角补偿）
    if (VMC_InverseSolve(&c->vmc_[1], virtual_force[1],
                         c->lqr[1].control_output.Tp + Delta_Tp[1]) == 0) {
-    // if (VMC_InverseSolve(&c->vmc_[1], 0.0f,
-    //                      Delta_Tp[1]) == 0) {
      VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3],
                          &c->output.joint[2]);
    } else {
@ -452,9 +575,11 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {

  for (int i = 0; i < 4; i++) {
    if (fabsf(c->output.joint[i]) > 20.0f) {
-      c->output.joint[i] = (c->output.joint[i] > 0) ? 20.0f : -20.0f;
+      c->output.joint[i] = (c->output.joint[i] > 0) ? 25.0f : -25.0f;
    }
  }

  return CHASSIS_OK;
 }
+
+
--- a/User/module/balance_chassis.h
+++ b/User/module/balance_chassis.h
@ -40,8 +40,17 @@ typedef enum {
  CHASSIS_MODE_RELAX,            /* 放松模式，电机不输出。一般情况底盘初始化之后的模式 */
  CHASSIS_MODE_RECOVER,                 /* 复位模式 */
  CHASSIS_MODE_WHELL_LEG_BALANCE,      /* 轮子+腿平衡模式，底盘自我平衡 */
+  CHASSIS_MODE_JUMP,                   /* 跳跃模式，底盘跳跃 */
+  CHASSIS_MODE_ROTOR,                  /* 小陀螺模式，底盘高速旋转 */
 } Chassis_Mode_t;

+typedef enum {
+  CHASSIS_JUMP_STATE_READY,  /* 准备跳跃 */
+  CHASSIS_JUMP_STATE_JUMPING, /* 跳跃中 */
+  CHASSIS_JUMP_STATE_LANDING, /* 着陆中 */
+  CHASSIS_JUMP_STATE_END, /* 跳跃结束 */
+} Chassis_JumpState_t;
+
 typedef struct {
  Chassis_Mode_t mode;           /* 底盘模式 */
  MoveVector_t move_vec;         /* 底盘运动向量 */
@ -83,6 +92,10 @@ typedef struct {

  float mech_zero_yaw; /* 机械零点 */

+  float theta;
+  float x;
+  float phi;
+
  /* 低通滤波器截止频率 */
  struct {
    float in;  /* 输入 */
@ -113,6 +126,8 @@ typedef struct {
  LQR_t lqr[2]; /* 两条腿的LQR控制器 */

  int8_t state;
+  uint64_t jump_time;
+

  float angle;
  float height;
@ -123,6 +138,8 @@ typedef struct {
    float velocity_x;     /* 机体x速度 */
    float last_velocity_x; /* 上一次速度用于数值微分 */
    float target_x;       /* 目标x位置 */
+    float target_velocity_x; /* 目标速度 */
+    float last_target_velocity_x; /* 上一次目标速度 */
  } chassis_state;

  /* yaw控制相关 */
@ -206,6 +223,7 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd);
 int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd);


+
 /**
 * \brief 底盘输出值
 *
--- a/User/module/cmd.c
+++ b/User/module/cmd.c
--- a/User/module/cmd.h
+++ b/User/module/cmd.h
@ -0,0 +1,19 @@
+/*
+  控制命令
+*/
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdbool.h>
+#include <stdint.h>
+
+
+
+
+#ifdef __cplusplus
+}
+#endif
--- a/User/module/config.c
+++ b/User/module/config.c
@ -20,7 +20,7 @@ Config_RobotParam_t robot_config = {
    .shoot_param = {
            .trig_step_angle=M_2PI/8,
            .shot_delay_time=0.05f,
-            .shot_burst_num=1,
+            .shot_burst_num=50,
            .fric_motor_param[0] = {
                .can = BSP_CAN_2,
                .id = 0x201,
@ -168,8 +168,8 @@ Config_RobotParam_t robot_config = {

    .chassis_param = {
        .yaw={
-            .k=0.8f,
-            .p=1.2f,
+            .k=1.0f,
+            .p=1.0f,
            .i=0.01f,
            .d=0.05f,
            .i_limit=0.0f,
@ -211,12 +211,12 @@ Config_RobotParam_t robot_config = {
        },

        .tp={
-            .k=1.0f,
+            .k=2.0f,
            .p=2.0f,          /* 俯仰角比例系数 */
            .i=0.0f,           /* 俯仰角积分系数 */
            .d=0.0f,           /* 俯仰角微分系数 */
            .i_limit=0.0f,     /* 积分限幅 */
-            .out_limit=2.0f,   /* 输出限幅，腿长差值限制 */
+            .out_limit=10.0f,   /* 输出限幅，腿长差值限制 */
            .d_cutoff_freq=30.0f, /* 微分截止频率 */
            .range=-1.0f,      /* 不使用循环误差处理 */
        },
@ -253,21 +253,26 @@ Config_RobotParam_t robot_config = {
                .hip_length = 0.0f    // 髋宽 (L5) (m)
            }
        },
+        
 .lqr_gains = {
-    .k11_coeff = { -1.508572585852218e+02f, 1.764949368139731e+02f, -9.850368126414553e+01f, -1.786139736968997e+00f }, // theta
-    .k12_coeff = { 6.466280284100411e+00f, -6.582699834130516e+00f, -7.131858380770703e+00f, -2.414590752579311e-01f },  // d_theta
-    .k13_coeff = { -7.182568574598301e+01f, 7.405968297046749e+01f, -2.690651220502175e+01f, -1.029850390463813e-01f },  // x
-    .k14_coeff = { -7.645548919162933e+01f, 7.988837668034076e+01f, -3.105733981791483e+01f, -4.296847184537235e-01f },  // d_x
-    .k15_coeff = { -9.403058188674812e+00f, 2.314767704216332e+01f, -1.651965553704901e+01f, 3.907902082528794e+00f }, // phi
-    .k16_coeff = { -4.023111056381187e+00f, 6.154951770170482e+00f, -3.705537084098432e+00f, 8.264904615264155e-01f },  // d_phi
-    .k21_coeff = { 1.254775776629822e+02f, -8.971732439896309e+01f, 4.744038360386896e+00f, 1.088353622361175e+01f }, // theta
-    .k22_coeff = { 1.061139172689013e+01f, -1.011235824540459e+01f, 3.034478775177782e+00f, 1.254920921163464e+00f },  // d_theta
-    .k23_coeff = { -2.675556963667067e+01f, 4.511381902505539e+01f, -2.800741296230217e+01f, 7.647205920058282e+00f },  // x
-    .k24_coeff = { -4.067721095665326e+01f, 6.068996620706580e+01f, -3.488384556019462e+01f, 9.374136714284193e+00f },  // d_x
-    .k25_coeff = { 7.359316334738203e+01f, -7.896223244854855e+01f, 2.961892943386949e+01f, 4.406632136721399e+00f }, // phi
-    .k26_coeff = { 1.624843000878248e+01f, -1.680831323767654e+01f, 6.018754311678180e+00f, 2.337719500754984e-01f },  // d_phi
+    .k11_coeff = { -1.498109852818409e+02f, 1.918923604951453e+02f, -1.080984818173493e+02f, -1.540703437137126e+00f }, // theta
+    .k12_coeff = { 8.413682810667103e+00f, -6.603584762798329e+00f, -6.421063158996702e+00f, -5.450666844349098e-02f },  // d_theta
+    .k13_coeff = { -3.146938649452867e+01f, 3.671781823697795e+01f, -1.548019975847165e+01f, -3.659482046966758e-01f },  // x
+    .k14_coeff = { -3.363190268966418e+01f, 4.073114332036178e+01f, -1.877821151363973e+01f, -6.755848684037919e-01f },  // d_x
+    .k15_coeff = { 1.813346556940570e+00f, 1.542139674818206e+01f, -1.784151260000496e+01f, 6.975189972486323e+00f }, // phi
+    .k16_coeff = { -1.683339907923917e+00f, 3.762911505427638e+00f, -2.966405826579746e+00f, 1.145611903160937e+00f },  // d_phi
+    .k21_coeff = { 3.683683451383080e+02f, -3.184826128050574e+02f, 7.012281968985167e+01f, 1.232691707185163e+01f }, // theta
+    .k22_coeff = { 2.256451382235226e+01f, -2.387074220964917e+01f, 8.355357863648345e+00f, 1.148384164091676e+00f },  // d_theta
+    .k23_coeff = { 1.065026516142075e+01f, 1.016717837738013e+01f, -1.810442639595643e+01f, 7.368019318950717e+00f },  // x
+    .k24_coeff = { 2.330418054102148e+00f, 2.193671212435775e+01f, -2.502198732490354e+01f, 9.621144599080463e+00f },  // d_x
+    .k25_coeff = { 1.784444885521937e+02f, -2.030738611028434e+02f, 8.375405599993576e+01f, -2.204042546242858e-01f }, // phi
+    .k26_coeff = { 2.646425532528492e+01f, -3.034702782249508e+01f, 1.275100635568078e+01f, -4.878639273974432e-01f },  // d_phi
 },

+        .theta = 0.0f,
+        .x = 0.0f,
+        .phi = -0.1f,
+
        .virtual_chassis_param = {
            .motors = {
                    .can = BSP_CAN_1,
--- a/User/module/shoot.c
+++ b/User/module/shoot.c
@ -133,7 +133,7 @@ int8_t Shoot_Init(Shoot_t *s, Shoot_Params_t *param, float target_freq)
 	
    memset(&s->shoot_Anglecalu,0,sizeof(s->shoot_Anglecalu));
    memset(&s->output,0,sizeof(s->output));
-    s->target_variable.target_rpm=4000.0f;
+    s->target_variable.target_rpm=5000.0f;

 	return 0;
 }
@ -303,3 +303,4 @@ int8_t Shoot_Control(Shoot_t *s, Shoot_CMD_t *cmd)



+
--- a/User/task/atti_esti.c
+++ b/User/task/atti_esti.c
@ -6,11 +6,14 @@
 /* Includes ----------------------------------------------------------------- */
 #include "task/user_task.h"
 /* USER INCLUDE BEGIN */
+#include "bsp/mm.h"
 #include "bsp/pwm.h"
+#include "bsp/gpio.h"
 #include "component/ahrs.h"
 #include "component/pid.h"
-#include "device/bmi088.h"
 #include "module/gimbal.h"
+#include "device/bmi088.h"
+#include "task/user_task.h"
 /* USER INCLUDE END */

 /* Private typedef ---------------------------------------------------------- */
@ -29,9 +32,42 @@ KPID_t imu_temp_ctrl_pid;
 Gimbal_IMU_t gimbal_to_send;

 BMI088_Cali_t cali_bmi088= {
-  .gyro_offset = {0.0f,0.0f,0.0f},
+  .gyro_offset = {-0.00149519544f,-0.00268901442f,0.00169837975f},
 };

+static const KPID_Params_t imu_temp_ctrl_pid_param = {
+    .k = 0.3f,
+    .p = 1.0f,
+    .i = 0.01f,
+    .d = 0.0f,
+    .i_limit = 1.0f,
+    .out_limit = 1.0f,
+};
+
+/* 校准相关变量 */
+typedef enum {
+  CALIB_IDLE,     // 空闲状态
+  CALIB_RUNNING,  // 正在校准
+  CALIB_DONE      // 校准完成
+} CalibState_t;
+
+static CalibState_t calib_state = CALIB_IDLE;
+static uint16_t calib_count = 0;
+static struct {
+  float x_sum;
+  float y_sum; 
+  float z_sum;
+} gyro_sum= {0.0f,0.0f,0.0f};
+static void start_gyro_calibration(void) {
+  if (calib_state == CALIB_IDLE) {
+    calib_state = CALIB_RUNNING;
+    calib_count = 0;
+    gyro_sum.x_sum = 0.0f;
+    gyro_sum.y_sum = 0.0f;
+    gyro_sum.z_sum = 0.0f;
+  }
+}
+#define CALIB_SAMPLES 5000  // 校准采样数量

 /* USER STRUCT END */

@ -50,7 +86,13 @@ void Task_atti_esti(void *argument) {
  /* USER CODE INIT BEGIN */
  BMI088_Init(&bmi088,&cali_bmi088);
  AHRS_Init(&gimbal_ahrs, &magn, BMI088_GetUpdateFreq(&bmi088));
-
+  PID_Init(&imu_temp_ctrl_pid, KPID_MODE_NO_D,
+           1.0f / BMI088_GetUpdateFreq(&bmi088), &imu_temp_ctrl_pid_param);
+  BSP_PWM_Start(BSP_PWM_IMU_HEAT_PWM);
+  
+  /* 注册按钮回调函数并启用中断 */
+  BSP_GPIO_RegisterCallback(BSP_GPIO_USER_KEY, start_gyro_calibration);
+  BSP_GPIO_EnableIRQ(BSP_GPIO_USER_KEY);
  /* USER CODE INIT END */
  
  while (1) {
@ -70,19 +112,50 @@ void Task_atti_esti(void *argument) {
    BMI088_ParseGyro(&bmi088);
    // IST8310_Parse(&ist8310);

-    /* 根据设备接收到的数据进行姿态解析 */
-    AHRS_Update(&gimbal_ahrs, &bmi088.accl, &bmi088.gyro, &magn);
+    /* 陀螺仪校准处理 */
+    if (calib_state == CALIB_RUNNING) {
+      /* 累加陀螺仪数据用于计算零偏 */
+      gyro_sum.x_sum += bmi088.gyro.x;
+      gyro_sum.y_sum += bmi088.gyro.y;
+      gyro_sum.z_sum += bmi088.gyro.z;
+      calib_count++;

-    /* 根据解析出来的四元数计算欧拉角 */
-    AHRS_GetEulr(&eulr_to_send, &gimbal_ahrs);
+      /* 达到采样数量后计算平均值并更新零偏 */
+      if (calib_count >= CALIB_SAMPLES) {
+        /* 计算平均值作为零偏 */
+        cali_bmi088.gyro_offset.x = gyro_sum.x_sum / CALIB_SAMPLES;
+        cali_bmi088.gyro_offset.y = gyro_sum.y_sum / CALIB_SAMPLES;
+        cali_bmi088.gyro_offset.z = gyro_sum.z_sum / CALIB_SAMPLES;
+        
+        /* 校准完成，重置为空闲状态以便下次校准 */
+        calib_state = CALIB_IDLE;
+        
+        /* 重新初始化BMI088以应用新的校准数据 */
+        BMI088_Init(&bmi088, &cali_bmi088);
+        AHRS_Init(&gimbal_ahrs, &magn, BMI088_GetUpdateFreq(&bmi088));
+      }
+    }
+
+    /* 只有在非校准状态下才进行正常的姿态解析 */
+    if (calib_state != CALIB_RUNNING) {
+      /* 根据设备接收到的数据进行姿态解析 */
+      AHRS_Update(&gimbal_ahrs, &bmi088.accl, &bmi088.gyro, &magn);
+
+      /* 根据解析出来的四元数计算欧拉角 */
+      AHRS_GetEulr(&eulr_to_send, &gimbal_ahrs);
+    }
+    
    osKernelUnlock();

+    /* 创建修改后的数据副本用于发送到消息队列 */
    gimbal_to_send.eulr = eulr_to_send;
    gimbal_to_send.gyro = bmi088.gyro;

    osMessageQueueReset(task_runtime.msgq.gimbal.imu);
    osMessageQueuePut(task_runtime.msgq.gimbal.imu, &gimbal_to_send, 0, 0);

+    BSP_PWM_SetComp(BSP_PWM_IMU_HEAT_PWM, PID_Calc(&imu_temp_ctrl_pid, 40.0f, bmi088.temp, 0.0f, 0.0f));
+
    /* USER CODE END */
    osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
  }
--- a/User/task/command.c
+++ b/User/task/command.c
@ -1,5 +1,5 @@
 /*
-    command Task
+    cmd Task
    
 */

@ -19,14 +19,14 @@

 /* Private function --------------------------------------------------------- */
 /* Exported functions ------------------------------------------------------- */
-void Task_command(void *argument) {
+void Task_cmd(void *argument) {
  (void)argument; /* 未使用argument，消除警告 */

  
  /* 计算任务运行到指定频率需要等待的tick数 */
-  const uint32_t delay_tick = osKernelGetTickFreq() / COMMAND_FREQ;
+  const uint32_t delay_tick = osKernelGetTickFreq() / CMD_FREQ;

-  osDelay(COMMAND_INIT_DELAY); /* 延时一段时间再开启任务 */
+  osDelay(CMD_INIT_DELAY); /* 延时一段时间再开启任务 */

  uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
  /* USER CODE INIT BEGIN */
--- a/User/task/config.yaml
+++ b/User/task/config.yaml
@ -58,6 +58,6 @@
  description: ''
  freq_control: true
  frequency: 500.0
-  function: Task_command
-  name: command
+  function: Task_cmd
+  name: cmd
  stack: 256
--- a/User/task/ctrl_chassis.c
+++ b/User/task/ctrl_chassis.c
@ -55,10 +55,10 @@ void Task_ctrl_chassis(void *argument) {
  while (1) {
    tick += delay_tick; /* 计算下一个唤醒时刻 */
    /* USER CODE BEGIN */
-    if (osMessageQueueGet(task_runtime.msgq.Chassis_cmd, &chassis_cmd, NULL, 0) != osOK) {
+    if (osMessageQueueGet(task_runtime.msgq.chassis.cmd, &chassis_cmd, NULL, 0) != osOK) {
        // 没有新命令，保持上次命令
    }
-    osMessageQueueGet(task_runtime.msgq.chassis_yaw, &chassis.feedback.yaw, NULL, 0);
+    osMessageQueueGet(task_runtime.msgq.chassis.yaw, &chassis.feedback.yaw, NULL, 0);
 
    Chassis_UpdateFeedback(&chassis);

--- a/User/task/ctrl_gimbal.c
+++ b/User/task/ctrl_gimbal.c
@ -49,7 +49,7 @@ void Task_ctrl_gimbal(void *argument) {
    Gimbal_UpdateFeedback(&gimbal);

    // osMessageQueueReset(task_runtime.msgq.chassis_yaw);
-    osMessageQueuePut(task_runtime.msgq.chassis_yaw, &gimbal.feedback.motor.yaw, 0, 0);
+    osMessageQueuePut(task_runtime.msgq.chassis.yaw, &gimbal.feedback.motor.yaw, 0, 0);
    
    Gimbal_Control(&gimbal, &gimbal_cmd);

--- a/User/task/ctrl_shoot.c
+++ b/User/task/ctrl_shoot.c
@ -40,6 +40,7 @@ void Task_ctrl_shoot(void *argument) {
    /* USER CODE BEGIN */
    osMessageQueueGet(task_runtime.msgq.shoot.shoot_cmd, &shoot_cmd, NULL, 0);
    Shoot_UpdateFeedback(&shoot);
+    //  Shoot_Test(&shoot);
    Shoot_Control(&shoot,&shoot_cmd);
    /* USER CODE END */
    osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
--- a/User/task/init.c
+++ b/User/task/init.c
@ -41,14 +41,14 @@ void Task_Init(void *argument) {
  task_runtime.thread.ctrl_shoot = osThreadNew(Task_ctrl_shoot, NULL, &attr_ctrl_shoot);
  task_runtime.thread.monitor = osThreadNew(Task_monitor, NULL, &attr_monitor);
  task_runtime.thread.ai = osThreadNew(Task_ai, NULL, &attr_ai);
-  task_runtime.thread.command = osThreadNew(Task_command, NULL, &attr_command);
+  task_runtime.thread.cmd = osThreadNew(Task_cmd, NULL, &attr_cmd);

  // 创建消息队列
  /* USER MESSAGE BEGIN */
  task_runtime.msgq.user_msg= osMessageQueueNew(2u, 10, NULL);
-  task_runtime.msgq.chassis_imu = osMessageQueueNew(2u, sizeof(Chassis_IMU_t), NULL);
-  task_runtime.msgq.Chassis_cmd = osMessageQueueNew(2u, sizeof(Chassis_CMD_t), NULL);
-  task_runtime.msgq.chassis_yaw = osMessageQueueNew(2u, sizeof(MOTOR_Feedback_t), NULL);
+  task_runtime.msgq.chassis.imu = osMessageQueueNew(2u, sizeof(Chassis_IMU_t), NULL);
+  task_runtime.msgq.chassis.cmd = osMessageQueueNew(2u, sizeof(Chassis_CMD_t), NULL);
+  task_runtime.msgq.chassis.yaw = osMessageQueueNew(2u, sizeof(MOTOR_Feedback_t), NULL);
  task_runtime.msgq.gimbal.imu= osMessageQueueNew(2u, sizeof(Gimbal_IMU_t), NULL);
  task_runtime.msgq.gimbal.cmd= osMessageQueueNew(2u, sizeof(Gimbal_CMD_t), NULL);
  task_runtime.msgq.shoot.shoot_cmd = osMessageQueueNew(2u, sizeof(Shoot_CMD_t), NULL);
--- a/User/task/rc.c
+++ b/User/task/rc.c
@ -62,9 +62,11 @@ void Task_rc(void *argument) {
      break;
    case DR16_SW_MID:
      cmd_for_chassis.mode = CHASSIS_MODE_RECOVER;
+      // cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
      break;
    case DR16_SW_DOWN:
      cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
+      // cmd_for_chassis.mode = CHASSIS_MODE_JUMP;
      break;
    default:
      cmd_for_chassis.mode = CHASSIS_MODE_RELAX;
@ -76,8 +78,8 @@ void Task_rc(void *argument) {
    cmd_for_chassis.height = dr16.data.ch_res;

    osMessageQueueReset(
-        task_runtime.msgq.Chassis_cmd); // 重置消息队列，防止阻塞
-    osMessageQueuePut(task_runtime.msgq.Chassis_cmd, &cmd_for_chassis, 0,
+        task_runtime.msgq.chassis.cmd); // 重置消息队列，防止阻塞
+    osMessageQueuePut(task_runtime.msgq.chassis.cmd, &cmd_for_chassis, 0,
                      0); // 非阻塞发送底盘控制命令

    switch (dr16.data.sw_l) {
--- a/User/task/user_task.c
+++ b/User/task/user_task.c
@ -49,8 +49,8 @@ const osThreadAttr_t attr_ai = {
    .priority = osPriorityNormal,
    .stack_size = 256 * 4,
 };
-const osThreadAttr_t attr_command = {
-    .name = "command",
+const osThreadAttr_t attr_cmd = {
+    .name = "cmd",
    .priority = osPriorityNormal,
    .stack_size = 256 * 4,
 };
--- a/User/task/user_task.h
+++ b/User/task/user_task.h
@ -21,7 +21,7 @@ extern "C" {
 #define CTRL_SHOOT_FREQ (500.0)
 #define MONITOR_FREQ (500.0)
 #define AI_FREQ (500.0)
-#define COMMAND_FREQ (500.0)
+#define CMD_FREQ (500.0)

 /* 任务初始化延时ms */
 #define TASK_INIT_DELAY (100u)
@ -33,7 +33,7 @@ extern "C" {
 #define CTRL_SHOOT_INIT_DELAY (0)
 #define MONITOR_INIT_DELAY (0)
 #define AI_INIT_DELAY (0)
-#define COMMAND_INIT_DELAY (0)
+#define CMD_INIT_DELAY (0)

 /* Exported defines --------------------------------------------------------- */
 /* Exported macro ----------------------------------------------------------- */
@ -51,16 +51,18 @@ typedef struct {
        osThreadId_t ctrl_shoot;
        osThreadId_t monitor;
        osThreadId_t ai;
-        osThreadId_t command;
+        osThreadId_t cmd;
    } thread;

    /* USER MESSAGE BEGIN */
    struct {
        osMessageQueueId_t user_msg; /* 用户自定义任务消息队列 */

-        osMessageQueueId_t chassis_imu;
-        osMessageQueueId_t Chassis_cmd;
-        osMessageQueueId_t chassis_yaw;
+        struct {
+            osMessageQueueId_t imu;
+            osMessageQueueId_t cmd;
+            osMessageQueueId_t yaw;
+        }chassis;
        struct {
            osMessageQueueId_t imu;
            osMessageQueueId_t cmd;
@ -68,6 +70,11 @@ typedef struct {
        struct {
            osMessageQueueId_t shoot_cmd; /* 发射命令队列 */
        }shoot;
+        struct {
+            osMessageQueueId_t rc;
+            osMessageQueueId_t ref;
+            osMessageQueueId_t ai;
+        }cmd;   
    } msgq;
    /* USER MESSAGE END */

@ -92,7 +99,7 @@ typedef struct {
        UBaseType_t ctrl_shoot;
        UBaseType_t monitor;
        UBaseType_t ai;
-        UBaseType_t command;
+        UBaseType_t cmd;
    } stack_water_mark;

    /* 各任务运行频率 */
@ -105,7 +112,7 @@ typedef struct {
        float ctrl_shoot;
        float monitor;
        float ai;
-        float command;
+        float cmd;
    } freq;

    /* 任务最近运行时间 */
@ -118,7 +125,7 @@ typedef struct {
        float ctrl_shoot;
        float monitor;
        float ai;
-        float command;
+        float cmd;
    } last_up_time;

 } Task_Runtime_t;
@ -136,7 +143,7 @@ extern const osThreadAttr_t attr_ctrl_gimbal;
 extern const osThreadAttr_t attr_ctrl_shoot;
 extern const osThreadAttr_t attr_monitor;
 extern const osThreadAttr_t attr_ai;
-extern const osThreadAttr_t attr_command;
+extern const osThreadAttr_t attr_cmd;

 /* 任务函数声明 */
 void Task_Init(void *argument);
@ -148,7 +155,7 @@ void Task_ctrl_gimbal(void *argument);
 void Task_ctrl_shoot(void *argument);
 void Task_monitor(void *argument);
 void Task_ai(void *argument);
-void Task_command(void *argument);
+void Task_cmd(void *argument);

 #ifdef __cplusplus
 }
--- a/utils/Simulation-master/balance/parallel_legs/get_k_length.m
+++ b/utils/Simulation-master/balance/parallel_legs/get_k_length.m
@ -48,8 +48,8 @@ function K = get_k_length(leg_length)
    B=subs(B,[R,L,LM,l,mw,mp,M,Iw,Ip,IM,g],[R1,L1,LM1,l1,mw1,mp1,M1,Iw1,Ip1,IM1,9.8]);
    B=double(B);
    
-    Q=diag([700 1 600 200 1000 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
-    R=[240 0;0 25];                %T Tp
+    Q=diag(1000 1 200 200 5000 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
+    R=[150 0;0 25];                %T Tp
    
    K=lqr(A,B,Q,R);
  
--- a/utils/Simulation-master/balance/series_legs/get_k_length.m
+++ b/utils/Simulation-master/balance/series_legs/get_k_length.m
@ -17,10 +17,10 @@ function K = get_k_length(leg_length)
    R1=0.072;                         %驱动轮半径
    L1=leg_length/2;                  %摆杆重心到驱动轮轴距离
    LM1=leg_length/2;                 %摆杆重心到其转轴距离
-    l1=-0.03;                          %机体质心距离转轴距离
+    l1=0.01;                          %机体质心距离转轴距离
    mw1=0.60;                         %驱动轮质量
    mp1=1.8;                         %杆质量
-    M1=10.0;                          %机体质量
+    M1=12.0;                          %机体质量
    Iw1=mw1*R1^2;                     %驱动轮转动惯量
    Ip1=mp1*((L1+LM1)^2+0.05^2)/12.0; %摆杆转动惯量
    IM1=M1*(0.3^2+0.12^2)/12.0;       %机体绕质心转动惯量
@ -48,8 +48,8 @@ function K = get_k_length(leg_length)
    B=subs(B,[R,L,LM,l,mw,mp,M,Iw,Ip,IM,g],[R1,L1,LM1,l1,mw1,mp1,M1,Iw1,Ip1,IM1,9.8]);
    B=double(B);
    
-    Q=diag([2000 1 600 200 2000 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
-    R=[240 0;0 50];                %T Tp
+    Q=diag([1500 100 2500 1500 8000 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
+    R=[300 0;0 55];                %T Tp
    
    K=lqr(A,B,Q,R);
  
--- a/未命名.jdebug
+++ b/未命名.jdebug
@ -0,0 +1,361 @@
+/*********************************************************************
+*                 (c) SEGGER Microcontroller GmbH                    *
+*                      The Embedded Experts                          *
+*                         www.segger.com                             *
+**********************************************************************
+
+File          : /Users/lvzucheng/Documents/R/balance_infantry/未命名.jdebug
+Created       : 8 Oct 2025 23:23
+Ozone Version : V3.38d
+*/
+
+/*********************************************************************
+*
+*       OnProjectLoad
+*
+* Function description
+*   Project load routine. Required.
+*
+**********************************************************************
+*/
+void OnProjectLoad (void) {
+  //
+  // Dialog-generated settings
+  //
+  Project.AddPathSubstitute ("/Users/lvzucheng/Documents/R/balance_infantry", "$(ProjectDir)");
+  Project.AddPathSubstitute ("/users/lvzucheng/documents/r/balance_infantry", "$(ProjectDir)");
+  Project.SetDevice ("STM32F407IG");
+  Project.SetHostIF ("USB", "207400620");
+  Project.SetTargetIF ("SWD");
+  Project.SetTIFSpeed ("4 MHz");
+  Project.AddSvdFile ("$(InstallDir)/Config/CPU/Cortex-M4F.svd");
+  //
+  // User settings
+  //
+  File.Open ("$(ProjectDir)/build/Debug/DevC.elf");
+}
+
+/*********************************************************************
+*
+*       OnStartupComplete
+*
+* Function description
+*   Called when program execution has reached/passed
+*   the startup completion point. Optional.
+*
+**********************************************************************
+*/
+//void OnStartupComplete (void) {
+//}
+
+/*********************************************************************
+*
+*      TargetReset
+*
+* Function description
+*   Replaces the default target device reset routine. Optional.
+*
+* Notes
+*   This example demonstrates the usage when
+*   debugging an application in RAM on a Cortex-M target device.
+*
+**********************************************************************
+*/
+//void TargetReset (void) {
+//
+//  unsigned int SP;
+//  unsigned int PC;
+//  unsigned int VectorTableAddr;
+//
+//  VectorTableAddr = Elf.GetBaseAddr();
+//  //
+//  // Set up initial stack pointer
+//  //
+//  if (VectorTableAddr != 0xFFFFFFFF) {
+//    SP = Target.ReadU32(VectorTableAddr);
+//    Target.SetReg("SP", SP);
+//  }
+//  //
+//  // Set up entry point PC
+//  //
+//  PC = Elf.GetEntryPointPC();
+//
+//  if (PC != 0xFFFFFFFF) {
+//    Target.SetReg("PC", PC);
+//  } else if (VectorTableAddr != 0xFFFFFFFF) {
+//    PC = Target.ReadU32(VectorTableAddr + 4);
+//    Target.SetReg("PC", PC);
+//  } else {
+//    Util.Error("Project file error: failed to set entry point PC", 1);
+//  }
+//}
+
+/*********************************************************************
+*
+*       BeforeTargetReset
+*
+* Function description
+*   Event handler routine. Optional.
+*
+**********************************************************************
+*/
+//void BeforeTargetReset (void) {
+//}
+
+/*********************************************************************
+*
+*       AfterTargetReset
+*
+* Function description
+*   Event handler routine. Optional.
+*   The default implementation initializes SP and PC to reset values.
+**
+**********************************************************************
+*/
+void AfterTargetReset (void) {
+  _SetupTarget();
+}
+
+/*********************************************************************
+*
+*       DebugStart
+*
+* Function description
+*   Replaces the default debug session startup routine. Optional.
+*
+**********************************************************************
+*/
+//void DebugStart (void) {
+//}
+
+/*********************************************************************
+*
+*       TargetConnect
+*
+* Function description
+*   Replaces the default target IF connection routine. Optional.
+*
+**********************************************************************
+*/
+//void TargetConnect (void) {
+//}
+
+/*********************************************************************
+*
+*       BeforeTargetConnect
+*
+* Function description
+*   Event handler routine. Optional.
+*
+**********************************************************************
+*/
+//void BeforeTargetConnect (void) {
+//}
+
+/*********************************************************************
+*
+*       AfterTargetConnect
+*
+* Function description
+*   Event handler routine. Optional.
+*
+**********************************************************************
+*/
+//void AfterTargetConnect (void) {
+//}
+
+/*********************************************************************
+*
+*       TargetDownload
+*
+* Function description
+*   Replaces the default program download routine. Optional.
+*
+**********************************************************************
+*/
+//void TargetDownload (void) {
+//}
+
+/*********************************************************************
+*
+*       BeforeTargetDownload
+*
+* Function description
+*   Event handler routine. Optional.
+*
+**********************************************************************
+*/
+//void BeforeTargetDownload (void) {
+//}
+
+/*********************************************************************
+*
+*      AfterTargetDownload
+*
+* Function description
+*   Event handler routine. Optional.
+*   The default implementation initializes SP and PC to reset values.
+*
+**********************************************************************
+*/
+void AfterTargetDownload (void) {
+  _SetupTarget();
+}
+
+/*********************************************************************
+*
+*       BeforeTargetDisconnect
+*
+* Function description
+*   Event handler routine. Optional.
+*
+**********************************************************************
+*/
+//void BeforeTargetDisconnect (void) {
+//}
+
+/*********************************************************************
+*
+*       AfterTargetDisconnect
+*
+* Function description
+*   Event handler routine. Optional.
+*
+**********************************************************************
+*/
+//void AfterTargetDisconnect (void) {
+//}
+
+/*********************************************************************
+*
+*       AfterTargetHalt
+*
+* Function description
+*   Event handler routine. Optional.
+*
+**********************************************************************
+*/
+//void AfterTargetHalt (void) {
+//}
+
+/*********************************************************************
+*
+*       BeforeTargetResume
+*
+* Function description
+*   Event handler routine. Optional.
+*
+**********************************************************************
+*/
+//void BeforeTargetResume (void) {
+//}
+
+/*********************************************************************
+*
+*       OnSnapshotLoad
+*
+* Function description
+*   Called upon loading a snapshot. Optional.
+*
+* Additional information
+*   This function is used to restore the target state in cases
+*   where values cannot simply be written to the target.
+*   Typical use: GPIO clock needs to be enabled, before
+*   GPIO is configured.
+*
+**********************************************************************
+*/
+//void OnSnapshotLoad (void) {
+//}
+
+/*********************************************************************
+*
+*       OnSnapshotSave
+*
+* Function description
+*   Called upon saving a snapshot. Optional.
+*
+* Additional information
+*   This function is usually used to save values of the target
+*   state which can either not be trivially read,
+*   or need to be restored in a specific way or order.
+*   Typically use: Memory Mapped Registers,
+*   such as PLL and GPIO configuration.
+*
+**********************************************************************
+*/
+//void OnSnapshotSave (void) {
+//}
+
+/*********************************************************************
+*
+*       OnError
+*
+* Function description
+*   Called when an error ocurred. Optional.
+*
+**********************************************************************
+*/
+//void OnError (void) {
+//}
+
+/*********************************************************************
+*
+*       AfterProjectLoad
+*
+* Function description
+*   After Project load routine. Optional.
+*
+**********************************************************************
+*/
+//void AfterProjectLoad (void) {
+//}
+
+/*********************************************************************
+*
+*       OnDebugStartBreakSymbolReached
+*
+* Function description
+*   Called when program execution has reached/passed
+*   the symbol to be breaked at during debug start. Optional.
+*
+**********************************************************************
+*/
+//void OnDebugStartBreakSymReached (void) {
+//}
+
+/*********************************************************************
+*
+*       _SetupTarget
+*
+* Function description
+*   Setup the target.
+*   Called by AfterTargetReset() and AfterTargetDownload().
+*
+*   Auto-generated function. May be overridden by Ozone.
+*
+**********************************************************************
+*/
+void _SetupTarget(void) {
+  unsigned int SP;
+  unsigned int PC;
+  unsigned int VectorTableAddr;
+
+  VectorTableAddr = Elf.GetBaseAddr();
+  //
+  // Set up initial stack pointer
+  //
+  SP = Target.ReadU32(VectorTableAddr);
+  if (SP != 0xFFFFFFFF) {
+    Target.SetReg("SP", SP);
+  }
+  //
+  // Set up entry point PC
+  //
+  PC = Elf.GetEntryPointPC();
+  if (PC != 0xFFFFFFFF) {
+    Target.SetReg("PC", PC);
+  } else {
+    Util.Error("Project script error: failed to set up entry point PC", 1);
+  }
+}
Author	SHA1	Message	Date
Robofish	dd3501693b	修can发送	2025-10-13 01:29:09 +08:00
Robofish	85d16a1bc0	添加cmd	2025-10-10 22:02:47 +08:00
Robofish	22654da273	改名字	2025-10-10 20:16:28 +08:00
Robofish	db7f569b5c	改轮腿	2025-10-10 17:31:19 +08:00
Robofish	596de8c320	修ctrl	2025-10-10 16:59:25 +08:00
Robofish	64216877e5	好qr	2025-10-10 00:21:11 +08:00
Robofish	dcc3b55df8	改代码	2025-10-09 17:27:14 +08:00
Robofish	17c6a92fd0	稀烂小陀螺	2025-10-09 00:03:30 +08:00
Robofish	8f0f615fb1	改一下速度	2025-10-07 23:20:04 +08:00
Robofish	ec8dd40ced	添加速度规划	2025-10-07 21:48:38 +08:00