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

添加速度输入, 腿长pid待调整

Browse Source
This commit is contained in:
kai
2024-03-24 22:34:47 +08:00
parent 5b7c5cf88f
commit d5b5c254c4
2 changed files with 21 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

28
application/chassis/balance.c
View File

@@ -55,8 +55,8 @@ void BalanceInit()
.can_handle = &hcan1}, .can_handle = &hcan1},
.controller_param_init_config = { .controller_param_init_config = {
.angle_PID = { .angle_PID = {
.Kp = 0.3, .Kp = 0.2,
.Kd = 0.1, .Kd = 0,
.Ki = 0, .Ki = 0,
.DeadBand = 0.0001, .DeadBand = 0.0001,
.Improve = PID_DerivativeFilter | PID_Derivative_On_Measurement, .Improve = PID_DerivativeFilter | PID_Derivative_On_Measurement,
@@ -105,10 +105,10 @@ void BalanceInit()
// 腿长控制 // 腿长控制
PID_Init_Config_s leg_length_pid_conf = { PID_Init_Config_s leg_length_pid_conf = {
.Kp = 600, .Kp = 800,
.Kd = 150, .Kd = 300,
.Ki = 0, .Ki = 0,
.MaxOut = 60, .MaxOut = 20,
.DeadBand = 0.0001f, .DeadBand = 0.0001f,
.Improve = PID_ChangingIntegrationRate | PID_Trapezoid_Intergral | PID_DerivativeFilter | PID_Derivative_On_Measurement, .Improve = PID_ChangingIntegrationRate | PID_Trapezoid_Intergral | PID_DerivativeFilter | PID_Derivative_On_Measurement,
.CoefA = 0.01, .CoefA = 0.01,
@@ -148,7 +148,7 @@ static void ControlSwitch()
{ {
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.002 * (float)rc_data[TEMP].rc.rocker_r1; // speed x, unit m/s
chassis_cmd_recv.delta_leglen = -0.0000015f * (float)rc_data[TEMP].rc.dial; chassis_cmd_recv.delta_leglen = -0.000005f * (float)rc_data[TEMP].rc.dial;
} }
} }
else else
@@ -161,6 +161,10 @@ static void ResetChassis()
{ {
EnableAllMotor(); // 打开全部电机,关节复位到起始角度,驱动电机响应速度输入以从墙角或固连中脱身 EnableAllMotor(); // 打开全部电机,关节复位到起始角度,驱动电机响应速度输入以从墙角或固连中脱身
// 复位时清空距离和腿长积累量,保证顺利站起
chassis.dist = chassis.target_dist = 0;
l_side.target_len = r_side.target_len = 0.12;
// 撞墙时前后移动保证能重新站立,执行速度输入 // 撞墙时前后移动保证能重新站立,执行速度输入
LKMotorSetRef(l_driven, chassis_cmd_recv.vx * 2); LKMotorSetRef(l_driven, chassis_cmd_recv.vx * 2);
LKMotorSetRef(r_driven, -chassis_cmd_recv.vx * 2); LKMotorSetRef(r_driven, -chassis_cmd_recv.vx * 2);
@@ -221,8 +225,16 @@ static void WokingStateSet()
l_side.target_len += chassis_cmd_recv.delta_leglen; l_side.target_len += chassis_cmd_recv.delta_leglen;
r_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.25); VAL_LIMIT(l_side.target_len, 0.12, 0.22);
VAL_LIMIT(r_side.target_len, 0.12, 0.25); VAL_LIMIT(r_side.target_len, 0.12, 0.22);
// 加速度限幅,防止键盘控制摔倒
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_dist += chassis.target_v * del_t;
} }

2
application/chassis/balance.h
View File

@@ -8,7 +8,7 @@
#define LIMIT_LINK_RAD 0.205467224 // 初始限位角度,见ParamAssemble #define LIMIT_LINK_RAD 0.205467224 // 初始限位角度,见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.7f #define MAX_ACC_REF 0.5f
#define MAX_DIST_TRACK 0.1f #define MAX_DIST_TRACK 0.1f
#define MAX_VEL_TRACK 0.5f #define MAX_VEL_TRACK 0.5f