add上台阶

User/module/balance_chassis.c

@@ -31,6 +31,7 @@ static void Chassis_UpdateChassisState(Chassis_t *c);
 static void Chassis_ResetControllers(Chassis_t *c);
 static void Chassis_SelectYawZeroPoint(Chassis_t *c);
 static void Chassis_JumpControl(Chassis_t *c, const Chassis_CMD_t *c_cmd, float *target_L0, float *extra_force);
+static void Chassis_ClimbControl(Chassis_t *c, const Chassis_CMD_t *c_cmd);
 
 /* Private functions -------------------------------------------------------- */
 
@@ -138,6 +139,11 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) {
   if (c == NULL) return CHASSIS_ERR_NULL;
   if (mode == c->mode) return CHASSIS_OK;
 
+  /* 上台阶已完成，忽略重复的CLIMB_STEP请求，需先切别的模式再回来 */
+  if (mode == CHASSIS_MODE_CLIMB_STEP && c->climb.completed) {
+    return CHASSIS_OK;
+  }
+
   /* RECOVER 进行中时，忽略 BALANCE 指令，等自起完成后自动切换 */
   if (c->mode == CHASSIS_MODE_RECOVER &&
       mode == CHASSIS_MODE_WHELL_LEG_BALANCE) {
@@ -180,6 +186,19 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) {
     c->rotor_state.exiting = false;
   }
 
+  /* 进入上台阶模式：重置状态机 */
+  if (mode == CHASSIS_MODE_CLIMB_STEP) {
+    c->climb.state = CLIMB_EXTEND;
+    c->climb.completed = false;
+    c->climb.state_start_time = (uint32_t)(BSP_TIME_Get_us() / 1000);
+  }
+
+  /* 离开上台阶模式时重置completed标志 */
+  if (c->mode == CHASSIS_MODE_CLIMB_STEP && mode != CHASSIS_MODE_CLIMB_STEP) {
+    c->climb.completed = false;
+    c->climb.state = CLIMB_IDLE;
+  }
+
   c->mode = mode;
   return CHASSIS_OK;
 }
@@ -419,6 +438,100 @@ static void Chassis_JumpControl(Chassis_t *c, const Chassis_CMD_t *c_cmd, float
   }
 }
 
+/**
+ * @brief 上台阶控制
+ * @param c 底盘结构体指针
+ * @param c_cmd 控制指令
+ *
+ * @note 流程：
+ *   EXTEND(伸腿前进，削弱前摆力) -> RETRACT(腿后摆超阈值，快速收腿) -> SETTLE(稳定) -> 自动退出到BALANCE
+ *   单次执行：完成后即使CMD继续发CLIMB_STEP也不重复，需切换到其他模式后再回来
+ */
+static void Chassis_ClimbControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
+  uint32_t current_time = (uint32_t)(BSP_TIME_Get_us() / 1000);
+  uint32_t elapsed_time = current_time - c->climb.state_start_time;
+
+  /* 已完成一次上台阶，自动切回BALANCE */
+  if (c->climb.completed) {
+    c->mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
+    Chassis_ResetControllers(c);
+    return;
+  }
+
+  /* 上台阶状态机中始终运行LQR+VMC保持平衡 */
+  Chassis_CMD_t climb_cmd = *c_cmd;
+
+  switch (c->climb.state) {
+    case CLIMB_IDLE:
+      /* 不应到达此状态，进入CLIMB_STEP时会设为EXTEND */
+      c->climb.state = CLIMB_EXTEND;
+      c->climb.state_start_time = current_time;
+      break;
+
+    case CLIMB_EXTEND: {
+      /* 伸腿前进：腿长最大，给定前进速度，削弱摆力矩让腿自由后摆 */
+      climb_cmd.move_vec.vx = c->param->climb.forward_speed / 2.0f;
+      climb_cmd.height = 1.0f;  /* 最大腿长 */
+
+      /* 运行LQR和VMC */
+      Chassis_LQRControl(c, &climb_cmd);
+
+      /* 削弱LQR的摆力矩Tp，让腿撞到台阶后能自由后摆 */
+      c->lqr[0].control_output.Tp *= c->param->climb.tp_scale;
+      c->lqr[1].control_output.Tp *= c->param->climb.tp_scale;
+
+      Chassis_VMCControl(c, &climb_cmd);
+      Chassis_Output(c);
+
+      /* 检查腿后摆角度：任一腿theta超过阈值则收腿 */
+      float avg_theta = (c->vmc_[0].leg.theta + c->vmc_[1].leg.theta) / 2.0f;
+      if (fabsf(avg_theta) > c->param->climb.theta_retract_threshold) {
+        c->climb.state = CLIMB_RETRACT;
+        c->climb.state_start_time = current_time;
+      }
+
+      /* 超时保护：10秒未触发收腿则放弃 */
+      if (elapsed_time > 10000) {
+        c->climb.completed = true;
+      }
+    } break;
+
+    case CLIMB_RETRACT: {
+      /* 快速收腿，继续前进 */
+      climb_cmd.move_vec.vx = c->param->climb.forward_speed / 2.0f;
+      climb_cmd.height = -1.0f;  /* 最小腿长 */
+
+      Chassis_LQRControl(c, &climb_cmd);
+      Chassis_VMCControl(c, &climb_cmd);
+      Chassis_Output(c);
+
+      /* 腿收到位或超时，进入稳定阶段 */
+      bool legs_retracted = (c->vmc_[0].leg.L0 < c->param->leg.min_length + 0.02f) &&
+                            (c->vmc_[1].leg.L0 < c->param->leg.min_length + 0.02f);
+      if (legs_retracted || elapsed_time > 1000) {
+        c->climb.state = CLIMB_SETTLE;
+        c->climb.state_start_time = current_time;
+      }
+    } break;
+
+    case CLIMB_SETTLE:
+      /* 稳定阶段：恢复正常控制，等待稳定 */
+      climb_cmd.height = 0.0f;
+      Chassis_LQRControl(c, &climb_cmd);
+      Chassis_VMCControl(c, &climb_cmd);
+      Chassis_Output(c);
+
+      if (elapsed_time > c->param->climb.settle_time_ms) {
+        c->climb.completed = true;
+      }
+      break;
+
+    default:
+      c->climb.completed = true;
+      break;
+  }
+}
+
 
 /**
  * @brief 初始化底盘
@@ -715,6 +828,10 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
       Chassis_Output(c);
       break;
 
+    case CHASSIS_MODE_CLIMB_STEP:
+      Chassis_ClimbControl(c, c_cmd);
+      break;
+
     default:
       return CHASSIS_ERR_MODE;
   }

User/module/balance_chassis.h

@@ -42,7 +42,8 @@ typedef enum {
   CHASSIS_MODE_RECOVER,          /* 复位模式 */
   // CHASSIS_MODE_CALIBRATE,        /* 校准模式 */
   CHASSIS_MODE_WHELL_LEG_BALANCE, /* 轮子+腿平衡模式，底盘自我平衡，支持跳跃 */
-  CHASSIS_MODE_BALANCE_ROTOR /*小陀螺*/
+  CHASSIS_MODE_BALANCE_ROTOR, /*小陀螺*/
+  CHASSIS_MODE_CLIMB_STEP  /* 上台阶模式 */
 } Chassis_Mode_t;
 
 /* 跳跃状态枚举 */
@@ -62,11 +63,20 @@ typedef enum {
   RECOVER_COMPLETE   /* 完成：切换到平衡模式 */
 } Recover_State_t;
 
+/* 上台阶状态枚举 */
+typedef enum {
+  CLIMB_IDLE,         /* 待机，未触发 */
+  CLIMB_EXTEND,       /* 伸腿前进：腿长最大，削弱前摆力，等待撞到台阶 */
+  CLIMB_RETRACT,      /* 收腿：腿后摆超过阈值后快速收腿 */
+  CLIMB_SETTLE        /* 稳定：收腿完成，短暂稳定后退出 */
+} Climb_State_t;
+
 typedef struct {
   Chassis_Mode_t mode;           /* 底盘模式 */
   MoveVector_t move_vec;         /* 底盘运动向量 */
   float height;               /* 目标高度 */
   bool jump_trigger;             /* 跳跃触发标志 */
+  bool climb_trigger;            /* 上台阶触发标志 */
 } Chassis_CMD_t;
 
 typedef struct {
@@ -156,6 +166,14 @@ typedef struct {
     float align_threshold;         /* 退出对齐阈值 (rad)，yaw误差小于此值时回到BALANCE模式 */
   } rotor_ctrl;
 
+  /* 上台阶参数 */
+  struct {
+    float forward_speed;           /* 前进速度 (m/s) */
+    float theta_retract_threshold; /* 腿后摆收腿阈值 (rad)，约30° = 0.52 */
+    float tp_scale;                /* 摆力矩缩放系数，<1削弱前摆力 */
+    uint32_t settle_time_ms;       /* 收腿后稳定时间 (ms) */
+  } climb;
+
   /* LQR 目标状态偏移 */
   struct {
     float theta;
@@ -231,6 +249,13 @@ typedef struct {
     Recover_State_t state[2]; /* 左右腿自起状态 */
   } recover;
 
+  /* 上台阶控制相关 */
+  struct {
+    Climb_State_t state;         /* 上台阶状态 */
+    bool completed;              /* 本次已完成标志，防止重复触发 */
+    uint32_t state_start_time;   /* 当前状态开始时间 (ms) */
+  } climb;
+
   /* 电机离线检测 */
   struct {
     bool any_offline;        /* 是否有电机离线 */

User/module/config.c

@@ -448,6 +448,13 @@ Config_RobotParam_t robot_config = {
         .retract_force = -20.0f,       /* 收腿前馈力减小 */
     },
 
+    .climb = {
+        .forward_speed = 1.0f,         /* 上台阶前进速度 (m/s) */
+        .theta_retract_threshold = 0.52f, /* 腿后摆收腿阈值 (rad)，约30° */
+        .tp_scale = 0.1f,             /* 摆力矩缩放：削弱到10%，让腿自由后摆 */
+        .settle_time_ms = 500,        /* 收腿后稳定时间 (ms) */
+    },
+
     .lqr_offset = {
         .theta = 0.0f,
         .x = 0.0f,
@@ -485,7 +492,7 @@ Config_RobotParam_t robot_config = {
 #if CMD_ENABLE_MODULE_BALANCE_CHASSIS
             .balance_sw_up   = CHASSIS_MODE_RELAX,
             .balance_sw_mid  = CHASSIS_MODE_WHELL_LEG_BALANCE,
-            .balance_sw_down = CHASSIS_MODE_BALANCE_ROTOR,
+            .balance_sw_down = CHASSIS_MODE_CLIMB_STEP,
 #endif
         },
     },