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

继续修

Browse Source
This commit is contained in:
Robofish 2026-03-15 21:29:41 +08:00
parent 0a442e8976
commit 4773c252f2
1 changed files with 3 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
User/module/balance_chassis.c
View File

@ -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;