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

修改与轮电机固连杆，使用机体速度计算LQR增益

Browse Source
This commit is contained in:
kai
2024-04-28 16:14:10 +08:00
parent 41e13aee44
commit b7e4c80cb4
8 changed files with 58 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files

59
application/chassis/balance.c
View File

@@ -58,7 +58,7 @@ void BalanceInit()
.can_handle = &hcan1},
.controller_param_init_config = {
.angle_PID = {
.Kp = 0.3,
.Kp = 0.1,
.Kd = 0,
.Ki = 0,
.DeadBand = 0.0001,
@@ -102,9 +102,9 @@ void BalanceInit()
.motor_type = LK9025,
};
driven_conf.can_init_config.tx_id = 1;
driven[RD] = r_driven = LKMotorInit(&driven_conf);
driven_conf.can_init_config.tx_id = 2;
driven[LD] = l_driven = LKMotorInit(&driven_conf);
driven_conf.can_init_config.tx_id = 2;
driven[RD] = r_driven = LKMotorInit(&driven_conf);
// 腿长控制
PID_Init_Config_s leg_length_pid_conf = {
@@ -219,21 +219,21 @@ static void ResetChassis()
chassis_cmd_recv.offset_angle = chassis.target_yaw = chassis.yaw;
// 撞墙时前后移动保证能重新站立,执行速度输入
LKMotorSetRef(l_driven, chassis_cmd_recv.vx + chassis_cmd_recv.rotate_w);
LKMotorSetRef(r_driven, -chassis_cmd_recv.vx + chassis_cmd_recv.rotate_w);
LKMotorSetRef(l_driven, chassis_cmd_recv.vx * 2);
LKMotorSetRef(r_driven, -chassis_cmd_recv.vx * 2);
// 若关节完成复位,进入ready态
if (abs(lf->measure.total_angle) < 0.05 && abs(lf->measure.total_angle) > 0.025 &&
abs(lb->measure.total_angle) < 0.05 && abs(lb->measure.total_angle) > 0.025 &&
abs(rf->measure.total_angle) < 0.05 && abs(rf->measure.total_angle) > 0.025 &&
abs(rb->measure.total_angle) < 0.05 && abs(rb->measure.total_angle) > 0.025)
if (abs(lf->measure.total_angle) < 0.05 && abs(lf->measure.total_angle) > 0.03 &&
abs(lb->measure.total_angle) < 0.05 && abs(lb->measure.total_angle) > 0.03 &&
abs(rf->measure.total_angle) < 0.05 && abs(rf->measure.total_angle) > 0.03 &&
abs(rb->measure.total_angle) < 0.05 && abs(rb->measure.total_angle) > 0.03)
{
chassis_status = ROBOT_READY; // 底盘已经准备好重新站立
}
else if (abs(lf->measure.total_angle) <= 0.025 &&
abs(lb->measure.total_angle) <= 0.025 &&
abs(rf->measure.total_angle) <= 0.025 &&
abs(rb->measure.total_angle) <= 0.025)
else if (abs(lf->measure.total_angle) <= 0.03 &&
abs(lb->measure.total_angle) <= 0.03 &&
abs(rf->measure.total_angle) <= 0.03 &&
abs(rb->measure.total_angle) <= 0.03)
{ // 双阈值保证关节能够复位而不会进入死区
chassis_status = ROBOT_READY; // 底盘已经准备好重新站立
@@ -279,6 +279,9 @@ static void WokingStateSet()
// 运动模式
EnableAllMotor();
// 保证关节电机为开环扭矩控制
for (uint8_t i = 0; i < JOINT_CNT; i++)
HTMotorOuterLoop(joint[i], OPEN_LOOP);
// 设置目标速度/腿长/距离
l_side.target_len += chassis_cmd_recv.delta_leglen;
@@ -297,6 +300,13 @@ static void WokingStateSet()
// 角度输入
chassis.target_yaw = chassis_cmd_recv.offset_angle;
// TODO 转向速度限幅
// TODO 最大dist误差限幅
// TODO 最大速度误差限幅
}
@@ -344,11 +354,12 @@ static void SynthesizeMotion() /* 腿部控制:抗劈叉; 轮子控制:转向 */
l_side.T_wheel -= steer_v_pid.Output;
r_side.T_wheel += steer_v_pid.Output;
// 抗劈叉
static float swerving_speed_ff, ff_coef = 0;
swerving_speed_ff = ff_coef * steer_v_pid.Output; // 用于抗劈叉的前馈
PIDCalculate(&anti_crash_pid, l_side.phi5 - r_side.phi5, 0);
l_side.T_hip += anti_crash_pid.Output + swerving_speed_ff;
r_side.T_hip -= anti_crash_pid.Output + swerving_speed_ff;
l_side.T_hip += anti_crash_pid.Output - swerving_speed_ff;
r_side.T_hip -= anti_crash_pid.Output - swerving_speed_ff;
}
@@ -358,8 +369,8 @@ static void LegControl() /* 腿长控制和Roll补偿 */
l_side.target_len += roll_compensate_pid.Output;
r_side.target_len -= roll_compensate_pid.Output;
static float gravity_comp = 57.63;
static float roll_extra_comp_p = 400;
static float gravity_comp = 60;
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;
@@ -385,13 +396,6 @@ void BalanceTask()
WokingStateSet();
// 参数组装
ParamAssemble();
// stop表示复位尚未完成,reset表明还未切换到其他模式,故都不执行运动模态的代码
if (chassis_status == ROBOT_STOP ||
chassis_cmd_recv.chassis_mode == CHASSIS_RESET ||
chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE)
return; // 复位模态或急停,直接退出
// 将五连杆映射成单杆
Link2Leg(&l_side, &chassis);
Link2Leg(&r_side, &chassis);
@@ -407,6 +411,13 @@ void BalanceTask()
// VMC映射成关节输出
VMCProject(&l_side);
VMCProject(&r_side);
// stop表示复位尚未完成,reset表明还未切换到其他模式,故都不执行运动模态的代码
if (chassis_status == ROBOT_STOP ||
chassis_cmd_recv.chassis_mode == CHASSIS_RESET ||
chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE)
return; // 复位模态或急停,直接退出
// 运动模态,电机输出映射和限幅
WattLimitSet();
}