保存config
This commit is contained in:
parent
cc4d62efd5
commit
6ced1fd904
@ -195,11 +195,6 @@
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>target_pose</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>8</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>c->param->mech_param.sign.leg[i]</ItemText>
|
||||
</Ww>
|
||||
</WatchWindow1>
|
||||
<Tracepoint>
|
||||
<THDelay>0</THDelay>
|
||||
|
||||
BIN
MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.axf
Normal file
BIN
MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.axf
Normal file
Binary file not shown.
File diff suppressed because it is too large
Load Diff
@ -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;
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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_thigh = 1.57f, /* 左前腿大腿最大角度，单位：弧度 */
|
||||
.lf_calf = 1.57f, /* 左前腿小腿最大角度，单位：弧度 */
|
||||
.rf_hip = 1.57f, /* 右前腿髋关节最大角度，单位：弧度 */
|
||||
.rf_thigh = 1.57f, /* 右前腿大腿最大角度，单位：弧度 */
|
||||
.rf_calf = 1.57f, /* 右前腿小腿最大角度，单位：弧度 */
|
||||
.lr_hip = 1.57f, /* 左后腿髋关节最大角度，单位：弧度 */
|
||||
.lr_thigh = 1.57f, /* 左后腿大腿最大角度，单位：弧度 */
|
||||
.lr_calf = 1.57f, /* 左后腿小腿最大角度，单位：弧度 */
|
||||
.rr_hip = 1.57f, /* 右后腿髋关节最大角度，单位：弧度 */
|
||||
.rr_thigh = 1.57f, /* 右后腿大腿最大角度，单位：弧度 */
|
||||
.rr_calf = 1.57f, /* 右后腿小腿最大角度，单位：弧度 */
|
||||
.lf_hip = 0.50f, /* 左前腿髋关节最大角度，单位：弧度 */
|
||||
.lf_thigh = 2.50f, /* 左前腿大腿最大角度，单位：弧度 */
|
||||
.lf_calf = 2.60f, /* 左前腿小腿最大角度，单位：弧度 */
|
||||
.rf_hip = 0.50f, /* 右前腿髋关节最大角度，单位：弧度 */
|
||||
.rf_thigh = 0.0f, /* 右前腿大腿最大角度，单位：弧度 */
|
||||
.rf_calf = -1.2f, /* 右前腿小腿最大角度，单位：弧度 */
|
||||
.lr_hip = 0.50f, /* 左后腿髋关节最大角度，单位：弧度 */
|
||||
.lr_thigh = 2.50f, /* 左后腿大腿最大角度，单位：弧度 */
|
||||
.lr_calf = 2.60f, /* 左后腿小腿最大角度，单位：弧度 */
|
||||
.rr_hip = 0.50f, /* 右后腿髋关节最大角度，单位：弧度 */
|
||||
.rr_thigh = 0.0f, /* 右后腿大腿最大角度，单位：弧度 */
|
||||
.rr_calf = -1.2f, /* 右后腿小腿最大角度，单位：弧度 */
|
||||
}
|
||||
},
|
||||
.min = {
|
||||
.named = {
|
||||
.lf_hip = 1.57f, /* 左前腿髋关节最大角度，单位：弧度 */
|
||||
.lf_thigh = 1.57f, /* 左前腿大腿最大角度，单位：弧度 */
|
||||
.lf_calf = 1.57f, /* 左前腿小腿最大角度，单位：弧度 */
|
||||
.rf_hip = 1.57f, /* 右前腿髋关节最大角度，单位：弧度 */
|
||||
.rf_thigh = 1.57f, /* 右前腿大腿最大角度，单位：弧度 */
|
||||
.rf_calf = 1.57f, /* 右前腿小腿最大角度，单位：弧度 */
|
||||
.lr_hip = 1.57f, /* 左后腿髋关节最大角度，单位：弧度 */
|
||||
.lr_thigh = 1.57f, /* 左后腿大腿最大角度，单位：弧度 */
|
||||
.lr_calf = 1.57f, /* 左后腿小腿最大角度，单位：弧度 */
|
||||
.rr_hip = 1.57f, /* 右后腿髋关节最大角度，单位：弧度 */
|
||||
.rr_thigh = 1.57f, /* 右后腿大腿最大角度，单位：弧度 */
|
||||
.rr_calf = 1.57f, /* 右后腿小腿最大角度，单位：弧度 */
|
||||
.lf_hip = -0.50f, /* 左前腿髋关节最大角度，单位：弧度 */
|
||||
.lf_thigh = 0.0f, /* 左前腿大腿最大角度，单位：弧度 */
|
||||
.lf_calf = 1.20f, /* 左前腿小腿最大角度，单位：弧度 */
|
||||
.rf_hip = -0.50f, /* 右前腿髋关节最大角度，单位：弧度 */
|
||||
.rf_thigh = -2.50f, /* 右前腿大腿最大角度，单位：弧度 */
|
||||
.rf_calf = -2.6f, /* 右前腿小腿最大角度，单位：弧度 */
|
||||
.lr_hip = -0.50f, /* 左后腿髋关节最大角度，单位：弧度 */
|
||||
.lr_thigh = 0.0f, /* 左后腿大腿最大角度，单位：弧度 */
|
||||
.lr_calf = 1.20f, /* 左后腿小腿最大角度，单位：弧度 */
|
||||
.rr_hip = -0.50f, /* 右后腿髋关节最大角度，单位：弧度 */
|
||||
.rr_thigh = -2.50f, /* 右后腿大腿最大角度，单位：弧度 */
|
||||
.rr_calf = -2.6f, /* 右后腿小腿最大角度，单位：弧度 */
|
||||
}
|
||||
}
|
||||
},
|
||||
|
||||
|
||||
.zero_point = {
|
||||
.named = {
|
||||
.lf_hip = 1.98f, /* 左前腿髋关节零点角度，单位：弧度 */
|
||||
.lf_thigh = -2.86f, /* 左前腿大腿零点角度，单位：弧度 */
|
||||
.lf_calf = -32.367f, /* 左前腿小腿零点角度，单位：弧度 */
|
||||
.rf_hip = 5.56f, /* 右前腿髋关节零点角度，单位：弧度 */
|
||||
.rf_thigh = 12.92f, /* 右前腿大腿零点角度，单位：弧度 */
|
||||
.rf_calf = 37.047f, /* 右前腿小腿零点角度，单位：弧度 */
|
||||
.lr_hip = 4.76f, /* 左后腿髋关节零点角度，单位：弧度 */
|
||||
.lr_thigh = -4.57f, /* 左后腿大腿零点角度，单位：弧度 */
|
||||
.lr_calf = -35.847f, /* 左后腿小腿零点角度，单位：弧度 */
|
||||
.rr_hip = 2.42f, /* 右后腿髋关节零点角度，单位：弧度 */
|
||||
.rr_thigh = 8.85f, /* 右后腿大腿零点角度，单位：弧度 */
|
||||
.rr_calf = 38.55f, /* 右后腿小腿零点角度，单位：弧度 */
|
||||
.lf_hip = 0.02f, /* 左前腿髋关节零点角度，单位：弧度 */
|
||||
.lf_thigh = -3.17f, /* 左前腿大腿零点角度，单位：弧度 */
|
||||
.lf_calf = 2.84f - JOINT_CALF_OFFSET, /* 左前腿小腿零点角度，单位：弧度 */
|
||||
|
||||
.rf_hip = 5.37f, /* 右前腿髋关节零点角度，单位：弧度 */
|
||||
.rf_thigh = 9.38f, /* 右前腿大腿零点角度，单位：弧度 */
|
||||
.rf_calf = 4.45f + JOINT_CALF_OFFSET, /* 右前腿小腿零点角度，单位：弧度 */
|
||||
|
||||
.lr_hip = 4.70f, /* 左后腿髋关节零点角度，单位：弧度 */
|
||||
.lr_thigh = -4.11f, /* 左后腿大腿零点角度，单位：弧度 */
|
||||
.lr_calf = 2.56f - JOINT_CALF_OFFSET, /* 左后腿小腿零点角度，单位：弧度 */
|
||||
|
||||
.rr_hip = 2.85f, /* 右后腿髋关节零点角度，单位：弧度 */
|
||||
.rr_thigh = 10.81f, /* 右后腿大腿零点角度，单位：弧度 */
|
||||
.rr_calf = 4.83f + JOINT_CALF_OFFSET, /* 右后腿小腿零点角度，单位：弧度 */
|
||||
}
|
||||
},
|
||||
|
||||
|
||||
|
||||
.sign = {
|
||||
.leg = {
|
||||
[0] = { .hip = -1, .thigh = -1, .calf = -1 }, /* 左前腿 */
|
||||
[1] = { .hip = 1, .thigh = 1, .calf = 1 }, /* 右前腿 */
|
||||
[2] = { .hip = -1, .thigh = -1, .calf = -1 }, /* 左后腿 */
|
||||
[3] = { .hip = 1, .thigh = 1, .calf = 1 }, /* 右后腿 */
|
||||
[0] = { .hip = 1, .thigh = 1, .calf = -1 }, /* 左前腿 */
|
||||
[1] = { .hip = -1, .thigh = -1, .calf = 1 }, /* 右前腿 */
|
||||
[2] = { .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轴，单位：米 */
|
||||
}
|
||||
|
||||
},
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user