添加离地检测

application/chassis/balance.c

@@ -112,10 +112,10 @@ void BalanceInit()
 
     // 腿长控制
     PID_Init_Config_s leg_length_pid_conf = {
-        .Kp = 600,
+        .Kp = 1200,
-        .Kd = 100,
+        .Kd = 150,
         .Ki = 0,
-        .MaxOut = 20,
+        .MaxOut = 60,
         .DeadBand = 0.0001f,
         .Improve = PID_ChangingIntegrationRate | PID_Trapezoid_Intergral | PID_DerivativeFilter | PID_Derivative_On_Measurement,
         .Derivative_LPF_RC = 0.05,
@@ -128,7 +128,7 @@ void BalanceInit()
         .Kp = 0.0008f,
         .Kd = 0.0001f,
         .Ki = 0.0f,
-        .MaxOut = 0.04,
+        .MaxOut = 0.05,
         .DeadBand = 0.001f,
         .Improve = PID_DerivativeFilter | PID_Derivative_On_Measurement,
         .Derivative_LPF_RC = 0.05,
@@ -152,7 +152,7 @@ void BalanceInit()
         .Kp = 3,
         .Kd = 0.0f,
         .Ki = 0.0f,
-        .MaxOut = 10,
+        .MaxOut = 20,
         .DeadBand = 0.0f,
         .Improve = PID_DerivativeFilter | PID_Derivative_On_Measurement,
         .Derivative_LPF_RC = 0.05,
@@ -164,7 +164,7 @@ void BalanceInit()
         .Kp = 15,
         .Kd = 1,
         .Ki = 0.0,
-        .MaxOut = 10,
+        .MaxOut = 30,
         .DeadBand = 0.001f,
         .Improve = PID_DerivativeFilter | PID_ChangingIntegrationRate | PID_Integral_Limit,
         .Derivative_LPF_RC = 0.01,
@@ -212,7 +212,7 @@ static void ControlSwitch()
         {
             chassis_cmd_recv.chassis_mode = CHASSIS_FREE_DEBUG;                        // 自由转动&前后
             chassis_cmd_recv.vx = 0.003 * (float)rc_data[TEMP].rc.rocker_r1;           // speed x, unit m/s
-            chassis_cmd_recv.delta_leglen = -0.000001f * (float)rc_data[TEMP].rc.dial;
+            chassis_cmd_recv.delta_leglen = -0.0000005f * (float)rc_data[TEMP].rc.dial;
             chassis_cmd_recv.offset_angle -= 0.000005 * (float)rc_data[TEMP].rc.rocker_r_;
         }
     }
@@ -231,6 +231,8 @@ static void ResetChassis()
     l_side.target_len = r_side.target_len = 0.12;
     // 角度输入为当前角度
     chassis_cmd_recv.offset_angle = chassis.target_yaw = chassis.yaw;
+    // 驱动轮支持力为定值
+    l_side.normal_force = r_side.normal_force = 100.0f;
 
     // 撞墙时前后移动保证能重新站立,执行速度输入
     LKMotorSetRef(l_driven, chassis_cmd_recv.vx + chassis_cmd_recv.rotate_w);
@@ -283,6 +285,8 @@ static void WokingStateSet()
         chassis.dist = chassis.target_dist = 0;
         // 角度输入为当前角度
         chassis_cmd_recv.offset_angle = chassis.target_yaw = chassis.yaw;
+        // 驱动轮支持力为定值
+        l_side.normal_force = r_side.normal_force = 100.0f;
 
         for (uint8_t i = 0; i < JOINT_CNT; i++)
             HTMotorStop(joint[i]);
@@ -379,8 +383,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 = 80;
+    static float gravity_comp = 60;
-    static float roll_extra_comp_p = 300;
+    static float roll_extra_comp_p = 400;
     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;
@@ -421,15 +425,18 @@ void BalanceTask()
     // VMC映射成关节输出
     VMCProject(&l_side);
     VMCProject(&r_side);
-    // 驱动轮支持力解算
-    NormalForceSolve(&l_side, Chassis_IMU_data, del_t);
-    NormalForceSolve(&r_side, Chassis_IMU_data, del_t);
 
     // stop表示复位尚未完成,reset表明还未切换到其他模式,故都不执行运动模态的代码
     if (chassis_status == ROBOT_STOP ||
         chassis_cmd_recv.chassis_mode == CHASSIS_RESET ||
         chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE)
         return; // 复位模态或急停,直接退出
+    else
+    {
+        // 正常模式下再进行驱动轮支持力解算
+        NormalForceSolve(&l_side, Chassis_IMU_data, del_t);
+        NormalForceSolve(&r_side, Chassis_IMU_data, del_t);
+    }
 
     // 运动模态,电机输出映射和限幅
     WattLimitSet();

application/chassis/lqr_calc.h

@@ -25,6 +25,23 @@ static void CalcLQR(LinkNPodParam *p, ChassisParam *chassis)
     float l = p->leg_len;
     float lsqr = l * l;
 
+    // 离地检测
+    if (p->normal_force < 20.0f)
+    {
+        for (size_t i = 0; i < 12; i++)
+        {
+            // 除 theta 和 theta_dot 的关节输出外，其余增益全部置0
+            if(i != 6 && i != 7)
+            {
+                for (size_t j = 0; j < 3; j++)
+                {
+                    k[i][j] = 0;
+                }
+            }
+        }
+    }
+    
+    // 计算增益
     for (uint8_t i = 0; i < 2; ++i)
     {
         uint8_t j = i * 6;