上弹簧了

MDK-ARM/CtrBoard-H7_ALL.uvoptx

@@ -153,76 +153,37 @@
           <Name>-U-O142 -O2254 -S0 -C0 -N00("ARM CoreSight SW-DP") -D00(5BA02477) -L00(0) -TO18 -TC10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO7 -FD20000000 -FC1000 -FN1 -FF0STM32H72x-73x_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32H723VGTx$CMSIS\Flash\STM32H72x-73x_1024.FLM)</Name>
         </SetRegEntry>
       </TargetDriverDllRegistry>
-      <Breakpoint>
+      <Breakpoint/>
-        <Bp>
-          <Number>0</Number>
-          <Type>0</Type>
-          <LineNumber>255</LineNumber>
-          <EnabledFlag>1</EnabledFlag>
-          <Address>134218668</Address>
-          <ByteObject>0</ByteObject>
-          <HtxType>0</HtxType>
-          <ManyObjects>0</ManyObjects>
-          <SizeOfObject>0</SizeOfObject>
-          <BreakByAccess>0</BreakByAccess>
-          <BreakIfRCount>1</BreakIfRCount>
-          <Filename>startup_stm32h723xx.s</Filename>
-          <ExecCommand></ExecCommand>
-          <Expression>\\CtrBoard_H7_ALL\startup_stm32h723xx.s\255</Expression>
-        </Bp>
-        <Bp>
-          <Number>1</Number>
-          <Type>0</Type>
-          <LineNumber>0</LineNumber>
-          <EnabledFlag>1</EnabledFlag>
-          <Address>128</Address>
-          <ByteObject>0</ByteObject>
-          <HtxType>0</HtxType>
-          <ManyObjects>0</ManyObjects>
-          <SizeOfObject>0</SizeOfObject>
-          <BreakByAccess>0</BreakByAccess>
-          <BreakIfRCount>1</BreakIfRCount>
-          <Filename></Filename>
-          <ExecCommand></ExecCommand>
-          <Expression>0x00000080</Expression>
-        </Bp>
-        <Bp>
-          <Number>2</Number>
-          <Type>0</Type>
-          <LineNumber>0</LineNumber>
-          <EnabledFlag>1</EnabledFlag>
-          <Address>134</Address>
-          <ByteObject>0</ByteObject>
-          <HtxType>0</HtxType>
-          <ManyObjects>0</ManyObjects>
-          <SizeOfObject>0</SizeOfObject>
-          <BreakByAccess>0</BreakByAccess>
-          <BreakIfRCount>1</BreakIfRCount>
-          <Filename></Filename>
-          <ExecCommand></ExecCommand>
-          <Expression>0x00000086</Expression>
-        </Bp>
-      </Breakpoint>
       <WatchWindow1>
         <Ww>
           <count>0</count>
           <WinNumber>1</WinNumber>
-          <ItemText>chassis</ItemText>
+          <ItemText>L_length</ItemText>
         </Ww>
         <Ww>
           <count>1</count>
           <WinNumber>1</WinNumber>
-          <ItemText>shoot</ItemText>
+          <ItemText>R_length</ItemText>
         </Ww>
         <Ww>
           <count>2</count>
           <WinNumber>1</WinNumber>
-          <ItemText>dr16</ItemText>
+          <ItemText>L_fn</ItemText>
         </Ww>
         <Ww>
           <count>3</count>
           <WinNumber>1</WinNumber>
-          <ItemText>gimbal</ItemText>
+          <ItemText>R_fn</ItemText>
+        </Ww>
+        <Ww>
+          <count>4</count>
+          <WinNumber>1</WinNumber>
+          <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,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,6 +428,23 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
       Chassis_Output(c);
     } 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_Output(c);
@@ -565,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) {
@@ -579,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,6 +39,7 @@ extern "C" {
 typedef enum {
   CHASSIS_MODE_RELAX,            /* 放松模式，电机不输出。一般情况底盘初始化之后的模式 */
   CHASSIS_MODE_RECOVER,          /* 复位模式 */
+  // CHASSIS_MODE_CALIBRATE,        /* 校准模式 */
   CHASSIS_MODE_WHELL_LEG_BALANCE /* 轮子+腿平衡模式，底盘自我平衡 */
 } Chassis_Mode_t;
 

User/task/rc.c

@@ -116,7 +116,7 @@ switch (dr16.data.sw_l) {
     // 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_JUMP;
+      // cmd_for_chassis.mode = CHASSIS_MODE_CALIBRATE;
       break;
     default:
       cmd_for_chassis.mode = CHASSIS_MODE_RELAX;

utils/算弹簧/cacl.py

@@ -0,0 +1,99 @@
+import numpy as np
+import matplotlib.pyplot as plt
+from matplotlib.widgets import Slider
+
+# --- 全局常数定义 (单位: mm 或 N) ---
+L_AB = 215.0
+L_AC = 42.0
+L_BD = 50.0
+L_BE = 254.0  # 50 + 204
+Fs = 280.0    # 弹簧力 220N
+
+def calculate_force_ae(L_mm):
+    # 1. 几何计算
+    cos_theta = (L_AB**2 + L_BE**2 - L_mm**2) / (2 * L_AB * L_BE)
+    
+    if abs(cos_theta) > 1:
+        return np.nan
+        
+    theta = np.arccos(cos_theta) # 修正：定义 theta
+    sin_theta = np.sin(theta)
+
+    # 2. 计算弹簧长度 L_CD (米)
+    dx = L_AB - L_BD * cos_theta
+    dy = L_AC - L_BD * sin_theta
+    L_CD_m = np.sqrt(dx**2 + dy**2) / 1000.0
+
+    # 3. 计算 B 点扭矩 Mb (N·m)
+    # 基于之前推导的力臂公式: |2.1*cos(theta) - 10.75*sin(theta)|
+    # 这里的系数 2.1 是 L_BD*L_AC/1000, 10.75 是 L_AB*L_BD/1000
+    arm = abs(L_BD * cos_theta * L_AC - L_AB * L_BD * sin_theta) / 1000000.0
+    Mb = (Fs / L_CD_m) * arm
+
+    # 4. 计算 AE 方向的力分量 F_AE (N)
+    L_BE_m = L_BE / 1000.0
+    sin_psi = (L_AB * sin_theta) / L_mm if L_mm != 0 else np.nan
+    
+    F_AE = (Mb / L_BE_m) * sin_psi
+    return F_AE
+
+# --- 可视化部分 ---
+def update_plot(val):
+    L_ae = slider.val
+    ax_mech.clear()
+    
+    A = np.array([0, 0])
+    B = np.array([L_AB, 0])
+    C = np.array([0, L_AC])
+    
+    cos_theta = (L_AB**2 + L_BE**2 - L_ae**2) / (2 * L_AB * L_BE)
+    
+    if abs(cos_theta) <= 1:
+        theta = np.arccos(cos_theta)
+        # E 和 D 坐标计算 (对应手绘图趋势，y轴向上)
+        E = np.array([L_AB - L_BE * np.cos(theta), L_BE * np.sin(theta)])
+        D = np.array([L_AB - L_BD * np.cos(theta), L_BD * np.sin(theta)])
+        
+        ax_mech.plot([A[0], B[0]], [A[1], B[1]], 'ro-', lw=3, label='Base AB')
+        ax_mech.plot([A[0], C[0]], [A[1], C[1]], 'go-', lw=3, label='Link AC')
+        ax_mech.plot([B[0], E[0]], [B[1], E[1]], 'bo-', lw=3, label='Link BE')
+        ax_mech.plot([C[0], D[0]], [C[1], D[1]], 'k--', lw=2, label='Spring CD')
+        ax_mech.plot([A[0], E[0]], [A[1], E[1]], 'y:', label='Distance AE')
+
+        nodes = {'A': A, 'B': B, 'C': C, 'D': D, 'E': E}
+        for name, pos in nodes.items():
+            ax_mech.text(pos[0], pos[1] + 5, name, fontsize=10, fontweight='bold')
+
+    ax_mech.set_aspect('equal')
+    ax_mech.set_xlim(-60, L_AB + 100)
+    ax_mech.set_ylim(-50, L_BE + 50)
+    ax_mech.grid(True, linestyle=':')
+    ax_mech.set_title(f'Mechanism Pose (AE = {L_ae:.1f}mm)')
+    
+    # 更新力曲线上的红点
+    current_f = calculate_force_ae(L_ae)
+    line_force.set_data([L_ae], [current_f])
+    fig.canvas.draw_idle()
+
+# --- 初始化界面 ---
+fig, (ax_mech, ax_force) = plt.subplots(1, 2, figsize=(15, 6))
+plt.subplots_adjust(bottom=0.2)
+
+L_min, L_max = 39.1, 468.9
+L_range = np.linspace(L_min, L_max, 500)
+F_range = [calculate_force_ae(l) for l in L_range]
+
+ax_force.plot(L_range, F_range, 'b-', label='Force $F_{AE}$ (N)')
+ax_force.axhline(0, color='k', alpha=0.2)
+line_force, = ax_force.plot([], [], 'ro')
+ax_force.set_xlabel('AE Length (mm)')
+ax_force.set_ylabel('Force (N)')
+ax_force.grid(True, alpha=0.3)
+ax_force.set_title('Static Force Characteristic')
+
+ax_slider = plt.axes([0.2, 0.05, 0.6, 0.03])
+slider = Slider(ax_slider, 'AE Length', L_min, L_max, valinit=250.0)
+slider.on_changed(update_plot)
+
+update_plot(250.0)
+plt.show()