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

测试

Browse Source
This commit is contained in:
Robofish 2025-10-05 17:16:04 +08:00
parent 3e400fadc0
commit 593cf97ae2
1 changed files with 9 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
User/module/balance_chassis.c
View File

@ -218,7 +218,7 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
case CHASSIS_MODE_RECOVER: { case CHASSIS_MODE_RECOVER: {
float fn; float fn;
fn = -15.0f; fn = -25.0f;
VMC_InverseSolve(&c->vmc_[0], fn, 0.0f); VMC_InverseSolve(&c->vmc_[0], fn, 0.0f);
VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0], VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0],
@ -263,6 +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[1] = 1.0f;
out[2] = 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++) {
@ -332,8 +336,10 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
c->feedback.yaw.rotor_abs_angle, 0.0f, c->dt); c->feedback.yaw.rotor_abs_angle, 0.0f, c->dt);
/* 轮毂力矩输出（参考C++版本的减速比） */ /* 轮毂力矩输出（参考C++版本的减速比） */
c->output.wheel[0] = c->lqr[0].control_output.T / 4.5f + c->yaw_control.yaw_force; // c->output.wheel[0] = c->lqr[0].control_output.T / 4.5f + c->yaw_control.yaw_force;
c->output.wheel[1] = c->lqr[1].control_output.T / 4.5f - c->yaw_control.yaw_force; // c->output.wheel[1] = c->lqr[1].control_output.T / 4.5f - c->yaw_control.yaw_force;
c->output.wheel[0] = c->lqr[0].control_output.T / 4.5f;
c->output.wheel[1] = c->lqr[1].control_output.T / 4.5f;
/* 腿长控制和VMC逆解算（使用PID控制） */ /* 腿长控制和VMC逆解算（使用PID控制） */
float virtual_force[2]; float virtual_force[2];
float target_L0[2]; float target_L0[2];