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-17 08:41:22 +08:00
parent 0f4dfbd61d
commit 55403f1f87
1 changed files with 8 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
User/module/balance_chassis.c
View File

@ -1047,12 +1047,19 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
} }
/* 有速度指令时：纯速度控制，目标位移跟随当前位移（消除位移误差） /* 有速度指令时：纯速度控制，目标位移跟随当前位移（消除位移误差）
* 无速度指令时：位移+速度双环控制，积分保持位置锁定 */ * 无速度指令时：位移+速度双环控制，积分保持位置锁定
* 位移误差钳位到±2m，防止积分漂移过大 */
if (fabsf(c->chassis_state.target_velocity_x) > 0.01f) { if (fabsf(c->chassis_state.target_velocity_x) > 0.01f) {
c->chassis_state.target_x = c->chassis_state.position_x; c->chassis_state.target_x = c->chassis_state.position_x;
} else { } else {
c->chassis_state.target_x += c->chassis_state.target_velocity_x * c->dt; c->chassis_state.target_x += c->chassis_state.target_velocity_x * c->dt;
} }
float pos_err = c->chassis_state.target_x - c->chassis_state.position_x;
if (pos_err > 2.0f) {
c->chassis_state.target_x = c->chassis_state.position_x + 2.0f;
} else if (pos_err < -2.0f) {
c->chassis_state.target_x = c->chassis_state.position_x - 2.0f;
}
/* ==================== 状态更新 ==================== */ /* ==================== 状态更新 ==================== */