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

加回rol补偿

Browse Source
This commit is contained in:
Robofish 2025-10-07 16:10:13 +08:00
parent b4fe3ca2c3
commit 042ab6e390
1 changed files with 8 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
User/module/balance_chassis.c
View File

@ -397,8 +397,10 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
// 横滚角补偿力（左腿减少支撑力）
virtual_force[0] =
(c->lqr[0].control_output.Tp) * sinf(c->vmc_[0].leg.theta) +
pid_output + 50.0f + roll_compensation_force;
pid_output + 60.0f + roll_compensation_force;
// virtual_force[0] =
// (c->lqr[0].control_output.Tp) * sinf(c->vmc_[0].leg.theta) +
// pid_output + 60.0f;
// VMC逆解算（包含摆角补偿）
if (VMC_InverseSolve(&c->vmc_[0], virtual_force[0],
c->lqr[0].control_output.Tp + Delta_Tp[0]) == 0) {
@ -421,7 +423,10 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
// 横滚角补偿力（右腿增加支撑力）
virtual_force[1] =
(c->lqr[1].control_output.Tp) * sinf(c->vmc_[1].leg.theta) +
pid_output + 55.0f - roll_compensation_force;
pid_output + 65.0f - roll_compensation_force;
// virtual_force[1] =
// (c->lqr[1].control_output.Tp) * sinf(c->vmc_[1].leg.theta) +
// pid_output + 65.0f;
// VMC逆解算（包含摆角补偿）
if (VMC_InverseSolve(&c->vmc_[1], virtual_force[1],