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-01 03:16:28 +08:00
parent 57b47b800e
commit c024cb537e
2 changed files with 6 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
User/module/balance_chassis.c
View File

@ -402,8 +402,8 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
target_state.d_theta = 0.0f; // 目标摆杆角速度 target_state.d_theta = 0.0f; // 目标摆杆角速度
// target_state.x = 0; // 目标位置 // target_state.x = 0; // 目标位置
// target_state.d_x = 0.0f; // 目标速度 // target_state.d_x = 0.0f; // 目标速度
target_state.phi = -0.1f; // 目标俯仰角 target_state.phi = 0.0f; // 目标俯仰角
target_state.d_phi = 0.0f; // 目标俯仰角速度 target_state.d_phi = -0.4f; // 目标俯仰角速度
// target_state.theta = -0.8845f * leg_L0[leg] + 0.40663f; // 目标摆杆角度 // target_state.theta = -0.8845f * leg_L0[leg] + 0.40663f; // 目标摆杆角度
// target_state.d_theta = 0.0f; // 目标摆杆角速度 // target_state.d_theta = 0.0f; // 目标摆杆角速度
target_state.x = c->chassis_state.target_x; // 目标位置 target_state.x = c->chassis_state.target_x; // 目标位置
@ -462,8 +462,8 @@ 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); 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);
/* 轮毂力矩输出（参考C++版本的减速比） */ /* 轮毂力矩输出（参考C++版本的减速比） */
c->output.wheel[0] = Tw[0] / 4.5f - c->yaw_control.yaw_force; c->output.wheel[0] = Tw[0] / 4.5f + c->yaw_control.yaw_force;
c->output.wheel[1] = Tw[1] / 4.5f + c->yaw_control.yaw_force; c->output.wheel[1] = Tw[1] / 4.5f - c->yaw_control.yaw_force;
/* 腿长控制和VMC逆解算（使用PID控制） */ /* 腿长控制和VMC逆解算（使用PID控制） */
float virtual_force[2]; float virtual_force[2];
float target_L0[2]; float target_L0[2];

4
User/module/config.c
View File

@ -28,9 +28,9 @@ Config_RobotParam_t robot_config = {
.chassis_param = { .chassis_param = {
.yaw={ .yaw={
.k=1.0f, .k=1.0f,
.p=1.0f, .p=1.2f,
.i=0.0f, .i=0.0f,
.d=0.0f, .d=0.05f,
.i_limit=0.0f, .i_limit=0.0f,
.out_limit=1.0f, .out_limit=1.0f,
.d_cutoff_freq=30.0f, .d_cutoff_freq=30.0f,