添加增益矩阵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();
 }