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

修复关节复位后无法恢复的bug

Browse Source
This commit is contained in:
kai
2024-04-03 20:26:25 +08:00
parent 994e726a7a
commit 9f97a793ce
4 changed files with 31 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

51
application/chassis/balance.c
View File

@@ -58,7 +58,7 @@ void BalanceInit()
.can_handle = &hcan1},
.controller_param_init_config = {
.angle_PID = {
.Kp = 0.2,
.Kp = 0.3,
.Kd = 0,
.Ki = 0,
.DeadBand = 0.0001,
@@ -191,7 +191,8 @@ static void ControlSwitch()
if (switch_is_up(rc_data->rc.switch_left))
{
chassis_cmd_recv.chassis_mode = CHASSIS_RESET;
chassis_cmd_recv.vx = 0.05 * (float)rc_data[TEMP].rc.rocker_r1; // speed x, unit m/s
chassis_cmd_recv.vx = 0.5 * (float)rc_data[TEMP].rc.rocker_r1; // speed x, unit m/s
chassis_cmd_recv.rotate_w = 0.5 * (float)rc_data[TEMP].rc.rocker_r_;
}
else
{
@@ -214,23 +215,25 @@ static void ResetChassis()
// 复位时清空距离和腿长积累量,保证顺利站起
chassis.dist = chassis.target_dist = 0;
l_side.target_len = r_side.target_len = 0.12;
// 角度输入为当前角度
chassis_cmd_recv.offset_angle = chassis.target_yaw = chassis.yaw;
// 撞墙时前后移动保证能重新站立,执行速度输入
LKMotorSetRef(l_driven, chassis_cmd_recv.vx * 2);
LKMotorSetRef(r_driven, -chassis_cmd_recv.vx * 2);
LKMotorSetRef(l_driven, chassis_cmd_recv.vx + chassis_cmd_recv.rotate_w);
LKMotorSetRef(r_driven, -chassis_cmd_recv.vx + chassis_cmd_recv.rotate_w);
// 若关节完成复位,进入ready态
if (abs(lf->measure.total_angle) < 0.05 && abs(lf->measure.total_angle) > 0.02 &&
abs(lb->measure.total_angle) < 0.05 && abs(lb->measure.total_angle) > 0.02 &&
abs(rf->measure.total_angle) < 0.05 && abs(rf->measure.total_angle) > 0.02 &&
abs(rb->measure.total_angle) < 0.05 && abs(rb->measure.total_angle) > 0.02)
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)
{
chassis_status = ROBOT_READY; // 底盘已经准备好重新站立
}
else if (abs(lf->measure.total_angle) <= 0.02 &&
abs(lb->measure.total_angle) <= 0.02 &&
abs(rf->measure.total_angle) <= 0.02 &&
abs(rb->measure.total_angle) <= 0.02)
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)
{ // 双阈值保证关节能够复位而不会进入死区
chassis_status = ROBOT_READY; // 底盘已经准备好重新站立
@@ -261,6 +264,12 @@ static void WokingStateSet()
}
else if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE) // 未收到遥控器和云台指令底盘进入急停
{
// 清空腿长和距离
l_side.target_len = r_side.target_len = 0.12;
chassis.dist = chassis.target_dist = 0;
// 角度输入为当前角度
chassis_cmd_recv.offset_angle = chassis.target_yaw = chassis.yaw;
for (uint8_t i = 0; i < JOINT_CNT; i++)
HTMotorStop(joint[i]);
for (uint8_t i = 0; i < DRIVEN_CNT; i++)
@@ -338,8 +347,8 @@ static void SynthesizeMotion() /* 腿部控制:抗劈叉; 轮子控制:转向 */
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;
}
@@ -376,6 +385,13 @@ 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);
@@ -391,13 +407,6 @@ 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();
}

2
application/chassis/balance.h
View File

@@ -9,8 +9,6 @@
#define BALANCE_GRAVITY_BIAS 0
#define ROLL_GRAVITY_BIAS 0
#define MAX_ACC_REF 0.5f
#define MAX_DIST_TRACK 0.1f
#define MAX_VEL_TRACK 0.5f
// IMU距离中心的距离
#define CENTER_IMU_W 0

3
application/chassis/lqr_calc.h
View File

@@ -25,9 +25,6 @@ static void CalcLQR(LinkNPodParam *p, ChassisParam *chassis)
float l = p->leg_len;
float lsqr = l * l;
// float dist_limit = abs(chassis->target_dist - chassis->dist) > MAX_DIST_TRACK ? sign(chassis->target_dist - chassis->dist) * MAX_DIST_TRACK : (chassis->target_dist - chassis->dist); // todo设置值
// float vel_limit = abs(chassis->target_v - chassis->vel) > MAX_VEL_TRACK ? sign(chassis->target_v - chassis->vel) * MAX_VEL_TRACK : (chassis->target_v - chassis->vel);
for (uint8_t i = 0; i < 2; ++i)
{
uint8_t j = i * 6;