From 994e726a7a31508f94c81cef7a43fc62d4f7f8eb Mon Sep 17 00:00:00 2001 From: kai <1797003616@qq.com> Date: Mon, 25 Mar 2024 20:43:42 +0800 Subject: [PATCH] =?UTF-8?q?=E6=B7=BB=E5=8A=A0roll=E8=A1=A5=E5=81=BF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- application/chassis/balance.c | 37 +++++++++++++++++++++++++---------- 1 file changed, 27 insertions(+), 10 deletions(-) diff --git a/application/chassis/balance.c b/application/chassis/balance.c index 22c0f34..14244ac 100644 --- a/application/chassis/balance.c +++ b/application/chassis/balance.c @@ -38,6 +38,7 @@ static ChassisParam chassis; // 综合运动补偿的PID控制器 static PIDInstance leglen_pid_l, leglen_pid_r; // 用PD模拟弹簧, 不要积分(弹簧是无积分二阶系统), 增益不可过大否则抗外界冲击响应时太"硬" +static PIDInstance roll_compensate_pid; // roll轴补偿,用于保持机体水平 static PIDInstance steer_p_pid, steer_v_pid; // 转向PID,有转向指令时使用IMU的加速度反馈积分以获取速度和位置状态量 static PIDInstance anti_crash_pid; // 抗劈叉,将输出以相反的方向叠加到左右腿的上 @@ -107,19 +108,29 @@ void BalanceInit() // 腿长控制 PID_Init_Config_s leg_length_pid_conf = { - .Kp = 800, - .Kd = 300, + .Kp = 600, + .Kd = 200, .Ki = 0, .MaxOut = 20, .DeadBand = 0.0001f, .Improve = PID_ChangingIntegrationRate | PID_Trapezoid_Intergral | PID_DerivativeFilter | PID_Derivative_On_Measurement, - .CoefA = 0.01, - .CoefB = 0.02, - .Derivative_LPF_RC = 0.08, + .Derivative_LPF_RC = 0.05, }; PIDInit(&leglen_pid_l, &leg_length_pid_conf); PIDInit(&leglen_pid_r, &leg_length_pid_conf); + // roll轴补偿 + PID_Init_Config_s roll_compensate_pid_conf = { + .Kp = 0.0006f, + .Kd = 0.00005f, + .Ki = 0.0f, + .MaxOut = 0.04, + .DeadBand = 0.001f, + .Improve = PID_DerivativeFilter | PID_Derivative_On_Measurement, + .Derivative_LPF_RC = 0.05, + }; + PIDInit(&roll_compensate_pid, &roll_compensate_pid_conf); + // 航向控制 // 角度环 PID_Init_Config_s steer_p_pid_conf = { @@ -186,7 +197,7 @@ static void ControlSwitch() { chassis_cmd_recv.chassis_mode = CHASSIS_FREE_DEBUG; // 自由转动&前后 chassis_cmd_recv.vx = 0.002 * (float)rc_data[TEMP].rc.rocker_r1; // speed x, unit m/s - chassis_cmd_recv.delta_leglen = -0.000005f * (float)rc_data[TEMP].rc.dial; + chassis_cmd_recv.delta_leglen = -0.000001f * (float)rc_data[TEMP].rc.dial; chassis_cmd_recv.offset_angle -= 0.00001 * (float)rc_data[TEMP].rc.rocker_r_; } } @@ -264,8 +275,8 @@ static void WokingStateSet() l_side.target_len += chassis_cmd_recv.delta_leglen; r_side.target_len += chassis_cmd_recv.delta_leglen; // 腿长限幅 - VAL_LIMIT(l_side.target_len, 0.12, 0.22); - VAL_LIMIT(r_side.target_len, 0.12, 0.22); + VAL_LIMIT(l_side.target_len, 0.12, 0.25); + VAL_LIMIT(r_side.target_len, 0.12, 0.25); // 加速度限幅,防止键盘控制摔倒 if (abs(chassis_cmd_recv.vx - chassis.target_v) / del_t < MAX_ACC_REF) @@ -334,9 +345,15 @@ static void SynthesizeMotion() /* 腿部控制:抗劈叉; 轮子控制:转向 */ static void LegControl() /* 腿长控制和Roll补偿 */ { + PIDCalculate(&roll_compensate_pid, chassis.roll, 0); + l_side.target_len += roll_compensate_pid.Output; + r_side.target_len -= roll_compensate_pid.Output; + static float gravity_comp = 57.63; - l_side.F_leg = PIDCalculate(&leglen_pid_l, l_side.height, l_side.target_len) + gravity_comp; - r_side.F_leg = PIDCalculate(&leglen_pid_r, r_side.height, r_side.target_len) + gravity_comp; + static float roll_extra_comp_p = 300; + float roll_comp = roll_extra_comp_p * chassis.roll; + l_side.F_leg = PIDCalculate(&leglen_pid_l, l_side.height, l_side.target_len) + gravity_comp - roll_comp; + r_side.F_leg = PIDCalculate(&leglen_pid_r, r_side.height, r_side.target_len) + gravity_comp + roll_comp; } static void WattLimitSet() /* 设定运动模态的输出 */