为电机增加了启动和停止模式,完成了gimbal的框架,模式过渡尚未完成

application/chassis/chassis.c

@@ -207,19 +207,6 @@ void ChassisTask()
     // 获取新的控制信息
     SubGetMessage(chassis_sub, &chassis_cmd_recv);
 
-    // 如果出现重要模块离线或遥控器设置为急停,让电机停止
-    if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE)
-    {
-        DJIMotorStop();
-    }
-
-    // 根据云台和底盘的角度offset将控制量映射到底盘坐标系上
-    // 底盘逆时针旋转为角度正方向;云台命令的方向以云台指向的方向为x,采用右手系(x指向正北时y在正东)
-    chassis_vx = chassis_cmd_recv.vx * arm_cos_f32(chassis_cmd_recv.offset_angle) -
-                 chassis_cmd_recv.vy * arm_sin_f32(chassis_cmd_recv.offset_angle);
-    chassis_vy = chassis_cmd_recv.vx * arm_sin_f32(chassis_cmd_recv.offset_angle) -
-                 chassis_cmd_recv.vy * arm_cos_f32(chassis_cmd_recv.offset_angle);
-
     // 根据控制模式设定旋转速度
     switch (chassis_cmd_recv.chassis_mode)
     {
@@ -232,10 +219,20 @@ void ChassisTask()
     case CHASSIS_ROTATE:
         // chassis_cmd_recv.wz // 当前维持定值,后续增加不规则的变速策略
         break;
+    case CHASSIS_ZERO_FORCE:
+        DJIMotorStop(); // 如果出现重要模块离线或遥控器设置为急停,让电机停止
+        break;
     default:
         break;
     }
 
+    // 根据云台和底盘的角度offset将控制量映射到底盘坐标系上
+    // 底盘逆时针旋转为角度正方向;云台命令的方向以云台指向的方向为x,采用右手系(x指向正北时y在正东)
+    chassis_vx = chassis_cmd_recv.vx * arm_cos_f32(chassis_cmd_recv.offset_angle) -
+                 chassis_cmd_recv.vy * arm_sin_f32(chassis_cmd_recv.offset_angle);
+    chassis_vy = chassis_cmd_recv.vx * arm_sin_f32(chassis_cmd_recv.offset_angle) -
+                 chassis_cmd_recv.vy * arm_cos_f32(chassis_cmd_recv.offset_angle);
+
     // 根据控制模式进行正运动学解算,计算底盘输出
     MecanumCalculate();
 

application/gimbal/gimbal.c

@@ -4,22 +4,22 @@
 #include "ins_task.h"
 #include "message_center.h"
 
+// 需要将控制值统一修改为角度制而不是编码器ECD值
+#define YAW_ALIGN_ECD 0 // 云台和底盘对齐指向相同方向时的电机编码器值
+#define PITCH_HORIZON_ECD 0 // 云台处于水平位置时编码器值
 
-#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电机
 
-static attitude_t* Gimbal_IMU_data;                // 云台IMU数据
-static dji_motor_instance* yaw_motor;                    // yaw电机
-static dji_motor_instance *pitch_motor;                  // pitch电机
-
-static Publisher_t* gimbal_pub;
+static Publisher_t *gimbal_pub;
 static Gimbal_Upload_Data_s gimbal_feedback_data; // 回传给gimbal_cmd的云台状态信息
-static Subscriber_t* gimbal_sub;
-static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv;         // 来自gimbal_cmd的控制信息
-
+static Subscriber_t *gimbal_sub;
+static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv; // 来自gimbal_cmd的控制信息
 
 void GimbalInit()
 {
-    Gimbal_IMU_data=INS_Init();
+    Gimbal_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源
 
     // YAW
     Motor_Init_Config_s yaw_config = {
@@ -36,7 +36,9 @@ void GimbalInit()
             .speed_PID = {
 
             },
-            .other_angle_feedback_ptr=&Gimbal_IMU_data->YawTotalAngle,
+            .other_angle_feedback_ptr = &Gimbal_IMU_data->YawTotalAngle,
+            // 还需要增加角速度额外反馈指针
+            // .other_speed_feedback_ptr=&Gimbal_IMU_data.wz;
         },
         .controller_setting_init_config = {
             .angle_feedback_source = MOTOR_FEED,
@@ -60,6 +62,9 @@ void GimbalInit()
             .speed_PID = {
 
             },
+            .other_angle_feedback_ptr = &Gimbal_IMU_data->Pitch,
+            // 还需要增加角速度额外反馈指针
+            // .other_speed_feedback_ptr=&Gimbal_IMU_data.wy,
         },
         .controller_setting_init_config = {
             .angle_feedback_source = MOTOR_FEED,
@@ -69,13 +74,74 @@ void GimbalInit()
         },
         .motor_type = GM6020};
 
-        yaw_motor=DJIMotorInit(&yaw_config);
-        pitch_motor=DJIMotorInit(&pitch_config);
+    yaw_motor = DJIMotorInit(&yaw_config);
+    pitch_motor = DJIMotorInit(&pitch_config);
 
-        gimbal_pub=PubRegister("gimbal_feed",sizeof(Gimbal_Upload_Data_s));
-        gimbal_sub=SubRegister("gimbal_cmd",sizeof(Gimbal_Ctrl_Cmd_s));
+    gimbal_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));
+    gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));
 }
 
+// /**
+//  * @brief 
+//  * 
+//  */
+// static void TransitionMode()
+// {
+
+// }
+
 void GimbalTask()
 {
+    // 获取云台控制数据
+    // 后续增加未收到数据的处理
+    SubGetMessage(gimbal_sub, &gimbal_cmd_recv);
+
+    // 根据控制模式进行电机反馈切换和过渡,视觉模式在robot_cmd模块就已经设置好,gimbal只看yaw_ref和pitch_ref
+    //  是否要增加不同模式之间的过渡?
+    switch (gimbal_cmd_recv.gimbal_mode)
+    {
+    case GIMBAL_ZERO_FORCE:
+        DJIMotorStop();
+        break;
+    case GIMBAL_GYRO_MODE:
+        DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, OTHER_FEED);
+        DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, OTHER_FEED);
+        DJIMotorChangeFeed(pitch_motor, ANGLE_LOOP, OTHER_FEED);
+        DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, OTHER_FEED);
+        DJIMotorSetRef(yaw_motor,gimbal_cmd_recv.yaw);
+        DJIMotorSetRef(pitch_motor,gimbal_cmd_recv.pitch);
+        break;
+    //是否考虑直接让云台和底盘重合,其他模式都使用陀螺仪反馈?
+    case GIMBAL_FREE_MODE:
+        DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, MOTOR_FEED);
+        DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, MOTOR_FEED);
+        DJIMotorChangeFeed(pitch_motor, ANGLE_LOOP, MOTOR_FEED);
+        DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, MOTOR_FEED);
+        DJIMotorSetRef(yaw_motor,YAW_ALIGN_ECD);
+        DJIMotorSetRef(pitch_motor,PITCH_HORIZON_ECD);
+        break;
+    default:
+        break;
+    }
+    // 过渡示例:
+    /* 需要给每个case增加如下判断,并添加一个过渡行为函数和过渡标志位
+    case xxx:
+        if(last_mode!=xxx)
+        {
+            transition_flag=1;
+        }
+        break;
+
+    void TransitMode()
+    {
+        motor_output=lpf_coef * last_output+(1 - lpf_coef) * concur_output;
+    }
+    */
+
+    // 获取反馈数据
+    gimbal_feedback_data.gimbal_imu_data=*Gimbal_IMU_data;
+    gimbal_feedback_data.yaw_motor_ecd=pitch_motor->motor_measure.ecd;
+    
+    // 推送消息
+    PubPushMessage(gimbal_pub,&gimbal_feedback_data);
 }

application/robot_def.h

@@ -53,6 +53,10 @@ typedef enum
 } App_Status_e;
 
 // 底盘模式设置
+/**
+ * @brief 后续考虑修改为云台跟随底盘,而不是让底盘去追云台,云台的惯量比底盘小.
+ * 
+ */
 typedef enum
 {
     CHASSIS_ZERO_FORCE,        // 电流零输入
@@ -65,9 +69,8 @@ typedef enum
 typedef enum
 {
     GIMBAL_ZERO_FORCE,  // 电流零输入
-    GIMBAL_FREE_MODE,   // 云台自由运动模式，反馈值为电机total_angle
-    GIMBAL_GYRO_MODE,   // 云台陀螺仪反馈模式，反馈值为陀螺仪pitch,total_yaw_angle
-    GIMBAL_VISUAL_MODE, // 视觉模式，反馈值为陀螺仪，输入为视觉数据
+    GIMBAL_FREE_MODE,   // 云台自由运动模式,即与底盘分离(底盘此时应为NO_FOLLOW)反馈值为电机total_angle
+    GIMBAL_GYRO_MODE,   // 云台陀螺仪反馈模式,反馈值为陀螺仪pitch,total_yaw_angle,底盘可以为小陀螺和跟随模式
 } gimbal_mode_e;
 
 // 发射模式设置
@@ -173,6 +176,7 @@ typedef struct
 typedef struct
 {
     attitude_t gimbal_imu_data;
+    uint16_t yaw_motor_ecd;
 } Gimbal_Upload_Data_s;
 
 typedef struct