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

修复急刹情况

Browse Source
This commit is contained in:
kai
2024-04-28 22:25:29 +08:00
parent b7e4c80cb4
commit 8fc322fd51
2 changed files with 10 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
application/chassis/balance.c
View File

@@ -109,7 +109,7 @@ void BalanceInit()
// 腿长控制 // 腿长控制
PID_Init_Config_s leg_length_pid_conf = { PID_Init_Config_s leg_length_pid_conf = {
.Kp = 600, .Kp = 600,
.Kd = 200, .Kd = 100,
.Ki = 0, .Ki = 0,
.MaxOut = 20, .MaxOut = 20,
.DeadBand = 0.0001f, .DeadBand = 0.0001f,
@@ -121,8 +121,8 @@ void BalanceInit()
// roll轴补偿 // roll轴补偿
PID_Init_Config_s roll_compensate_pid_conf = { PID_Init_Config_s roll_compensate_pid_conf = {
.Kp = 0.0006f, .Kp = 0.0008f,
.Kd = 0.00005f, .Kd = 0.0001f,
.Ki = 0.0f, .Ki = 0.0f,
.MaxOut = 0.04, .MaxOut = 0.04,
.DeadBand = 0.001f, .DeadBand = 0.001f,
@@ -137,7 +137,7 @@ void BalanceInit()
.Kp = 5, .Kp = 5,
.Kd = 0, .Kd = 0,
.Ki = 0.0f, .Ki = 0.0f,
.MaxOut = 4, .MaxOut = 3,
.DeadBand = 0.001f, .DeadBand = 0.001f,
.Improve = PID_DerivativeFilter | PID_Derivative_On_Measurement, .Improve = PID_DerivativeFilter | PID_Derivative_On_Measurement,
.Derivative_LPF_RC = 0.05, .Derivative_LPF_RC = 0.05,
@@ -158,7 +158,7 @@ void BalanceInit()
// 抗劈叉 // 抗劈叉
PID_Init_Config_s anti_crash_pid_conf = { PID_Init_Config_s anti_crash_pid_conf = {
.Kp = 8, .Kp = 8,
.Kd = 2.5, .Kd = 2,
.Ki = 0.0, .Ki = 0.0,
.MaxOut = 10, .MaxOut = 10,
.DeadBand = 0.001f, .DeadBand = 0.001f,
@@ -197,7 +197,7 @@ static void ControlSwitch()
else else
{ {
chassis_cmd_recv.chassis_mode = CHASSIS_FREE_DEBUG; // 自由转动&前后 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.vx = 0.004 * (float)rc_data[TEMP].rc.rocker_r1; // speed x, unit m/s
chassis_cmd_recv.delta_leglen = -0.000001f * (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.000005 * (float)rc_data[TEMP].rc.rocker_r_; chassis_cmd_recv.offset_angle -= 0.000005 * (float)rc_data[TEMP].rc.rocker_r_;
} }
@@ -291,9 +291,6 @@ static void WokingStateSet()
VAL_LIMIT(r_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)
chassis.target_v = chassis_cmd_recv.vx;
else
chassis.target_v += sign(chassis_cmd_recv.vx - chassis.target_v) * MAX_ACC_REF * del_t; chassis.target_v += sign(chassis_cmd_recv.vx - chassis.target_v) * MAX_ACC_REF * del_t;
// 模型距离参考输入 // 模型距离参考输入
chassis.target_dist += chassis.target_v * del_t; chassis.target_dist += chassis.target_v * del_t;
@@ -355,7 +352,7 @@ static void SynthesizeMotion() /* 腿部控制:抗劈叉; 轮子控制:转向 */
r_side.T_wheel += steer_v_pid.Output; r_side.T_wheel += steer_v_pid.Output;
// 抗劈叉 // 抗劈叉
static float swerving_speed_ff, ff_coef = 0; static float swerving_speed_ff, ff_coef = 3;
swerving_speed_ff = ff_coef * steer_v_pid.Output; // 用于抗劈叉的前馈 swerving_speed_ff = ff_coef * steer_v_pid.Output; // 用于抗劈叉的前馈
PIDCalculate(&anti_crash_pid, l_side.phi5 - r_side.phi5, 0); PIDCalculate(&anti_crash_pid, l_side.phi5 - r_side.phi5, 0);
l_side.T_hip += anti_crash_pid.Output - swerving_speed_ff; l_side.T_hip += anti_crash_pid.Output - swerving_speed_ff;
@@ -369,7 +366,7 @@ static void LegControl() /* 腿长控制和Roll补偿 */
l_side.target_len += roll_compensate_pid.Output; l_side.target_len += roll_compensate_pid.Output;
r_side.target_len -= roll_compensate_pid.Output; r_side.target_len -= roll_compensate_pid.Output;
static float gravity_comp = 60; static float gravity_comp = 80;
static float roll_extra_comp_p = 300; static float roll_extra_comp_p = 300;
float roll_comp = roll_extra_comp_p * chassis.roll; 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; l_side.F_leg = PIDCalculate(&leglen_pid_l, l_side.height, l_side.target_len) + gravity_comp - roll_comp;

2
application/chassis/balance.h
View File

@@ -8,7 +8,7 @@
#define LIMIT_LINK_RAD 0.220039368 // 初始限位角度,见ParamAssemble #define LIMIT_LINK_RAD 0.220039368 // 初始限位角度,见ParamAssemble
#define BALANCE_GRAVITY_BIAS 0 #define BALANCE_GRAVITY_BIAS 0
#define ROLL_GRAVITY_BIAS 0 #define ROLL_GRAVITY_BIAS 0
#define MAX_ACC_REF 0.5f #define MAX_ACC_REF 0.8f
#define MAX_DIST_TRACK 1.0f #define MAX_DIST_TRACK 1.0f
#define MAX_VEL_TRACK 0.5f #define MAX_VEL_TRACK 0.5f