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 16:20:58 +08:00
parent 39e18ab745
commit 557a41e8b6
1 changed files with 5 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
User/module/balance_chassis.c
View File

@ -770,7 +770,8 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
/* 小陀螺平移：将世界坐标系期望速度投影到机体坐标系 */
float yaw_angle = c->feedback.imu.euler.yaw;
float world_vx = c_cmd->move_vec.vx * c->param->rotor_ctrl.max_trans_speed;
c->chassis_state.target_velocity_x = world_vx * cosf(yaw_angle);
float world_vy = c_cmd->move_vec.vy * c->param->rotor_ctrl.max_trans_speed;
c->chassis_state.target_velocity_x = world_vx * cosf(yaw_angle) + world_vy * sinf(yaw_angle);
c->chassis_state.last_target_velocity_x = c->chassis_state.target_velocity_x;
c->chassis_state.target_x += c->chassis_state.target_velocity_x * c->dt;
} else {
@ -819,8 +820,8 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
/* ==================== Yaw轴控制 ==================== */
if (c_cmd->mode == CHASSIS_MODE_BALANCE_ROTOR || c->mode == CHASSIS_MODE_BALANCE_ROTOR) {
/* 小陀螺模式：PID跟踪目标角速度，带速度斜坡 */
float target_spin = c_cmd->move_vec.vy * c->param->rotor_ctrl.max_spin_speed;
/* 小陀螺模式：自动以max_spin_speed恒速旋转 */
float target_spin = c->param->rotor_ctrl.max_spin_speed;
if (c->rotor_state.exiting) {
/* 退出过渡：目标角速度为0，用减速斜坡 */
@ -860,7 +861,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
c->yaw_control.yaw_force = PID_Calc(&c->pid.yaw, c->yaw_control.target_yaw,
c->feedback.yaw.rotor_abs_angle, 0.0f, c->dt);
} else {
float current_yaw_omega = c->feedback.imu.gyro.z;
float current_yaw_omega = -c->feedback.imu.gyro.z; /* 取反匹配yaw_force→轮子旋转方向 */
c->yaw_control.yaw_force = PID_Calc(&c->pid.rotor, c->rotor_state.current_spin_speed,
current_yaw_omega, 0.0f, c->dt);
}