拿出vmc

User/module/balance_chassis.c

@@ -411,70 +411,6 @@ static void Chassis_JumpControl(Chassis_t *c, const Chassis_CMD_t *c_cmd, float
   }
 }
 
-/**
- * @brief 倒地自起控制
- * @note 自起流程（左右腿独立状态机）：
- *   STRETCH(伸腿) -> BACKLEG(后甩腿) -> COMPLETE(完成)
- *   两腿均 COMPLETE 后自动切换到 WHELL_LEG_BALANCE 模式
- *
- * @param c 底盘结构体指针
- */
-static void Chassis_RecoverControl(Chassis_t *c) {
-  float theta_l = c->vmc_[0].leg.theta;
-  float theta_r = c->vmc_[1].leg.theta;
-
-  /* ===== 左腿状态机 ===== */
-  switch (c->recover.state[0]) {
-    case RECOVER_FLIP:
-      /* 翻身阶段：theta朝上（|theta| > π/2），收腿慢慢翻转 */
-      /* 等待 theta 转到腿朝下（|theta| < π/2） */
-      if (fabsf(theta_l) < (float)M_PI_2) {
-        c->recover.state[0] = RECOVER_STRETCH;
-      }
-      break;
-
-    case RECOVER_STRETCH:
-      /* 伸腿：等待腿伸到位 */
-      if (c->vmc_[0].leg.L0 > 0.28f) {
-        c->recover.state[0] = RECOVER_BACKLEG;
-      }
-      break;
-
-    case RECOVER_BACKLEG:
-      /* 后甩腿：等待 phi0 趋近目标角度 */
-      if (fabsf(c->vmc_[0].leg.phi0 - 0.5f) < 0.1f) {
-        c->recover.state[0] = RECOVER_COMPLETE;
-      }
-      break;
-
-    case RECOVER_COMPLETE:
-      break;
-  }
-
-  /* ===== 右腿状态机 ===== */
-  switch (c->recover.state[1]) {
-    case RECOVER_FLIP:
-      if (fabsf(theta_r) < (float)M_PI_2) {
-        c->recover.state[1] = RECOVER_STRETCH;
-      }
-      break;
-
-    case RECOVER_STRETCH:
-      if (c->vmc_[1].leg.L0 > 0.28f) {
-        c->recover.state[1] = RECOVER_BACKLEG;
-      }
-      break;
-
-    case RECOVER_BACKLEG:
-      if (fabsf(c->vmc_[1].leg.phi0 - 0.5f) < 0.1f) {
-        c->recover.state[1] = RECOVER_COMPLETE;
-      }
-      break;
-
-    case RECOVER_COMPLETE:
-      break;
-  }
-}
 
 /**
  * @brief 初始化底盘
@@ -716,81 +652,23 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
     case CHASSIS_MODE_RELAX:
       Chassis_MotorRelax(c);
       Chassis_LQRControl(c, c_cmd);
+      Chassis_VMCControl(c, c_cmd);
       break;
 
-    case CHASSIS_MODE_RECOVER: {
+    // case CHASSIS_MODE_RECOVER: 
-      /* 运行自起状态机 */
+    //   Chassis_RecoverControl(c);
-      Chassis_RecoverControl(c);
+    //   break;
-
-      /* 根据状态决定输出力 */
-      float fn_left, fn_right;
-      float spring_left = Chassis_CalcSpringForce(c->vmc_[0].leg.L0);
-      float spring_right = Chassis_CalcSpringForce(c->vmc_[1].leg.L0);
-      if (isnan(spring_left)) spring_left = 0.0f;
-      if (isnan(spring_right)) spring_right = 0.0f;
-
-      /* 左腿 */
-      if (c->recover.state[0] == RECOVER_FLIP) {
-        fn_left = 30.0f - spring_left;   /* 收腿，借重力慢慢翻转 */
-      } else if (c->recover.state[0] == RECOVER_STRETCH) {
-        fn_left = -80.0f - spring_left;  /* 大力伸腿 */
-      } else if (c->recover.state[0] == RECOVER_BACKLEG) {
-        fn_left = -40.0f - spring_left;  /* 维持伸腿 */
-      } else {
-        fn_left = -20.0f - spring_left;
-      }
-
-      /* 右腿 */
-      if (c->recover.state[1] == RECOVER_FLIP) {
-        fn_right = 30.0f - spring_right;
-      } else if (c->recover.state[1] == RECOVER_STRETCH) {
-        fn_right = -80.0f - spring_right;
-      } else if (c->recover.state[1] == RECOVER_BACKLEG) {
-        fn_right = -40.0f - spring_right;
-      } else {
-        fn_right = -20.0f - spring_right;
-      }
-
-      VMC_InverseSolve(&c->vmc_[0], fn_left, 0.0f);
-      VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0], &c->output.joint[1]);
-      VMC_InverseSolve(&c->vmc_[1], fn_right, 0.0f);
-      VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3], &c->output.joint[2]);
-      c->output.wheel[0] = 0.0f;
-      c->output.wheel[1] = 0.0f;
-      Chassis_Output(c);
-
-      /* 两腿均完成，自动切换到平衡模式 */
-      if (c->recover.state[0] == RECOVER_COMPLETE &&
-          c->recover.state[1] == RECOVER_COMPLETE) {
-        Chassis_ResetControllers(c);
-        Chassis_SelectYawZeroPoint(c);
-        c->mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
-      }
-    } break;
-    
-    // case CHASSIS_MODE_CALIBRATE: {
-    //   Chassis_LQRControl(c, c_cmd);
-    //   LF= Chassis_CalcSpringForce(c->vmc_[0].leg.L0);
-    //   RF= Chassis_CalcSpringForce(c->vmc_[1].leg.L0);
-    //   L_fn = -LF;
-    //   VMC_InverseSolve(&c->vmc_[0], L_fn, L_tp);
-    //   VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0], &c->output.joint[1]);
-    //   VMC_InverseSolve(&c->vmc_[1], R_fn, R_tp);
-    //   VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3], &c->output.joint[2]);
-
-    //   c->output.wheel[0] = 0.0f;
-    //   c->output.wheel[1] = 0.0f;
-    //   Chassis_Output(c);
-    // } break;
 
     case CHASSIS_MODE_WHELL_LEG_BALANCE:
       Chassis_LQRControl(c, c_cmd);
+      Chassis_VMCControl(c, c_cmd);
       Chassis_Output(c);
       break;
     case CHASSIS_MODE_BALANCE_ROTOR:
       Chassis_LQRControl(c, c_cmd);
       c->output.wheel[0] += c_cmd->move_vec.vy * 0.2f;
       c->output.wheel[1] -= c_cmd->move_vec.vy * 0.2f;
+      Chassis_VMCControl(c, c_cmd);
       Chassis_Output(c);
       break;
 
@@ -952,6 +830,20 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
   c->output.wheel[0] = c->lqr[0].control_output.T / c->param->mech.wheel_gear_ratio + scaled_yaw_force;
   c->output.wheel[1] = c->lqr[1].control_output.T / c->param->mech.wheel_gear_ratio - scaled_yaw_force;
 
+  return CHASSIS_OK;
+}
+
+/**
+ * @brief VMC控制（虚拟模型控制）
+ * @param c 底盘结构体指针
+ * @param c_cmd 控制指令
+ * @return 操作结果
+ * @note 将LQR输出的力矩通过VMC逆解算转换为关节力矩，
+ *       同时处理横滚补偿、腿长控制、跳跃控制和摆角同步
+ */
+int8_t Chassis_VMCControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
+  if (c == NULL || c_cmd == NULL) return CHASSIS_ERR_NULL;
+
   /* ==================== 横滚角补偿 ==================== */
   /* 方法1: 几何前馈腿长补偿 (参考底盘文件夹算法) */
   float Rl = c->param->mech.hip_width / 2.0f;  /* 髋宽的一半 */

User/module/balance_chassis.h

@@ -291,6 +291,16 @@ 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 VMC控制（虚拟模型控制）
+ *
+ * \param c 包含底盘数据的结构体
+ * \param c_cmd 底盘控制指令
+ *
+ * \return 函数运行结果
+ */
+int8_t Chassis_VMCControl(Chassis_t *c, const Chassis_CMD_t *c_cmd);
+
 /**
  * \brief 底盘输出值
  *