diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index 15961e5..dd05833 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -491,14 +491,15 @@ static void Chassis_ClimbControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { float theta_abs = fabsf(avg_theta); float contact_angle = c->param->climb.theta_retract_threshold * 0.5f; float tp_min = c->param->climb.tp_scale; + float tp_initial = 0.8f; /* 初始Tp缩放,刚进入就削弱20% */ float dynamic_scale; if (theta_abs < contact_angle) { - dynamic_scale = 1.0f; /* 未撞到,保持正常Tp */ + dynamic_scale = tp_initial; /* 未撞到,从80%开始 */ } else { float t = (theta_abs - contact_angle) / (c->param->climb.theta_retract_threshold - contact_angle); t = LIMIT(t, 0.0f, 1.0f); - dynamic_scale = 1.0f + (tp_min - 1.0f) * sqrtf(t); /* sqrt: 撞到后快速衰减 */ + dynamic_scale = tp_initial + (tp_min - tp_initial) * sqrtf(t); /* sqrt: 撞到后快速衰减 */ } c->lqr[0].control_output.Tp *= dynamic_scale; c->lqr[1].control_output.Tp *= dynamic_scale;