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

添加速度输入, 腿长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},
.controller_param_init_config = {
.angle_PID = {
.Kp = 0.3,
.Kd = 0.1,
.Kp = 0.2,
.Kd = 0,
.Ki = 0,
.DeadBand = 0.0001,
.Improve = PID_DerivativeFilter | PID_Derivative_On_Measurement,
@@ -105,10 +105,10 @@ void BalanceInit()
// 腿长控制
PID_Init_Config_s leg_length_pid_conf = {
.Kp = 600,
.Kd = 150,
.Kp = 800,
.Kd = 300,
.Ki = 0,
.MaxOut = 60,
.MaxOut = 20,
.DeadBand = 0.0001f,
.Improve = PID_ChangingIntegrationRate | PID_Trapezoid_Intergral | PID_DerivativeFilter | PID_Derivative_On_Measurement,
.CoefA = 0.01,
@@ -148,7 +148,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.0000015f * (float)rc_data[TEMP].rc.dial;
chassis_cmd_recv.delta_leglen = -0.000005f * (float)rc_data[TEMP].rc.dial;
}
}
else
@@ -161,6 +161,10 @@ static void ResetChassis()
{
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(r_driven, -chassis_cmd_recv.vx * 2);
@@ -221,8 +225,16 @@ 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.25);
VAL_LIMIT(r_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.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 BALANCE_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_VEL_TRACK 0.5f