add leg test

User/component/vmc.c

@@ -122,7 +122,11 @@ int8_t VMC_ForwardSolve(VMC_t *vmc, float phi1, float phi4, float body_pitch, fl
     VMC_ANGLE_NORMALIZE(leg->phi0);
     
     // 计算角速度和长度变化率
-    leg->d_phi0 = VMC_ComputeNumericDerivative(leg->phi0, leg->last_phi0, vmc->dt);
+    float dphi0 = leg->phi0 - leg->last_phi0;
+    /* phi0 在 ±π 跳变时做循环差值处理 */
+    if (dphi0 > VMC_PI)  dphi0 -= VMC_2PI;
+    if (dphi0 < -VMC_PI) dphi0 += VMC_2PI;
+    leg->d_phi0 = (vmc->dt > 0) ? (dphi0 / vmc->dt) : 0.0f;
     leg->d_alpha = -leg->d_phi0;
     leg->d_theta = body_pitch_rate + leg->d_phi0;
     leg->d_L0 = VMC_ComputeNumericDerivative(leg->L0, leg->last_L0, vmc->dt);

User/module/balance_chassis.c

@@ -34,6 +34,7 @@ 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);
 static void Chassis_RecoverControl(Chassis_t *c);
+static void Chassis_LegTestControl(Chassis_t *c, const Chassis_CMD_t *c_cmd);
 
 /* Private functions -------------------------------------------------------- */
 
@@ -85,6 +86,8 @@ static void Chassis_ResetControllers(Chassis_t *c) {
   PID_Reset(&c->pid.roll_force);
   PID_Reset(&c->pid.tp[0]);
   PID_Reset(&c->pid.tp[1]);
+  PID_Reset(&c->pid.leg_test_theta[0]);
+  PID_Reset(&c->pid.leg_test_theta[1]);
 
   /* 重置LQR控制器 */
   LQR_Reset(&c->lqr[0]);
@@ -209,6 +212,11 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) {
     c->climb.state_start_time = 0;
   }
 
+  /* 进入腿部测试模式：重置初始化标志，进入后锁定当前摆角 */
+  if (mode == CHASSIS_MODE_LEG_TEST) {
+    c->leg_test.initialized = false;
+  }
+
   c->mode = mode;
   return CHASSIS_OK;
 }
@@ -665,6 +673,88 @@ static void Chassis_RecoverControl(Chassis_t *c) {
 }
 
 
+/**
+ * @brief 腿部测试控制
+ * @param c 底盘结构体指针
+ * @param c_cmd 控制指令
+ * @note 进入时锁定当前摆角，vx/vy 增减左/右腿目标摆角
+ *       PID控制摆角，恒力支撑腿长，轮子关闭
+ */
+static void Chassis_LegTestControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
+  /* 首次进入：锁定当前摆角 */
+  if (!c->leg_test.initialized) {
+    c->leg_test.target_theta[0] = c->vmc_[0].leg.theta;
+    c->leg_test.target_theta[1] = c->vmc_[1].leg.theta;
+    c->leg_test.initialized = true;
+  }
+
+  float sens = c->param->leg_test.theta_sensitivity;
+  float deadzone = 0.05f;  /* 摇杆死区 */
+
+  /* vx 控制左腿：有输入→累加目标角度（正/反转），松手→锁住当前实际角度 */
+  if (fabsf(c_cmd->move_vec.vx) > deadzone) {
+    c->leg_test.target_theta[0] += c_cmd->move_vec.vx * sens * c->dt;
+  } else {
+    c->leg_test.target_theta[0] = c->vmc_[0].leg.theta;
+  }
+
+  /* vy 控制右腿：同理 */
+  if (fabsf(c_cmd->move_vec.vy) > deadzone) {
+    c->leg_test.target_theta[1] += c_cmd->move_vec.vy * sens * c->dt;
+  } else {
+    c->leg_test.target_theta[1] = c->vmc_[1].leg.theta;
+  }
+
+  /* 目标角度归一化到 [-π, π]，防止无限增长 */
+  while (c->leg_test.target_theta[0] > (float)M_PI)  c->leg_test.target_theta[0] -= (float)M_2PI;
+  while (c->leg_test.target_theta[0] < -(float)M_PI) c->leg_test.target_theta[0] += (float)M_2PI;
+  while (c->leg_test.target_theta[1] > (float)M_PI)  c->leg_test.target_theta[1] -= (float)M_2PI;
+  while (c->leg_test.target_theta[1] < -(float)M_PI) c->leg_test.target_theta[1] += (float)M_2PI;
+
+  /* 轮子关闭：只清零输出，由 Chassis_Output 统一发送，避免重复 CAN 帧 */
+
+  /* 恒定支撑力 + PID摆角控制 → VMC逆解算 → 关节力矩 */
+  float support_force = c->param->leg_test.support_force;
+
+  for (int i = 0; i < 2; i++) {
+    /* 手动计算循环误差，避免 theta 在 ±π 跳变时 PID 炸 */
+    float error = c->leg_test.target_theta[i] - c->vmc_[i].leg.theta;
+    while (error > (float)M_PI)  error -= (float)M_2PI;
+    while (error < -(float)M_PI) error += (float)M_2PI;
+
+    /* 用误差直接算 P 和 D 输出，绕过 PID 内部的 set/fdb 差值 */
+    float kp = c->pid.leg_test_theta[i].param->k * c->pid.leg_test_theta[i].param->p;
+    float kd = c->pid.leg_test_theta[i].param->k * c->pid.leg_test_theta[i].param->d;
+    float out_limit = c->pid.leg_test_theta[i].param->out_limit;
+    float Tp = kp * error - kd * c->vmc_[i].leg.d_theta;
+    Tp = LIMIT(Tp, -out_limit, out_limit);
+
+    if (VMC_InverseSolve(&c->vmc_[i], support_force, Tp) == 0) {
+      if (i == 0) {
+        VMC_GetJointTorques(&c->vmc_[i], &c->output.joint[0], &c->output.joint[1]);
+      } else {
+        VMC_GetJointTorques(&c->vmc_[i], &c->output.joint[3], &c->output.joint[2]);
+      }
+    } else {
+      c->output.joint[i * 2] = 0.0f;
+      c->output.joint[i * 2 + 1] = 0.0f;
+    }
+  }
+
+  /* 轮子输出清零 */
+  c->output.wheel[0] = 0.0f;
+  c->output.wheel[1] = 0.0f;
+
+  /* 安全限制 */
+  for (int i = 0; i < 4; i++) {
+    c->output.joint[i] = LIMIT(c->output.joint[i], -30.0f, 30.0f);
+  }
+
+  /* 发送关节指令 */
+  Chassis_Output(c);
+}
+
+
 /**
  * @brief 初始化底盘
  * @param c 底盘结构体指针
@@ -707,12 +797,14 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) {
   /* 初始化PID控制器 */
   PID_Init(&c->pid.leg_length[0], KPID_MODE_CALC_D, target_freq, &param->leg_length);
   PID_Init(&c->pid.leg_length[1], KPID_MODE_CALC_D, target_freq, &param->leg_length);
-  PID_Init(&c->pid.yaw, KPID_MODE_CALC_D, target_freq, &param->yaw);
+  PID_Init(&c->pid.yaw, KPID_MODE_SET_D, target_freq, &param->yaw);
   PID_Init(&c->pid.rotor, KPID_MODE_CALC_D, target_freq, &param->rotor);
   PID_Init(&c->pid.roll, KPID_MODE_CALC_D, target_freq, &param->roll);
   PID_Init(&c->pid.roll_force, KPID_MODE_CALC_D, target_freq, &param->roll_force);
   PID_Init(&c->pid.tp[0], KPID_MODE_CALC_D, target_freq, &param->tp);
   PID_Init(&c->pid.tp[1], KPID_MODE_CALC_D, target_freq, &param->tp);
+  PID_Init(&c->pid.leg_test_theta[0], KPID_MODE_CALC_D, target_freq, &param->leg_test_theta);
+  PID_Init(&c->pid.leg_test_theta[1], KPID_MODE_CALC_D, target_freq, &param->leg_test_theta);
 
   /* 初始化LQR控制器 */
   LQR_Init(&c->lqr[0], &param->lqr_gains);
@@ -814,6 +906,11 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) {
   c->recover.state[1] = RECOVER_STRETCH;
   c->recover.state_start_time = 0;
 
+  /* 初始化腿部测试状态 */
+  c->leg_test.initialized = false;
+  c->leg_test.target_theta[0] = 0.0f;
+  c->leg_test.target_theta[1] = 0.0f;
+
   return CHASSIS_OK;
 }
 
@@ -980,6 +1077,10 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
       Chassis_ClimbControl(c, c_cmd);
       break;
 
+    case CHASSIS_MODE_LEG_TEST:
+      Chassis_LegTestControl(c, c_cmd);
+      break;
+
     default:
       return CHASSIS_ERR_MODE;
   }
@@ -1018,7 +1119,7 @@ void Chassis_Output(Chassis_t *c) {
   //   c->output.wheel[i] = LowPassFilter2p_Apply(&c->filter.wheel_out[i], c->output.wheel[i]);
   // }
 	//cap-add//supercap-add
-	PowerLimit_Output(c->power_limit, c->output.wheel, 2 );
+	// PowerLimit_Output(c->power_limit, c->output.wheel, 2 );
 	//PowerLimit_ChassicOutput()
   MOTOR_LK_SetOutput(&c->param->wheel_param[0], c->output.wheel[0]);
   MOTOR_LK_SetOutput(&c->param->wheel_param[1], c->output.wheel[1]);
@@ -1123,7 +1224,9 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
           /* 对齐完成，切换到平衡模式 */
           c->rotor_state.exiting = false;
           c->mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
-          Chassis_ResetControllers(c);
+          /* 只重置 yaw/rotor PID，保留 chassis_state 避免 LQR 突变 */
+          PID_Reset(&c->pid.yaw);
+          PID_Reset(&c->pid.rotor);
         }
       }
     } else {
@@ -1141,7 +1244,8 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
     /* 退出过渡减速完成后，用yaw PID对齐零点 */
     if (c->rotor_state.exiting && c->rotor_state.current_spin_speed == 0.0f) {
       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);
+                                          c->feedback.yaw.rotor_abs_angle,
+                                          c->feedback.imu.gyro.z, c->dt);
     } else {
       float current_yaw_omega = -c->feedback.imu.gyro.z;  /* 取反匹配yaw_force→轮子旋转方向 */
       c->yaw_control.yaw_force = PID_Calc(&c->pid.rotor, c->rotor_state.current_spin_speed,
@@ -1158,16 +1262,30 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
     float error_to_target1 = CircleError(base_target_1, c->yaw_control.current_yaw, M_2PI);
     float error_to_target2 = CircleError(base_target_2, c->yaw_control.current_yaw, M_2PI);
 
-    if (fabsf(error_to_target1) <= fabsf(error_to_target2)) {
+    /* 迟滞切换：只有当另一个零点明显更近（差值>0.3rad≈17°）时才切换，
-      c->yaw_control.target_yaw = base_target_1;
+     * 避免在两个零点正中间反复跳变 */
-      c->yaw_control.is_reversed = false;
+    float hysteresis = 0.3f;
+    if (c->yaw_control.is_reversed) {
+      /* 当前跟踪 target_2，只有 target_1 明显更近才切 */
+      if (fabsf(error_to_target1) < fabsf(error_to_target2) - hysteresis) {
+        c->yaw_control.target_yaw = base_target_1;
+        c->yaw_control.is_reversed = false;
+      } else {
+        c->yaw_control.target_yaw = base_target_2;
+      }
     } else {
-      c->yaw_control.target_yaw = base_target_2;
+      /* 当前跟踪 target_1，只有 target_2 明显更近才切 */
-      c->yaw_control.is_reversed = true;
+      if (fabsf(error_to_target2) < fabsf(error_to_target1) - hysteresis) {
+        c->yaw_control.target_yaw = base_target_2;
+        c->yaw_control.is_reversed = true;
+      } else {
+        c->yaw_control.target_yaw = base_target_1;
+      }
     }
 
     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);
+                                        c->feedback.yaw.rotor_abs_angle,
+                                        c->feedback.imu.gyro.z, c->dt);
   }
 
   /* ==================== 左腿LQR控制 ==================== */

User/module/balance_chassis.h

@@ -43,7 +43,8 @@ typedef enum {
   // CHASSIS_MODE_CALIBRATE,        /* 校准模式 */
   CHASSIS_MODE_WHELL_LEG_BALANCE, /* 轮子+腿平衡模式，底盘自我平衡，支持跳跃 */
   CHASSIS_MODE_BALANCE_ROTOR, /*小陀螺*/
-  CHASSIS_MODE_CLIMB_STEP  /* 上台阶模式 */
+  CHASSIS_MODE_CLIMB_STEP,  /* 上台阶模式 */
+  CHASSIS_MODE_LEG_TEST     /* 腿部测试模式：PID锁摆角+恒力支撑，摇杆控制摆角 */
 } Chassis_Mode_t;
 
 /* 跳跃状态枚举 */
@@ -174,6 +175,14 @@ typedef struct {
     uint32_t settle_time_ms;       /* 收腿后稳定时间 (ms) */
   } climb;
 
+  /* 腿部测试参数 */
+  struct {
+    float support_force;           /* 恒定支撑力 (N) */
+    float theta_sensitivity;       /* 摇杆→摆角灵敏度 (rad/s) */
+  } leg_test;
+
+  KPID_Params_t leg_test_theta;    /* 腿部测试摆角PID（独立于tp） */
+
   /* 倒地自起参数 */
   struct {
     float stretch_velocity;        /* 伸腿角速度 (rad/s) */
@@ -267,6 +276,12 @@ typedef struct {
     uint32_t state_start_time;   /* 当前状态开始时间 (ms) */
   } climb;
 
+  /* 腿部测试相关 */
+  struct {
+    float target_theta[2];       /* 左右腿目标摆角 (rad)，进入时锁定当前值 */
+    bool initialized;            /* 是否已锁定初始摆角 */
+  } leg_test;
+
   /* 电机离线检测 */
   struct {
     bool any_offline;        /* 是否有电机离线 */
@@ -286,6 +301,7 @@ typedef struct {
     KPID_t tp[2];
     KPID_t leg_length[2]; /* 两条腿的腿长PID */
     KPID_t leg_theta[2]; /* 两条腿的摆角PID */
+    KPID_t leg_test_theta[2]; /* 腿部测试摆角PID */
   } pid;
 
   /* 滤波器 */

User/module/config.c

@@ -491,6 +491,20 @@ Config_RobotParam_t robot_config = {
         .phi_tolerance = 0.5f,         /* 体姿接近站立容差 (rad)，约28° */
         .kd = 3.0f,                    /* MIT速度模式阻尼 */
     },
+    .leg_test = {
+        .support_force = 30.0f,        /* 恒定支撑力 (N) */
+        .theta_sensitivity = 1.0f,     /* 摇杆→摆角灵敏度 (rad/s) */
+    },
+    .leg_test_theta = {
+        .k = 20.0f,
+        .p = 5.0f,
+        .i = 0.0f,
+        .d = 1.0f,
+        .i_limit = 0.0f,
+        .out_limit = 30.0f,            /* 摆角力矩限幅 (Nm) */
+        .d_cutoff_freq = 30.0f,
+        .range = M_2PI,                /* 启用循环误差处理 */
+    },
     .lqr_offset = {
         .theta = 0.0f,
         .x = 0.0f,
@@ -522,15 +536,14 @@ Config_RobotParam_t robot_config = {
         .rc_mode_map = {
 #if CMD_ENABLE_MODULE_GIMBAL
             .gimbal_sw_up   = GIMBAL_MODE_RELAX,
-            // .gimbal_sw_mid  = GIMBAL_MODE_ABSOLUTE,
+            .gimbal_sw_mid  = GIMBAL_MODE_ABSOLUTE,
-            .gimbal_sw_mid = GIMBAL_MODE_RECOVER,
+            // .gimbal_sw_mid = GIMBAL_MODE_RECOVER,
             .gimbal_sw_down = GIMBAL_MODE_RELATIVE,
 #endif
 #if CMD_ENABLE_MODULE_BALANCE_CHASSIS
             .balance_sw_up   = CHASSIS_MODE_RELAX,
-            // .balance_sw_mid  = CHASSIS_MODE_WHELL_LEG_BALANCE,
+            .balance_sw_mid  = CHASSIS_MODE_WHELL_LEG_BALANCE,
-            .balance_sw_mid = CHASSIS_MODE_RELAX, // 先关闭轮腿平衡，避免不熟悉操作导致摔倒
+            .balance_sw_down = CHASSIS_MODE_BALANCE_ROTOR,
-            .balance_sw_down = CHASSIS_MODE_WHELL_LEG_BALANCE,
 #endif
         },
     },

User/module/gimbal.c

@@ -63,6 +63,17 @@ static int8_t Gimbal_SetMode(Gimbal_t *g, Gimbal_Mode_t mode) {
   }
   g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw;
 
+  /* 进入 RECOVER 时初始化双零点选择 */
+  if (mode == GIMBAL_MODE_RECOVER) {
+    float yaw_enc = g->feedback.motor.yaw.rotor_abs_angle;
+    float zero0 = g->param->mech_zero.yaw;
+    float zero1 = zero0 + M_PI;
+    if (zero1 >= M_2PI) zero1 -= M_2PI;
+    float err0 = fabsf(CircleError(zero0, yaw_enc, M_2PI));
+    float err1 = fabsf(CircleError(zero1, yaw_enc, M_2PI));
+    g->recover_yaw_reversed = (err1 < err0);
+  }
+
   g->mode = mode;
   return 0;
 }
@@ -220,14 +231,27 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd, Gimbal_AI_t *g_ai) {
     g->setpoint.eulr.yaw = g_cmd->setpoint_yaw;
     g->setpoint.eulr.pit = g_cmd->setpoint_pit;
   } else if (g_cmd->mode == GIMBAL_MODE_RECOVER) {
-    /* 双零点选近：mech_zero 和 mech_zero+π，选离当前编码器角度最近的 */
+    /* 双零点选近：mech_zero 和 mech_zero+π，带迟滞避免临界点跳变 */
     float yaw_enc = g->feedback.motor.yaw.rotor_abs_angle;
     float zero0 = g->param->mech_zero.yaw;
     float zero1 = zero0 + M_PI;
     if (zero1 >= M_2PI) zero1 -= M_2PI;
     float err0 = fabsf(CircleError(zero0, yaw_enc, M_2PI));
     float err1 = fabsf(CircleError(zero1, yaw_enc, M_2PI));
-    g->setpoint.eulr.yaw = (err0 <= err1) ? zero0 : zero1;
+
+    float hysteresis = 0.3f;
+    if (g->recover_yaw_reversed) {
+      /* 当前跟踪 zero1，只有 zero0 明显更近才切 */
+      if (err0 < err1 - hysteresis) {
+        g->recover_yaw_reversed = false;
+      }
+    } else {
+      /* 当前跟踪 zero0，只有 zero1 明显更近才切 */
+      if (err1 < err0 - hysteresis) {
+        g->recover_yaw_reversed = true;
+      }
+    }
+    g->setpoint.eulr.yaw = g->recover_yaw_reversed ? zero1 : zero0;
     g->setpoint.eulr.pit = 0.5f * (g->limit.pit.max + g->limit.pit.min);
   }
 

User/module/gimbal.h

@@ -171,6 +171,8 @@ typedef struct {
   Gimbal_Output_t out;        /* 云台输出 */
   Gimbal_Feedback_t feedback; /* 反馈 */
 
+  bool recover_yaw_reversed;  /* RECOVER模式下跟踪的是第二个零点(+π) */
+
 } Gimbal_t;
 
 /* Exported functions prototypes -------------------------------------------- */