diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index fbcdc88..7a2efce 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -800,20 +800,6 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { LQR_CalculateGainMatrix(&c->lqr[0], c->vmc_[0].leg.L0); LQR_CalculateGainMatrix(&c->lqr[1], c->vmc_[1].leg.L0); - /* 速度误差大、高速运动或双腿离地时,禁用x位移跟踪防止轮子失控 */ - { - float vel_error = c->chassis_state.target_velocity_x - c->chassis_state.velocity_x; - bool both_off_ground = !c->vmc_[0].leg.is_ground_contact && !c->vmc_[1].leg.is_ground_contact; - if (fabsf(c->chassis_state.target_velocity_x) > 0.5f || - fabsf(vel_error) > 0.7f || - both_off_ground) { - c->lqr[0].K_matrix[0][2] = 0.0f; /* 禁用T的x位移增益 */ - c->lqr[1].K_matrix[0][2] = 0.0f; - c->chassis_state.position_x = 0.0f; - c->chassis_state.target_x = 0.0f; - } - } - /* ==================== 目标状态 ==================== */ /* 腿长-位移补偿:根据腿长多项式拟合补偿位移偏移 */ float avg_L0 = (c->vmc_[0].leg.L0 + c->vmc_[1].leg.L0) * 0.5f;