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

离地时进行速度闭环，基本实现稳定飞坡

Browse Source
This commit is contained in:
kai
2024-05-03 23:22:05 +08:00
parent 69e228ace0
commit 7fee0c15c3
4 changed files with 50 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
application/chassis/balance.c
View File

@@ -113,7 +113,7 @@ void BalanceInit()
// 腿长控制
PID_Init_Config_s leg_length_pid_conf = {
.Kp = 1200,
.Kd = 150,
.Kd = 200,
.Ki = 0,
.MaxOut = 60,
.DeadBand = 0.0001f,
@@ -173,6 +173,7 @@ void BalanceInit()
// 状态初始化
l_side.target_len = r_side.target_len = 0.12;
l_side.gravity_ff = r_side.gravity_ff = 60.0f;
chassis.vel_cov = 100; // 速度协方差初始化
chassis_status = ROBOT_READY;
DWT_GetDeltaT(&balance_dwt_cnt);
@@ -231,8 +232,6 @@ static void ResetChassis()
l_side.target_len = r_side.target_len = 0.12;
// 角度输入为当前角度
chassis_cmd_recv.offset_angle = chassis.target_yaw = chassis.yaw;
// 驱动轮支持力为定值
l_side.normal_force = r_side.normal_force = 100.0f;
// 撞墙时前后移动保证能重新站立,执行速度输入
LKMotorSetRef(l_driven, chassis_cmd_recv.vx + chassis_cmd_recv.rotate_w);
@@ -285,8 +284,6 @@ static void WokingStateSet()
chassis.dist = chassis.target_dist = 0;
// 角度输入为当前角度
chassis_cmd_recv.offset_angle = chassis.target_yaw = chassis.yaw;
// 驱动轮支持力为定值
l_side.normal_force = r_side.normal_force = 100.0f;
for (uint8_t i = 0; i < JOINT_CNT; i++)
HTMotorStop(joint[i]);
@@ -383,11 +380,10 @@ static void LegControl() /* 腿长控制和Roll补偿 */
l_side.target_len += roll_compensate_pid.Output;
r_side.target_len -= roll_compensate_pid.Output;
static float gravity_comp = 60;
static float roll_extra_comp_p = 400;
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;
l_side.F_leg = PIDCalculate(&leglen_pid_l, l_side.height, l_side.target_len) + l_side.gravity_ff - roll_comp;
r_side.F_leg = PIDCalculate(&leglen_pid_r, r_side.height, r_side.target_len) + r_side.gravity_ff + roll_comp;
}
static void WattLimitSet() /* 设定运动模态的输出 */
@@ -425,18 +421,15 @@ void BalanceTask()
// VMC映射成关节输出
VMCProject(&l_side);
VMCProject(&r_side);
// 驱动轮支持力解算
NormalForceSolve(&l_side, Chassis_IMU_data);
NormalForceSolve(&r_side, Chassis_IMU_data);
// stop表示复位尚未完成,reset表明还未切换到其他模式,故都不执行运动模态的代码
if (chassis_status == ROBOT_STOP ||
chassis_cmd_recv.chassis_mode == CHASSIS_RESET ||
chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE)
return; // 复位模态或急停,直接退出
else
{
// 正常模式下再进行驱动轮支持力解算
NormalForceSolve(&l_side, Chassis_IMU_data, del_t);
NormalForceSolve(&r_side, Chassis_IMU_data, del_t);
}
// 运动模态,电机输出映射和限幅
WattLimitSet();