From 9f97a793cee3fd35f2551d38e0f1364c1871393e Mon Sep 17 00:00:00 2001 From: kai <1797003616@qq.com> Date: Wed, 3 Apr 2024 20:26:25 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E5=A4=8D=E5=85=B3=E8=8A=82=E5=A4=8D?= =?UTF-8?q?=E4=BD=8D=E5=90=8E=E6=97=A0=E6=B3=95=E6=81=A2=E5=A4=8D=E7=9A=84?= =?UTF-8?q?bug?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- application/chassis/balance.c | 51 ++++++++++++++++++++-------------- application/chassis/balance.h | 2 -- application/chassis/lqr_calc.h | 3 -- application/robot_def.h | 1 + 4 files changed, 31 insertions(+), 26 deletions(-) diff --git a/application/chassis/balance.c b/application/chassis/balance.c index 14244ac..291de3b 100644 --- a/application/chassis/balance.c +++ b/application/chassis/balance.c @@ -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(); } \ No newline at end of file diff --git a/application/chassis/balance.h b/application/chassis/balance.h index d32a1d8..16f58f4 100644 --- a/application/chassis/balance.h +++ b/application/chassis/balance.h @@ -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 diff --git a/application/chassis/lqr_calc.h b/application/chassis/lqr_calc.h index 16e2a48..394d1cb 100644 --- a/application/chassis/lqr_calc.h +++ b/application/chassis/lqr_calc.h @@ -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; diff --git a/application/robot_def.h b/application/robot_def.h index 6dec8d4..9c43fe7 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -141,6 +141,7 @@ typedef struct { // 控制部分 float vx; // 前进方向速度 + float rotate_w; // 旋转速度, 目前仅在复位模式下使用 float delta_leglen; // 腿长 float offset_angle; // 底盘和归中位置的夹角 chassis_mode_e chassis_mode;