搭建了chassis的框架

.vscode/settings.json

@@ -29,7 +29,8 @@
         "remote_control.h": "c",
         "gimbal_cmd.h": "c",
         "chassis_cmd.h": "c",
-        "referee.h": "c"
+        "referee.h": "c",
+        "arm_math.h": "c"
     },
     "C_Cpp.default.configurationProvider": "ms-vscode.makefile-tools",
 }

Makefile

@@ -22,7 +22,8 @@ TARGET = basic_framework
 # debug build?
 DEBUG = 1
 # optimization
-OPT = -Og
+OPT = -O0 # O0避免没有使用到的变量被优化,如果没有特殊的调试需求请修改成-Og.
+          # 为了更高的性能,正式上车不需要调试时修改成-O3
 
 
 #######################################

application/chassis/chassis.c

@@ -3,6 +3,13 @@
 #include "dji_motor.h"
 #include "super_cap.h"
 #include "message_center.h"
+#include "arm_math.h"
+
+#define OFFSET_X_CENTER  //纵向轮距(前进后退方向)
+#define OFFSET_Y_CENTER  //横向轮距(左右平移方向)
+#define RADIUS_WHEEL     //轮子半径
+#define PERIMETER_WHEEL
+#define REDUCTION_RATIO 19
 
 /* 底盘应用包含的模块和信息存储 */
 #ifdef CHASSIS_BOARD // 使用板载IMU获取底盘转动角速度
@@ -16,15 +23,13 @@ static dji_motor_instance *lb;
 static dji_motor_instance *rb;
 
 static Publisher_t* chassis_pub;
-static Chassis_Upload_Data_s chassis_feedback_data;
-static Subscriber_t* chassis_sub;
 static Chassis_Ctrl_Cmd_s chassis_cmd_recv;
+static Subscriber_t* chassis_sub;
+static Chassis_Upload_Data_s chassis_feedback_data;
 
+// 将云台系的速度投影到底盘
+static float chassis_vx,chassis_vy; 
 
-static void mecanum_calculate()
-{
-
-}
 
 void ChassisInit()
 {
@@ -144,7 +149,63 @@ void ChassisInit()
     chassis_pub=PubRegister("chassis_feed",sizeof(Chassis_Upload_Data_s));
 }
 
-void ChassisTask()
+
+/**
+ * @brief 计算每个轮毂电机的输出,正运动学解算
+ * 
+ */
+static void MecanumCalculate()
 {
 
 }
+
+/**
+ * @brief 根据每个轮子的速度反馈,计算底盘的实际运动速度,逆运动解算
+ * 
+ */
+static void EstimateSpeed()
+{
+
+}
+// chassis_cmd_recv chassis_feedback_data
+void ChassisTask()
+{
+    // 后续增加没收到消息的处理
+    // 获取新的控制信息
+    SubGetMessage(chassis_sub,&chassis_cmd_recv);
+
+    if(chassis_cmd_recv.chassis_cmd.chassis_mode==CHASSIS_ZERO_FORCE)
+    {
+        DJIMotorStop();
+    }
+
+    // 根据云台和底盘的角度offset将控制量映射到底盘坐标系上
+    chassis_vx=chassis_cmd_recv.chassis_cmd.vx*arm_cos_f32(chassis_cmd_recv.chassis_cmd.offset_angle)-
+               chassis_cmd_recv.chassis_cmd.vy*arm_sin_f32(chassis_cmd_recv.chassis_cmd.offset_angle);
+    chassis_vy=chassis_cmd_recv.chassis_cmd.vx*arm_sin_f32(chassis_cmd_recv.chassis_cmd.offset_angle)-
+               chassis_cmd_recv.chassis_cmd.vy*arm_cos_f32(chassis_cmd_recv.chassis_cmd.offset_angle);
+
+    // 根据控制模式设定旋转速度
+    switch (chassis_cmd_recv.chassis_cmd.chassis_mode)
+    {
+    case CHASSIS_NO_FOLLOW:
+        chassis_cmd_recv.chassis_cmd.wz=0;
+        break;
+    case CHASSIS_FOLLOW_GIMBAL_YAW:
+        chassis_cmd_recv.chassis_cmd.wz=0.05f*powf(chassis_cmd_recv.chassis_cmd.wz,2.0f); // 不开pid,以误差角平方为输出
+        break;
+    case CHASSIS_ROTATE:
+        //chassis_cmd_recv.chassis_cmd.wz 当前维持定值,后续增加不规则的变速策略
+        break;
+    default:
+        break;
+    }
+
+    //根据控制模式进行正运动学解算,计算底盘输出
+    MecanumCalculate();
+
+    //根据电机的反馈速度计算
+    EstimateSpeed();
+
+    PubPushMessage(chassis_pub,&chassis_feedback_data);
+}

application/cmd/chassis_cmd.c

@@ -34,5 +34,7 @@ void ChassisCMDInit()
 
 void ChassisCMDTask()
 {
+    SubGetMessage(chassis_feed_sub,&chassis_fetch_data);
+    SubGetMessage(gimbal2chassis_sub,&data_from_gimbal_cmd);
     
 }

application/gimbal/gimbal.c

@@ -5,6 +5,8 @@
 #include "message_center.h"
 
 
+#define YAW_ALIGN_ECD 0
+
 static attitude_t* Gimbal_IMU_data;                // 云台IMU数据
 static dji_motor_instance* yaw_motor;                    // yaw电机
 static dji_motor_instance *pitch_motor;                  // pitch电机

modules/message_center/message_center.c

@@ -178,7 +178,7 @@ uint8_t SubGetMessage(Subscriber_t *sub, void *data_ptr)
     return 1;
 }
 
-void PubPushMessage(Publisher_t *pub, void *data_ptr)
+uint8_t PubPushMessage(Publisher_t *pub, void *data_ptr)
 {
     Subscriber_t *iter = pub->first_subs; // iter作为订阅者指针,遍历订阅该事件的所有订阅者;如果为空说明遍历结束
     while (iter)                          // 遍历订阅了当前事件的所有订阅者,依次填入最新消息
@@ -195,4 +195,5 @@ void PubPushMessage(Publisher_t *pub, void *data_ptr)
 
         iter = iter->next_subs_queue; // 访问下一个订阅者
     }
+    return 1;
 }

modules/message_center/message_center.h

@@ -109,7 +109,8 @@ uint8_t SubGetMessage(Subscriber_t* sub,void* data_ptr);
  * 
  * @param pub 发布者实例指针
  * @param data_ptr 指向要发布的数据的指针
+ * @return uint8_t 新消息成功推送给几个订阅者
  */
-void PubPushMessage(Publisher_t* pub,void* data_ptr);
+uint8_t PubPushMessage(Publisher_t* pub,void* data_ptr);
 
 

modules/motor/dji_motor.c

@@ -4,6 +4,7 @@
 #define ECD_ANGLE_COEF 0.043945f // 360/8192
 
 static uint8_t idx = 0; // register idx,是该文件的全局电机索引,在注册时使用
+static uint8_t stop_flag=0;
 
 /* DJI电机的实例,此处仅保存指针,内存的分配将通过电机实例初始化时通过malloc()进行 */
 static dji_motor_instance *dji_motor_info[DJI_MOTOR_CNT] = {NULL};
@@ -256,7 +257,16 @@ void DJIMotorControl()
     {
         if (sender_enable_flag[i])
         {
+            if(stop_flag) // 如果紧急停止,将所有发送信息置零
+            {
+                memset(sender_assignment[i].tx_buff,0,8);
+            }
             CANTransmit(&sender_assignment[i]);
         }
     }
 }
+
+void DJIMotorStop()
+{
+    stop_flag=1;
+}

modules/motor/dji_motor.h

@@ -106,4 +106,10 @@ void DJIMotorChangeFeed(dji_motor_instance *motor, Closeloop_Type_e loop, Feedba
 void DJIMotorControl();
 
 
+/**
+ * @brief 停止所有电机
+ * 
+ */
+void DJIMotorStop();
+
 #endif // !DJI_MOTOR_H