保存config

MDK-ARM/CtrBoard-H7_ALL.uvoptx

@@ -195,11 +195,6 @@
           <WinNumber>1</WinNumber>
           <ItemText>target_pose</ItemText>
         </Ww>
-        <Ww>
-          <count>8</count>
-          <WinNumber>1</WinNumber>
-          <ItemText>c-&gt;param-&gt;mech_param.sign.leg[i]</ItemText>
-        </Ww>
       </WatchWindow1>
       <Tracepoint>
         <THDelay>0</THDelay>

User/component/kinematics.c

@@ -83,7 +83,7 @@ void Kinematics_ForwardKinematics(const float theta_in[3], const Kinematics_Link
  */
 int8_t Kinematics_InverseKinematics(const float foot_in[3], const Kinematics_LinkLength_t *leg_params, float theta_out[3], Kinematics_Sign_t *sign) {
     float px = foot_in[0];
-    float py = foot_in[1];
+    float py = -foot_in[1];
     float pz = foot_in[2];
 
     float b2y = leg_params->hip * sign->hip;
@@ -101,7 +101,12 @@ int8_t Kinematics_InverseKinematics(const float foot_in[3], const Kinematics_Lin
     float q3 = q3_ik(b3z, b4z, b);
     float q2 = q2_ik(q1, q3, px, py, pz, b3z, b4z);
 
+    // theta_out[0] = q1;
+    if (sign->hip == sign->thigh){
         theta_out[0] = q1;
+    } else {
+        theta_out[0] = -q1;
+    }
     theta_out[1] = q2 * sign->thigh;
     theta_out[2] = q3 * sign->calf;
     return 0;

User/module/chassis.c

@@ -190,13 +190,14 @@ int8_t Chassis_Control(Chassis_t *c, const CMD_ChassisCmd_t *c_cmd, uint32_t now
       chassis_mode_states = CHASSIS_MODE_RECOVER; /* 更新状态 */
 
       for (uint8_t i = 0; i < 4; i++) {
-        float target_pose[3] = {0.0,0.0,-0.2}; /* 目标位置 */
+        float target_pose[3] = {0.0,0.0861,-c->height}; /* 目标位置 */
         float angle_pose[3];
         Kinematics_InverseKinematics(target_pose, &c->param->mech_param.length, angle_pose, &c->param->mech_param.sign.leg[i]); /* 逆运动学计算 */
         c->output.id[i * 3].Pos = angle_pose[0]; /* 髋关节位置 */
         c->output.id[i * 3 + 1].Pos = angle_pose[1]; /* 大腿位置 */
         c->output.id[i * 3 + 2].Pos = angle_pose[2]; /* 小腿位置 */
       }
+
       // float target_pose[3] = {c_cmd->ctrl_vec.vx/10.0f, c_cmd->ctrl_vec.vy/10.0f, -0.2f}; /* 目标位置 */
       // float target_pose[3] = {0.0,0.0,-0.2}; /* 目标位置 */
       // float angle_pose[3];
@@ -204,7 +205,6 @@ int8_t Chassis_Control(Chassis_t *c, const CMD_ChassisCmd_t *c_cmd, uint32_t now
       // c->output.named.lf_hip.Pos = angle_pose[0]; /* 左前腿髋关节位置 */
       // c->output.named.lf_thigh.Pos = angle_pose[1]; /* 左前腿大腿位置 */
       // c->output.named.lf_calf.Pos = angle_pose[2]; /* 左前腿小腿位置 */
-
       break;
   }
 

User/module/chassis.h

@@ -98,7 +98,7 @@ typedef struct{
   joint_params ratio;      /* 关节减速比 */
   joint_limits limit; /* 关节的最小和最大角度，单位：弧度 */
   Kinematics_LinkLength_t length; /* 关节长度，单位：米 */
-  // Kinematics_LegOffset_t offset; /* 关节偏移，单位：米 */
+  Kinematics_LegOffset_t leg_offset; /* 关节偏移，单位：米 */
   Kinematics_DirectionSign_t sign; /* 关节侧向标志（左前/左后为-1，右前/右后为1） */
     
 }Chassis_Mech_Params_t;

User/module/config.c

@@ -19,11 +19,13 @@ Config_t param_default = {
         .left_leg = BSP_UART_LEFT_LEG,
         .right_leg = BSP_UART_RIGHT_LEG,
     },
+    .chassis_imu_type = IMU_WHEELREC_N100,
     .cali = {
         .bmi088 = {
             .gyro_offset = {0.0f, 0.0f, 0.0f},
         },
     },
+    
     .chassis = {
         .type = CHASSIS_TYPE_QUADRUPED,
 
@@ -45,66 +47,74 @@ Config_t param_default = {
 
         .mech_param = {
             .length = {
-                .hip = 0.05f,
+                .hip = 0.0861f,
                 .thigh = 0.20f,
                 .calf = 0.20f, 
             },
             .limit = {
                 .max = {
                     .named = {
-                        .lf_hip = 1.57f,   /* 左前腿髋关节最大角度，单位：弧度 */
+                        .lf_hip = 0.50f,   /* 左前腿髋关节最大角度，单位：弧度 */
-                        .lf_thigh = 1.57f, /* 左前腿大腿最大角度，单位：弧度 */
+                        .lf_thigh = 2.50f, /* 左前腿大腿最大角度，单位：弧度 */
-                        .lf_calf = 1.57f,  /* 左前腿小腿最大角度，单位：弧度 */
+                        .lf_calf = 2.60f,  /* 左前腿小腿最大角度，单位：弧度 */
-                        .rf_hip = 1.57f,   /* 右前腿髋关节最大角度，单位：弧度 */
+                        .rf_hip = 0.50f,   /* 右前腿髋关节最大角度，单位：弧度 */
-                        .rf_thigh = 1.57f, /* 右前腿大腿最大角度，单位：弧度 */
+                        .rf_thigh = 0.0f, /* 右前腿大腿最大角度，单位：弧度 */
-                        .rf_calf = 1.57f,  /* 右前腿小腿最大角度，单位：弧度 */
+                        .rf_calf = -1.2f,  /* 右前腿小腿最大角度，单位：弧度 */
-                        .lr_hip = 1.57f,   /* 左后腿髋关节最大角度，单位：弧度 */
+                        .lr_hip = 0.50f,   /* 左后腿髋关节最大角度，单位：弧度 */
-                        .lr_thigh = 1.57f, /* 左后腿大腿最大角度，单位：弧度 */
+                        .lr_thigh = 2.50f, /* 左后腿大腿最大角度，单位：弧度 */
-                        .lr_calf = 1.57f,  /* 左后腿小腿最大角度，单位：弧度 */
+                        .lr_calf = 2.60f,  /* 左后腿小腿最大角度，单位：弧度 */
-                        .rr_hip = 1.57f,   /* 右后腿髋关节最大角度，单位：弧度 */
+                        .rr_hip = 0.50f,   /* 右后腿髋关节最大角度，单位：弧度 */
-                        .rr_thigh = 1.57f, /* 右后腿大腿最大角度，单位：弧度 */
+                        .rr_thigh = 0.0f, /* 右后腿大腿最大角度，单位：弧度 */
-                        .rr_calf = 1.57f,  /* 右后腿小腿最大角度，单位：弧度 */
+                        .rr_calf = -1.2f,  /* 右后腿小腿最大角度，单位：弧度 */
                     }
                 },
                 .min = {
                     .named = {
-                        .lf_hip = 1.57f,   /* 左前腿髋关节最大角度，单位：弧度 */
+                        .lf_hip = -0.50f,   /* 左前腿髋关节最大角度，单位：弧度 */
-                        .lf_thigh = 1.57f, /* 左前腿大腿最大角度，单位：弧度 */
+                        .lf_thigh = 0.0f, /* 左前腿大腿最大角度，单位：弧度 */
-                        .lf_calf = 1.57f,  /* 左前腿小腿最大角度，单位：弧度 */
+                        .lf_calf = 1.20f,  /* 左前腿小腿最大角度，单位：弧度 */
-                        .rf_hip = 1.57f,   /* 右前腿髋关节最大角度，单位：弧度 */
+                        .rf_hip = -0.50f,   /* 右前腿髋关节最大角度，单位：弧度 */
-                        .rf_thigh = 1.57f, /* 右前腿大腿最大角度，单位：弧度 */
+                        .rf_thigh = -2.50f, /* 右前腿大腿最大角度，单位：弧度 */
-                        .rf_calf = 1.57f,  /* 右前腿小腿最大角度，单位：弧度 */
+                        .rf_calf = -2.6f,  /* 右前腿小腿最大角度，单位：弧度 */
-                        .lr_hip = 1.57f,   /* 左后腿髋关节最大角度，单位：弧度 */
+                        .lr_hip = -0.50f,   /* 左后腿髋关节最大角度，单位：弧度 */
-                        .lr_thigh = 1.57f, /* 左后腿大腿最大角度，单位：弧度 */
+                        .lr_thigh = 0.0f, /* 左后腿大腿最大角度，单位：弧度 */
-                        .lr_calf = 1.57f,  /* 左后腿小腿最大角度，单位：弧度 */
+                        .lr_calf = 1.20f,  /* 左后腿小腿最大角度，单位：弧度 */
-                        .rr_hip = 1.57f,   /* 右后腿髋关节最大角度，单位：弧度 */
+                        .rr_hip = -0.50f,   /* 右后腿髋关节最大角度，单位：弧度 */
-                        .rr_thigh = 1.57f, /* 右后腿大腿最大角度，单位：弧度 */
+                        .rr_thigh = -2.50f, /* 右后腿大腿最大角度，单位：弧度 */
-                        .rr_calf = 1.57f,  /* 右后腿小腿最大角度，单位：弧度 */
+                        .rr_calf = -2.6f,  /* 右后腿小腿最大角度，单位：弧度 */
                     }
                 }
             },
+
+
             .zero_point = {
                 .named = {
-                    .lf_hip = 1.98f,   /* 左前腿髋关节零点角度，单位：弧度 */
+                    .lf_hip = 0.02f,   /* 左前腿髋关节零点角度，单位：弧度 */
-                    .lf_thigh = -2.86f, /* 左前腿大腿零点角度，单位：弧度 */
+                    .lf_thigh = -3.17f, /* 左前腿大腿零点角度，单位：弧度 */
-                    .lf_calf = -32.367f,  /* 左前腿小腿零点角度，单位：弧度 */
+                    .lf_calf = 2.84f - JOINT_CALF_OFFSET,  /* 左前腿小腿零点角度，单位：弧度 */
-                    .rf_hip = 5.56f,   /* 右前腿髋关节零点角度，单位：弧度 */
+
-                    .rf_thigh = 12.92f, /* 右前腿大腿零点角度，单位：弧度 */
+                    .rf_hip = 5.37f,   /* 右前腿髋关节零点角度，单位：弧度 */
-                    .rf_calf = 37.047f,  /* 右前腿小腿零点角度，单位：弧度 */
+                    .rf_thigh = 9.38f, /* 右前腿大腿零点角度，单位：弧度 */
-                    .lr_hip = 4.76f,   /* 左后腿髋关节零点角度，单位：弧度 */
+                    .rf_calf = 4.45f + JOINT_CALF_OFFSET,  /* 右前腿小腿零点角度，单位：弧度 */
-                    .lr_thigh = -4.57f, /* 左后腿大腿零点角度，单位：弧度 */
+
-                    .lr_calf = -35.847f,  /* 左后腿小腿零点角度，单位：弧度 */
+                    .lr_hip = 4.70f,   /* 左后腿髋关节零点角度，单位：弧度 */
-                    .rr_hip = 2.42f,   /* 右后腿髋关节零点角度，单位：弧度 */
+                    .lr_thigh = -4.11f, /* 左后腿大腿零点角度，单位：弧度 */
-                    .rr_thigh = 8.85f, /* 右后腿大腿零点角度，单位：弧度 */
+                    .lr_calf = 2.56f - JOINT_CALF_OFFSET,  /* 左后腿小腿零点角度，单位：弧度 */
-                    .rr_calf = 38.55f,  /* 右后腿小腿零点角度，单位：弧度 */
+
+                    .rr_hip = 2.85f,   /* 右后腿髋关节零点角度，单位：弧度 */
+                    .rr_thigh = 10.81f, /* 右后腿大腿零点角度，单位：弧度 */
+                    .rr_calf = 4.83f + JOINT_CALF_OFFSET,  /* 右后腿小腿零点角度，单位：弧度 */
                 }
             },
+
+
+
             .sign = {
                 .leg = {
-                    [0] = { .hip = -1, .thigh = -1, .calf = -1 }, /* 左前腿 */
+                    [0] = { .hip = 1, .thigh = 1, .calf = -1 }, /* 左前腿 */
-                    [1] = { .hip = 1, .thigh = 1, .calf = 1 },   /* 右前腿 */
+                    [1] = { .hip = -1, .thigh = -1, .calf = 1 },   /* 右前腿 */
-                    [2] = { .hip = -1, .thigh = -1, .calf = -1 }, /* 左后腿 */
+                    [2] = { .hip = -1, .thigh = 1, .calf = -1 }, /* 左后腿 */
-                    [3] = { .hip = 1, .thigh = 1, .calf = 1 },   /* 右后腿 */
+                    [3] = { .hip = 1, .thigh = -1, .calf = 1 },   /* 右后腿 */
                 }
             },
             .ratio = {
@@ -123,6 +133,11 @@ Config_t param_default = {
                     .rr_calf = 12.66f, /* 右后腿小腿减速比 */
                 }
             },
+            .leg_offset = {
+                .x = 0.19625f, /* 腿偏移X轴，单位：米 */
+                .y = 0.0825f, /* 腿偏移Y轴，单位：米 */
+                .z = 0.0f, /* 腿偏移Z轴，单位：米 */
+            }
 
         },