diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index fe925b7..3018419 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -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.x = 0; // 目标位置 // target_state.d_x = 0.0f; // 目标速度 - target_state.phi = -0.1f; // 目标俯仰角 - target_state.d_phi = 0.0f; // 目标俯仰角速度 + target_state.phi = 0.0f; // 目标俯仰角 + target_state.d_phi = -0.4f; // 目标俯仰角速度 // target_state.theta = -0.8845f * leg_L0[leg] + 0.40663f; // 目标摆杆角度 // target_state.d_theta = 0.0f; // 目标摆杆角速度 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++版本的减速比) */ - 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[0] = Tw[0] / 4.5f + c->yaw_control.yaw_force; + c->output.wheel[1] = Tw[1] / 4.5f - c->yaw_control.yaw_force; /* 腿长控制和VMC逆解算(使用PID控制) */ float virtual_force[2]; float target_L0[2]; diff --git a/User/module/config.c b/User/module/config.c index 982703c..b2bc584 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -28,9 +28,9 @@ Config_RobotParam_t robot_config = { .chassis_param = { .yaw={ .k=1.0f, - .p=1.0f, + .p=1.2f, .i=0.0f, - .d=0.0f, + .d=0.05f, .i_limit=0.0f, .out_limit=1.0f, .d_cutoff_freq=30.0f,