添加增益矩阵k

application/chassis/balance.c

@@ -16,6 +16,7 @@
 #include "arm_math.h" // 需要用到较多三角函数
 #include "bsp_dwt.h"
 #include "bsp_log.h"
+#include "lqr_calc.h"
 
 // 计时变量
 static uint32_t balance_dwt_cnt;
@@ -109,7 +110,7 @@ static void ControlSwitch()
         if (rc_data->rc.rocker_l1 < -600)
         {
             chassis_cmd_recv.chassis_mode = CHASSIS_RESET;
-            chassis_cmd_recv.vx = 0.5 * (float)rc_data[TEMP].rc.rocker_r1; // speed x, unit m/s
+            chassis_cmd_recv.vx = 0.005 * (float)rc_data[TEMP].rc.rocker_r1; // speed x, unit m/s
         }
         else
         {
@@ -122,6 +123,23 @@ static void ControlSwitch()
 }
 
 
+// 工作状态设定
+static void WokingStateSet()
+{
+    if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE) // 未收到遥控器和云台指令底盘进入急停
+    {
+        for (uint8_t i = 0; i < JOINT_CNT; i++)
+            HTMotorStop(joint[i]);
+        for (uint8_t i = 0; i < DRIVEN_CNT; i++)
+            LKMotorStop(driven[i]);
+        return; // 关闭所有电机,发送的指令为零
+    }
+
+    // 运动模式
+    EnableAllMotor();
+}
+
+
 /**
  * @brief 将电机和imu的数据组装为LinkNPodParam结构体和chassisParam结构体
  *
@@ -153,6 +171,11 @@ static void ParamAssemble()
     r_side.w_ecd = -r_driven->measure.speed_rads;
 }
 
+static void WattLimitSet() /* 设定运动模态的输出 */
+{
+    LKMotorSetRef(l_driven, 195.3125 * l_side.T_wheel);
+    LKMotorSetRef(r_driven, 195.3125 * -r_side.T_wheel);
+}
 
 void BalanceTask()
 {
@@ -160,9 +183,17 @@ void BalanceTask()
 
     // 切换遥控器控制or云台板控制
     ControlSwitch();
+    // 设置目标参数和工作模式
+    WokingStateSet();
     // 参数组装
     ParamAssemble();
     // 将五连杆映射成单杆
     Link2Leg(&l_side, &chassis);
     Link2Leg(&r_side, &chassis);
+    // 根据单杆计算处的角度和杆长,计算反馈增益
+    CalcLQR(&l_side, &chassis);
+    CalcLQR(&r_side, &chassis);
+
+    // 运动模态,电机输出映射和限幅
+    WattLimitSet();
 }

application/chassis/lqr_calc.h

@@ -9,7 +9,18 @@
  */
 static void CalcLQR(LinkNPodParam *p, ChassisParam *chassis)
 {
-    float k[12][3] = {0};
+    float k[12][3] = {85.842511,-168.176021,-4.383994,
+                        -8.011354,-27.544550,0.946791,
+                        40.658824,-34.245857,-13.443024,
+                        33.057883,-37.645803,-8.872245,
+                        186.414265,-182.258244,59.110130,
+                        12.371459,-12.979501,4.575691,
+                        -2.044517,-13.973090,28.443243,
+                        -8.073717,8.553946,2.899594,
+                        109.977348,-109.218563,37.018016,
+                        67.432105,-68.632617,25.883284,
+                        -210.173934,175.310058,96.350481,
+                        -15.827060,13.395116,3.049615};
     float T[2] = {0};       // 0 T_wheel  1 T_hip
     float l = p->leg_len;
     float lsqr = l * l;