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

改rc，改pid

Browse Source
This commit is contained in:
Robofish 2025-10-05 17:39:38 +08:00
parent 593cf97ae2
commit 24160f21dc
3 changed files with 8 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
User/module/balance_chassis.c
View File

@ -263,10 +263,10 @@ void Chassis_Output(Chassis_t *c) {
// } // }
float out[4] = { float out[4] = {
c->output.joint[0], c->output.joint[1], c->output.joint[2], c->output.joint[3]}; c->output.joint[0], c->output.joint[1], c->output.joint[2], c->output.joint[3]};
out[0] = -1.0f; // out[0] = -1.0f;
out[1] = 1.0f; // out[1] = 1.0f;
out[2] = 1.0f; // out[2] = 1.0f;
out[3] = -1.0f; // out[3] = -1.0f;
Virtual_Chassis_SendJointTorque(&virtual_chassis, out); Virtual_Chassis_SendJointTorque(&virtual_chassis, out);
Virtual_Chassis_SendWheelCommand(&virtual_chassis, c->output.wheel[0], c->output.wheel[1]); Virtual_Chassis_SendWheelCommand(&virtual_chassis, c->output.wheel[0], c->output.wheel[1]);
// for (int i = 0; i < 2; i++) { // for (int i = 0; i < 2; i++) {
@ -382,7 +382,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
c->vmc_[0].leg.L0, leg_d_length[0], c->dt); c->vmc_[0].leg.L0, leg_d_length[0], c->dt);
// 腿长控制力 = LQR摆杆力矩的径向分量 + PID腿长控制输出 + 基础支撑力 - 横滚角补偿力（左腿减少支撑力） // 腿长控制力 = LQR摆杆力矩的径向分量 + PID腿长控制输出 + 基础支撑力 - 横滚角补偿力（左腿减少支撑力）
virtual_force[0] = (c->lqr[0].control_output.Tp) * sinf(c->vmc_[0].leg.theta) + pid_output + 50.0f - roll_compensation_force; virtual_force[0] = (c->lqr[0].control_output.Tp) * sinf(c->vmc_[0].leg.theta) + pid_output + 10.0f ;
// VMC逆解算（包含摆角补偿） // VMC逆解算（包含摆角补偿）
if (VMC_InverseSolve(&c->vmc_[0], virtual_force[0], if (VMC_InverseSolve(&c->vmc_[0], virtual_force[0],

2
User/module/config.c
View File

@ -49,7 +49,7 @@ Config_RobotParam_t robot_config = {
}, },
.leg_length={ .leg_length={
.k = 40.0f, .k = 10.0f,
.p = 5.0f, .p = 5.0f,
.i = 0.0f, .i = 0.0f,
.d = 1.0f, .d = 1.0f,

4
User/task/rc.c
View File

@ -63,8 +63,8 @@ void Task_rc(void *argument) {
cmd_for_chassis.mode = CHASSIS_MODE_RELAX; cmd_for_chassis.mode = CHASSIS_MODE_RELAX;
break; break;
} }
cmd_for_chassis.move_vec.vx = dr16.data.ch_l_x; cmd_for_chassis.move_vec.vx = dr16.data.ch_l_y;
cmd_for_chassis.move_vec.vy = dr16.data.ch_l_y; cmd_for_chassis.move_vec.vy = dr16.data.ch_l_x;
cmd_for_chassis.move_vec.wz = dr16.data.ch_r_x; cmd_for_chassis.move_vec.wz = dr16.data.ch_r_x;
cmd_for_chassis.height = dr16.data.res; cmd_for_chassis.height = dr16.data.res;