添加参数组装

application/chassis/balance.c

@@ -82,10 +82,10 @@ void BalanceInit()
         },
         .motor_type = LK9025,
     };
-    driven_conf.can_init_config.tx_id = 2;
-    driven[LD] = l_driven = LKMotorInit(&driven_conf);
     driven_conf.can_init_config.tx_id = 1;
     driven[RD] = r_driven = LKMotorInit(&driven_conf);
+    driven_conf.can_init_config.tx_id = 2;
+    driven[LD] = l_driven = LKMotorInit(&driven_conf);
 
     // 状态初始化
     chassis_status = ROBOT_READY;
@@ -100,7 +100,69 @@ static void EnableAllMotor() /* 打开所有电机 */
         LKMotorEnable(driven[i]);
 }
 
+/* 切换底盘遥控器控制和云台双板控制 */
+static void ControlSwitch()
+{ 
+    // 右侧拨杆向下,进入遥控器底盘控制,此时不响应云台控制指令
+    if (switch_is_down(rc_data->rc.switch_right) && RemoteControlIsOnline())
+    {
+        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
+        }
+        else
+        {
+            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
+        }
+    }
+    else
+        chassis_cmd_recv.chassis_mode = CHASSIS_ZERO_FORCE; // 皆离线,急停
+}
+
+
+/**
+ * @brief 将电机和imu的数据组装为LinkNPodParam结构体和chassisParam结构体
+ *
+ * @note HT04电机上电的编码器位置为零(校准过),请看Link2Pod()的note,以及HT04.c中的电机解码部分
+ * @note 海泰04电机顺时针旋转为正; LK9025电机逆时针旋转为正,此处皆需要转换为模型中给定的正方向
+ *
+ */
+static void ParamAssemble()
+{ 
+    // 机体参数,视为平面刚体
+    chassis.pitch = Chassis_IMU_data->Pitch * DEGREE_2_RAD;
+    chassis.pitch_w = Chassis_IMU_data->Gyro[0];
+    chassis.yaw = Chassis_IMU_data->YawTotalAngle * DEGREE_2_RAD;
+    chassis.wz = Chassis_IMU_data->Gyro[2];
+    chassis.roll = Chassis_IMU_data->Roll * DEGREE_2_RAD;
+    chassis.roll_w = Chassis_IMU_data->Gyro[1];
+
+    // HT04电机的角度是顺时针为正,LK9025电机的角度是逆时针为正
+    l_side.phi1 = PI + LIMIT_LINK_RAD - lb->measure.total_angle;
+    l_side.phi1_w = -lb->measure.speed_rads;
+    l_side.phi4 = -lf->measure.total_angle - LIMIT_LINK_RAD;
+    l_side.phi4_w = -lf->measure.speed_rads;
+    l_side.w_ecd = l_driven->measure.speed_rads;
+
+    r_side.phi1 = PI + LIMIT_LINK_RAD + rb->measure.total_angle;
+    r_side.phi1_w = rb->measure.speed_rads;
+    r_side.phi4 = rf->measure.total_angle - LIMIT_LINK_RAD;
+    r_side.phi4_w = rf->measure.speed_rads;
+    r_side.w_ecd = -r_driven->measure.speed_rads;
+}
+
+
 void BalanceTask()
 {
+    del_t = DWT_GetDeltaT(&balance_dwt_cnt);
 
+    // 切换遥控器控制or云台板控制
+    ControlSwitch();
+    // 参数组装
+    ParamAssemble();
+    // 将五连杆映射成单杆
+    Link2Leg(&l_side, &chassis);
+    Link2Leg(&r_side, &chassis);
 }

application/chassis/balance.h

@@ -12,9 +12,9 @@
 #define MAX_DIST_TRACK 0.1f
 #define MAX_VEL_TRACK 0.5f
 
-#define CENTER_IMU_R 0.09435f       // IMU距离中心的距离
+#define CENTER_IMU_R 0.09f       // IMU距离中心的距离
 #define CENTER_IMU_W 0
-#define CENTER_IMU_L 0.09435f
+#define CENTER_IMU_L 0.09f
 #define CENTER_IMU_H 0
 
 #define VEL_PROCESS_NOISE 25  // 速度过程噪声
@@ -32,8 +32,8 @@
 #define RB 3u
 
 #define DRIVEN_CNT 2u
-#define LD 0u
-#define RD 1u
+#define RD 0u
+#define LD 1u
 
 typedef struct
 {