上弹簧了

MDK-ARM/CtrBoard-H7_ALL.uvoptx

@@ -158,27 +158,32 @@
         <Ww>
           <count>0</count>
           <WinNumber>1</WinNumber>
-          <ItemText>chassis</ItemText>
+          <ItemText>L_length</ItemText>
         </Ww>
         <Ww>
           <count>1</count>
           <WinNumber>1</WinNumber>
-          <ItemText>L_fn</ItemText>
+          <ItemText>R_length</ItemText>
         </Ww>
         <Ww>
           <count>2</count>
           <WinNumber>1</WinNumber>
-          <ItemText>R_fn</ItemText>
+          <ItemText>L_fn</ItemText>
         </Ww>
         <Ww>
           <count>3</count>
           <WinNumber>1</WinNumber>
-          <ItemText>L_length</ItemText>
+          <ItemText>R_fn</ItemText>
         </Ww>
         <Ww>
           <count>4</count>
           <WinNumber>1</WinNumber>
-          <ItemText>R_length</ItemText>
+          <ItemText>LF</ItemText>
+        </Ww>
+        <Ww>
+          <count>5</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>RF</ItemText>
         </Ww>
       </WatchWindow1>
       <Tracepoint>

User/module/balance_chassis.c

@@ -21,13 +21,13 @@
 #define MAX_ACCELERATION    2.0f     /* 最大加速度 (m/s²) */
 #define WHEEL_GEAR_RATIO    8.0f     /* 轮毂电机扭矩 */
 #define BASE_LEG_LENGTH     0.16f    /* 基础腿长 (m) */
-#define LEG_LENGTH_RANGE    0.22f    /* 腿长调节范围 (m) */
+#define LEG_LENGTH_RANGE    0.18f    /* 腿长调节范围 (m) */
 #define MIN_LEG_LENGTH      0.10f    /* 最小腿长 (m) */
 #define MAX_LEG_LENGTH      0.34f    /* 最大腿长 (m) */
 #define NON_CONTACT_THETA   0.16f    /* 离地时的摆角目标 (rad) */
 #define LEFT_BASE_FORCE     60.0f    /* 左腿基础支撑力 (N) */
 #define RIGHT_BASE_FORCE    60.0f    /* 右腿基础支撑力 (N) */
-float L_fn = 0.0f, L_tp = 0.0f, R_fn = 0.0f, R_tp = 0.0f;
+// float L_fn = 0.0f, L_tp = 0.0f, R_fn = 0.0f, R_tp = 0.0f,LF =0.0f,RF=0.0f;
 /* Private function prototypes ---------------------------------------------- */
 static int8_t Chassis_MotorEnable(Chassis_t *c);
 static int8_t Chassis_MotorRelax(Chassis_t *c);
@@ -207,6 +207,38 @@ static void Chassis_UpdateChassisState(Chassis_t *c) {
   c->chassis_state.position_x += c->chassis_state.velocity_x * c->dt;
 }
 
+/**
+ * @brief 计算五杆机构弹簧等效支撑力
+ * @param leg_length 腿长 L_AE (m)，有效范围 [0.0391, 0.4689]
+ * @return 等效支撑力 (N)，无解时返回 NAN
+ */
+static float Chassis_CalcSpringForce(float leg_length)
+{
+    // 五杆机构几何参数 (单位: m)
+    const float L_AB = 0.215f;  // 基座长度
+    const float L_AC = 0.042f;  // 弹簧上端高度
+    const float L_BD = 0.050f;  // 杆BD长度
+    const float L_BE = 0.254f;  // 杆BE长度
+    const float Fs = 360.0f;    // 弹簧恒力 (N)
+    
+    // 通过余弦定理计算∠ABE的余弦值
+    float cos_theta = (L_AB*L_AB + L_BE*L_BE - leg_length*leg_length) / (2.0f * L_AB * L_BE);
+    
+    // 检查解的有效性
+    if (fabsf(cos_theta) > 1.0f) {
+        return NAN;  // 无物理解
+    }
+    
+    float theta = acosf(cos_theta);
+    float sin_theta = sinf(theta);
+    
+    // 计算等效支撑力
+    // F_eq = (Fs × |力臂| × L_AB × sin(θ)) / (L_CD × L_BE × L_AE)
+    return (Fs * fabsf(L_BD*cos_theta*L_AC - L_AB*L_BD*sin_theta) * L_AB*sin_theta) 
+           / (sqrtf((L_AB - L_BD*cos_theta)*(L_AB - L_BD*cos_theta) 
+                  + (L_AC - L_BD*sin_theta)*(L_AC - L_BD*sin_theta)) * L_BE * leg_length);
+}
+
 /* Public functions --------------------------------------------------------- */
 
 /**
@@ -357,6 +389,18 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
                    c->feedback.joint[2].rotor_abs_angle,
                    c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
 
+  /* 修正离地检测：加上弹簧提供的支撑力 */
+  float spring_Fn_left = Chassis_CalcSpringForce(c->vmc_[0].leg.L0);
+  float spring_Fn_right = Chassis_CalcSpringForce(c->vmc_[1].leg.L0);
+  if (!isnan(spring_Fn_left)) {
+    c->vmc_[0].leg.Fn += spring_Fn_left;
+    c->vmc_[0].leg.is_ground_contact = (c->vmc_[0].leg.Fn > 20.0f);
+  }
+  if (!isnan(spring_Fn_right)) {
+    c->vmc_[1].leg.Fn += spring_Fn_right;
+    c->vmc_[1].leg.is_ground_contact = (c->vmc_[1].leg.Fn > 20.0f);
+  }
+
   /* 根据模式执行控制 */
   switch (c->mode) {
     case CHASSIS_MODE_RELAX:
@@ -368,9 +412,15 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
       float fn = -20.0f, tp = 0.0f;
       Chassis_LQRControl(c, c_cmd);
 
-      VMC_InverseSolve(&c->vmc_[0], fn, tp);
+      /* 计算弹簧力补偿 */
+      float spring_left = Chassis_CalcSpringForce(c->vmc_[0].leg.L0);
+      float spring_right = Chassis_CalcSpringForce(c->vmc_[1].leg.L0);
+      float fn_left = fn - (isnan(spring_left) ? 0.0f : spring_left);
+      float fn_right = fn - (isnan(spring_right) ? 0.0f : spring_right);
+
+      VMC_InverseSolve(&c->vmc_[0], fn_left, tp);
       VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0], &c->output.joint[1]);
-      VMC_InverseSolve(&c->vmc_[1], fn, tp);
+      VMC_InverseSolve(&c->vmc_[1], fn_right, tp);
       VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3], &c->output.joint[2]);
 
       c->output.wheel[0] = c_cmd->move_vec.vx * 0.2f;
@@ -378,19 +428,22 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
       Chassis_Output(c);
     } break;
     
-    case CHASSIS_MODE_CALIBRATE: {
+    // case CHASSIS_MODE_CALIBRATE: {
 
-      Chassis_LQRControl(c, c_cmd);
       
-      VMC_InverseSolve(&c->vmc_[0], L_fn, L_tp);
+    //   Chassis_LQRControl(c, c_cmd);
-      VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0], &c->output.joint[1]);
+    //   LF= Chassis_CalcSpringForce(c->vmc_[0].leg.L0);
-      VMC_InverseSolve(&c->vmc_[1], R_fn, R_tp);
+    //   RF= Chassis_CalcSpringForce(c->vmc_[1].leg.L0);
-      VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3], &c->output.joint[2]);
+    //   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[0] = 0.0f;
-      c->output.wheel[1] = 0.0f;
+    //   c->output.wheel[1] = 0.0f;
-      Chassis_Output(c);
+    //   Chassis_Output(c);
-    } break;
+    // } break;
 
     case CHASSIS_MODE_WHELL_LEG_BALANCE:
       Chassis_LQRControl(c, c_cmd);
@@ -579,8 +632,10 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
   /* ==================== 左腿VMC控制 ==================== */
   float pid_output_left = PID_Calc(&c->pid.leg_length[0], target_L0[0],
                                     c->vmc_[0].leg.L0, leg_d_length[0], c->dt);
+  float spring_force_left = Chassis_CalcSpringForce(c->vmc_[0].leg.L0);
+  if (isnan(spring_force_left)) spring_force_left = 0.0f;  /* 处理无效值 */
   float virtual_force_left = c->lqr[0].control_output.Tp * sinf(c->vmc_[0].leg.theta) +
-                              pid_output_left + LEFT_BASE_FORCE;
+                              pid_output_left + LEFT_BASE_FORCE - spring_force_left;
 
   if (VMC_InverseSolve(&c->vmc_[0], virtual_force_left,
                        c->lqr[0].control_output.Tp + Delta_Tp[0]) == 0) {
@@ -593,8 +648,10 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
   /* ==================== 右腿VMC控制 ==================== */
   float pid_output_right = PID_Calc(&c->pid.leg_length[1], target_L0[1],
                                      c->vmc_[1].leg.L0, leg_d_length[1], c->dt);
+  float spring_force_right = Chassis_CalcSpringForce(c->vmc_[1].leg.L0);
+  if (isnan(spring_force_right)) spring_force_right = 0.0f;  /* 处理无效值 */
   float virtual_force_right = c->lqr[1].control_output.Tp * sinf(c->vmc_[1].leg.theta) +
-                               pid_output_right + RIGHT_BASE_FORCE;
+                               pid_output_right + RIGHT_BASE_FORCE - spring_force_right;
 
   if (VMC_InverseSolve(&c->vmc_[1], virtual_force_right,
                        c->lqr[1].control_output.Tp + Delta_Tp[1]) == 0) {

User/module/balance_chassis.h

@@ -39,7 +39,7 @@ extern "C" {
 typedef enum {
   CHASSIS_MODE_RELAX,            /* 放松模式，电机不输出。一般情况底盘初始化之后的模式 */
   CHASSIS_MODE_RECOVER,          /* 复位模式 */
-  CHASSIS_MODE_CALIBRATE,        /* 校准模式 */
+  // CHASSIS_MODE_CALIBRATE,        /* 校准模式 */
   CHASSIS_MODE_WHELL_LEG_BALANCE /* 轮子+腿平衡模式，底盘自我平衡 */
 } Chassis_Mode_t;
 

User/task/rc.c

@@ -115,8 +115,8 @@ switch (dr16.data.sw_l) {
     case DR16_SW_DOWN:
     // cmd_for_chassis.mode = CHASSIS_MODE_RECOVER;
       // cmd_for_chassis.mode = CHASSIS_MODE_ROTOR;
-      // cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
+      cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
-      cmd_for_chassis.mode = CHASSIS_MODE_CALIBRATE;
+      // cmd_for_chassis.mode = CHASSIS_MODE_CALIBRATE;
       break;
     default:
       cmd_for_chassis.mode = CHASSIS_MODE_RELAX;

utils/算弹簧/cacl.py

@@ -7,7 +7,7 @@ L_AB = 215.0
 L_AC = 42.0
 L_BD = 50.0
 L_BE = 254.0  # 50 + 204
-Fs = 220.0    # 弹簧力 220N
+Fs = 280.0    # 弹簧力 220N
 
 def calculate_force_ae(L_mm):
     # 1. 几何计算