TuxMonkey/bf_original_balance_chassis
1
0
Fork 0
mirror of https://gitee.com/dlmu-cone/bf_original_balance_chassis synced 2026-07-23 19:25:09 +08:00
Code Issues Packages Projects Releases Wiki Activity

添加roll补偿

Browse Source
This commit is contained in:
kai
2024-03-25 20:43:42 +08:00
parent 90180ef92b
commit 994e726a7a
1 changed files with 27 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

37
application/chassis/balance.c
View File

@@ -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() /* 设定运动模态的输出 */