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

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(l_driven, chassis_cmd_recv.vx + chassis_cmd_recv.rotate_w);
-    LKMotorSetRef(r_driven, -chassis_cmd_recv.vx * 2);
+    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 &&
+    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.02 &&
+        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.02 &&
+        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.02)
+        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 &&
+    else if (abs(lf->measure.total_angle) <= 0.025 &&
-             abs(lb->measure.total_angle) <= 0.02 &&
+             abs(lb->measure.total_angle) <= 0.025 &&
-             abs(rf->measure.total_angle) <= 0.02 &&
+             abs(rf->measure.total_angle) <= 0.025 &&
-             abs(rb->measure.total_angle) <= 0.02)
+             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;
+    l_side.T_hip += anti_crash_pid.Output + swerving_speed_ff;
-    r_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();
 }

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

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;

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;