robofish/rm_balance
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

改腿长

Browse Source
This commit is contained in:
Robofish 2025-10-14 21:34:49 +08:00
parent dd3501693b
commit a5456a9094
3 changed files with 525 additions and 525 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1028
Core/Src/stm32f4xx_it.c
View File

File diff suppressed because it is too large Load Diff

2
User/device/motor_rm.c
View File

@ -74,7 +74,7 @@ static int8_t MOTOR_RM_GetLogicalIndex(uint16_t can_id, MOTOR_RM_Module_t module
static float MOTOR_RM_GetRatio(MOTOR_RM_Module_t module) { static float MOTOR_RM_GetRatio(MOTOR_RM_Module_t module) {
switch (module) { switch (module) {
case MOTOR_M2006: return 36.0f; case MOTOR_M2006: return 36.0f;
case MOTOR_M3508: return 19.0f; case MOTOR_M3508: return 3591.0f / 187.0f;
case MOTOR_GM6020: return 1.0f; case MOTOR_GM6020: return 1.0f;
default: return 1.0f; default: return 1.0f;
} }

20
User/module/balance_chassis.c
View File

@ -409,9 +409,9 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
LQR_State_t target_state = {0}; LQR_State_t target_state = {0};
target_state.theta = c->param->theta; // 目标摆杆角度 target_state.theta = c->param->theta; // 目标摆杆角度
target_state.d_theta = 0.0f; // 目标摆杆角速度 target_state.d_theta = 0.0f; // 目标摆杆角速度
target_state.phi = 11.9601*pow((0.15f + c_cmd->height * 0.25f),3) + -11.8715*pow((0.15f + c_cmd->height * 0.25f),2) + 3.87083*(0.15f + c_cmd->height * 0.25f) + -0.414154; // 目标俯仰角 target_state.phi = 11.9601*pow((0.13f + c_cmd->height * 0.25f),3) + -11.8715*pow((0.13f + c_cmd->height * 0.25f),2) + 3.87083*(0.13f + c_cmd->height * 0.25f) + -0.414154; // 目标俯仰角
target_state.d_phi = 0.0f; // 目标俯仰角速度 target_state.d_phi = 0.0f; // 目标俯仰角速度
target_state.x = c->chassis_state.target_x -2.07411f*(0.15f + c_cmd->height * 0.25f) + 0.41182f; target_state.x = c->chassis_state.target_x -2.07411f*(0.13f + c_cmd->height * 0.25f) + 0.41182f;
target_state.d_x = target_dot_x; // 目标速度 target_state.d_x = target_dot_x; // 目标速度
if (c->mode != CHASSIS_MODE_ROTOR){ if (c->mode != CHASSIS_MODE_ROTOR){
@ -434,7 +434,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
LQR_SetTargetState(&c->lqr[0], &target_state); LQR_SetTargetState(&c->lqr[0], &target_state);
LQR_Control(&c->lqr[0]); LQR_Control(&c->lqr[0]);
} else { } else {
target_state.theta = 0.15f; // 非接触时，腿摆角目标为0.1rad，防止腿完全悬空 target_state.theta = 0.13f; // 非接触时，腿摆角目标为0.1rad，防止腿完全悬空
LQR_SetTargetState(&c->lqr[0], &target_state); LQR_SetTargetState(&c->lqr[0], &target_state);
c->lqr[0].control_output.T = 0.0f; c->lqr[0].control_output.T = 0.0f;
c->lqr[0].control_output.Tp = c->lqr[0].control_output.Tp =
@ -446,7 +446,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
LQR_SetTargetState(&c->lqr[1], &target_state); LQR_SetTargetState(&c->lqr[1], &target_state);
LQR_Control(&c->lqr[1]); LQR_Control(&c->lqr[1]);
}else { }else {
target_state.theta = 0.15f; // 非接触时，腿摆角目标为0.1rad，防止腿完全悬空 target_state.theta = 0.13f; // 非接触时，腿摆角目标为0.1rad，防止腿完全悬空
LQR_SetTargetState(&c->lqr[1], &target_state); LQR_SetTargetState(&c->lqr[1], &target_state);
c->lqr[1].control_output.T = 0.0f; c->lqr[1].control_output.T = 0.0f;
c->lqr[1].control_output.Tp = c->lqr[1].control_output.Tp =
@ -480,12 +480,12 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
// 目标腿长设定（移除横滚角补偿） // 目标腿长设定（移除横滚角补偿）
switch (c->state) { switch (c->state) {
case 0: // 正常状态，根据高度指令调节腿长 case 0: // 正常状态，根据高度指令调节腿长
target_L0[0] = 0.15f + c_cmd->height * 0.22f; // 左腿：基础腿长 + 高度调节 target_L0[0] = 0.13f + c_cmd->height * 0.22f; // 左腿：基础腿长 + 高度调节
target_L0[1] = 0.15f + c_cmd->height * 0.22f; // 右腿：基础腿长 + 高度调节 target_L0[1] = 0.13f + c_cmd->height * 0.22f; // 右腿：基础腿长 + 高度调节
break; break;
case 1: // 准备阶段，腿长收缩 case 1: // 准备阶段，腿长收缩
target_L0[0] = 0.15f; // 左腿：收缩到基础腿长 target_L0[0] = 0.13f; // 左腿：收缩到基础腿长
target_L0[1] = 0.15f; // 右腿：收缩到基础腿长 target_L0[1] = 0.13f; // 右腿：收缩到基础腿长
break; break;
case 2: // 起跳阶段，腿长快速伸展 case 2: // 起跳阶段，腿长快速伸展
target_L0[0] = 0.55f; // 左腿：伸展到最大腿长 target_L0[0] = 0.55f; // 左腿：伸展到最大腿长
@ -496,8 +496,8 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
target_L0[1] = 0.1f; // 右腿：缓冲腿长 target_L0[1] = 0.1f; // 右腿：缓冲腿长
break; break;
default: default:
target_L0[0] = 0.15f + c_cmd->height * 0.22f; target_L0[0] = 0.13f + c_cmd->height * 0.22f;
target_L0[1] = 0.15f + c_cmd->height * 0.22f; target_L0[1] = 0.13f + c_cmd->height * 0.22f;
break; break;
} }