添加航向控制和抗劈叉

application/chassis/balance.c

@@ -38,6 +38,8 @@ static ChassisParam chassis;
 
 // 综合运动补偿的PID控制器
 static PIDInstance leglen_pid_l, leglen_pid_r;     // 用PD模拟弹簧, 不要积分(弹簧是无积分二阶系统), 增益不可过大否则抗外界冲击响应时太"硬"
+static PIDInstance steer_p_pid, steer_v_pid;       // 转向PID,有转向指令时使用IMU的加速度反馈积分以获取速度和位置状态量
+static PIDInstance anti_crash_pid;                 // 抗劈叉,将输出以相反的方向叠加到左右腿的上
 
 // 底盘状态
 static Robot_Status_e chassis_status;
@@ -118,6 +120,42 @@ void BalanceInit()
     PIDInit(&leglen_pid_l, &leg_length_pid_conf);
     PIDInit(&leglen_pid_r, &leg_length_pid_conf);
 
+    // 航向控制
+    // 角度环
+    PID_Init_Config_s steer_p_pid_conf = {
+        .Kp = 5,
+        .Kd = 0,
+        .Ki = 0.0f,
+        .MaxOut = 4,
+        .DeadBand = 0.001f,
+        .Improve = PID_DerivativeFilter | PID_Derivative_On_Measurement,
+        .Derivative_LPF_RC = 0.05,
+    };
+    PIDInit(&steer_p_pid, &steer_p_pid_conf);
+    // 速度环
+    PID_Init_Config_s steer_v_pid_conf = {
+        .Kp = 3,
+        .Kd = 0.0f,
+        .Ki = 0.0f,
+        .MaxOut = 10,
+        .DeadBand = 0.0f,
+        .Improve = PID_DerivativeFilter | PID_Derivative_On_Measurement,
+        .Derivative_LPF_RC = 0.05,
+    };
+    PIDInit(&steer_v_pid, &steer_v_pid_conf);
+
+    // 抗劈叉
+    PID_Init_Config_s anti_crash_pid_conf = {
+        .Kp = 8,
+        .Kd = 2.5,
+        .Ki = 0.0,
+        .MaxOut = 10,
+        .DeadBand = 0.001f,
+        .Improve = PID_DerivativeFilter | PID_ChangingIntegrationRate | PID_Integral_Limit,
+        .Derivative_LPF_RC = 0.01,
+    };
+    PIDInit(&anti_crash_pid, &anti_crash_pid_conf);
+
     // 状态初始化
     l_side.target_len = r_side.target_len = 0.12;
     chassis.vel_cov = 100;      // 速度协方差初始化
@@ -149,6 +187,7 @@ static void ControlSwitch()
             chassis_cmd_recv.chassis_mode = CHASSIS_FREE_DEBUG;                        // 自由转动&前后
             chassis_cmd_recv.vx = 0.002 * (float)rc_data[TEMP].rc.rocker_r1;           // speed x, unit m/s
             chassis_cmd_recv.delta_leglen = -0.000005f * (float)rc_data[TEMP].rc.dial;
+            chassis_cmd_recv.offset_angle -= 0.00001 * (float)rc_data[TEMP].rc.rocker_r_;
         }
     }
     else
@@ -235,6 +274,9 @@ static void WokingStateSet()
         chassis.target_v += sign(chassis_cmd_recv.vx - chassis.target_v) * MAX_ACC_REF * del_t;
     // 模型距离参考输入
     chassis.target_dist += chassis.target_v * del_t;
+
+    // 角度输入
+    chassis.target_yaw = chassis_cmd_recv.offset_angle; 
 }
 
 
@@ -270,6 +312,26 @@ static void ParamAssemble()
 }
 
 
+static void SynthesizeMotion() /* 腿部控制:抗劈叉; 轮子控制:转向 */
+{
+    if (chassis_cmd_recv.chassis_mode == CHASSIS_FREE_DEBUG)
+    {
+        // 双环控制
+        float p_ref = PIDCalculate(&steer_p_pid, chassis.yaw, chassis.target_yaw);
+        PIDCalculate(&steer_v_pid, chassis.wz, p_ref);
+    }
+
+    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;
+}
+
+
 static void LegControl() /* 腿长控制和Roll补偿 */
 {
     static float gravity_comp = 57.63;
@@ -305,6 +367,8 @@ void BalanceTask()
     // 根据单杆计算处的角度和杆长,计算反馈增益
     CalcLQR(&l_side, &chassis);
     CalcLQR(&r_side, &chassis);
+    // 转向和抗劈叉
+    SynthesizeMotion();
     // 腿长控制,保持机体水平
     LegControl();
     // VMC映射成关节输出