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

fix

Browse Source
This commit is contained in:
Robofish 2026-03-15 15:10:55 +08:00
parent d5029eefdf
commit c522e563af
1 changed files with 8 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
User/module/balance_chassis.c
View File

@ -155,17 +155,12 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) {
Chassis_ResetControllers(c);
Chassis_SelectYawZeroPoint(c);
/* RELAX -> BALANCE：根据 pitch 角度判断是否需要自起 */
/* RELAX -> BALANCE：pitch过大时保持RELAX，避免进入未实现的RECOVER */
if (mode == CHASSIS_MODE_WHELL_LEG_BALANCE && c->mode == CHASSIS_MODE_RELAX) {
float pitch = c->feedback.imu.euler.pit;
/* pitch 绝对值大于 0.8rad（约46度）认为是倒地，需要自起 */
if (fabsf(pitch) > 0.2f) {
/* 根据 theta 判断腿朝上还是朝下 */
float theta_l = c->vmc_[0].leg.theta;
float theta_r = c->vmc_[1].leg.theta;
c->recover.state[0] = (fabsf(theta_l) > (float)M_PI_2) ? RECOVER_FLIP : RECOVER_STRETCH;
c->recover.state[1] = (fabsf(theta_r) > (float)M_PI_2) ? RECOVER_FLIP : RECOVER_STRETCH;
c->mode = CHASSIS_MODE_RECOVER;
if (fabsf(pitch) > 0.8f) {
/* pitch过大，机体未扶正，保持RELAX */
Chassis_MotorRelax(c);
return CHASSIS_OK;
}
/* 否则直接进入平衡模式 */
@ -704,9 +699,10 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
Chassis_VMCControl(c, c_cmd);
break;
// case CHASSIS_MODE_RECOVER:
// Chassis_RecoverControl(c);
// break;
case CHASSIS_MODE_RECOVER:
/* 自起功能未实现，安全放松电机 */
Chassis_MotorRelax(c);
break;
case CHASSIS_MODE_WHELL_LEG_BALANCE:
Chassis_LQRControl(c, c_cmd);