修复了djimotor发送时的致命错误,编写了shoot app的框架

application/chassis/chassis.c

@@ -26,7 +26,7 @@
 #define CENTER_GIMBAL_OFFSET_X 0 // 云台旋转中心距底盘几何中心的距离,前后方向,云台位于正中心时默认设为0
 #define CENTER_GIMBAL_OFFSET_Y 0 // 云台旋转中心距底盘几何中心的距离,左右方向,云台位于正中心时默认设为0
 #define RADIUS_WHEEL 60          // 轮子半径
-#define REDUCTION_RATIO 19       // 电机减速比,因为编码器量测的是转子的速度而不是输出轴的速度故需进行转换
+#define REDUCTION_RATIO 19.0f    // 电机减速比,因为编码器量测的是转子的速度而不是输出轴的速度故需进行转换
 
 /* 自动计算的参数 */
 #define HALF_WHEEL_BASE (WHEEL_BASE / 2.0f)
@@ -39,11 +39,11 @@
 #include "ins_task.h"
 static CANCommInstance *chasiss_can_comm; // 双板通信CAN comm
 IMU_Data_t *Chassis_IMU_data;
-#endif // CHASSIS_BOARD
+#endif                               // CHASSIS_BOARD
 static referee_info_t *referee_data; // 裁判系统的数据
 // static SuperCAP* cap; 尚未增加超级电容
 static dji_motor_instance *motor_lf; // left right forward back
-static dji_motor_instance *motor_rf; 
+static dji_motor_instance *motor_rf;
 static dji_motor_instance *motor_lb;
 static dji_motor_instance *motor_rb;
 
@@ -76,6 +76,7 @@ void ChassisInit()
         .controller_setting_init_config = {
             .angle_feedback_source = MOTOR_FEED,
             .speed_feedback_source = MOTOR_FEED,
+            .outer_loop_type = SPEED_LOOP,
             .close_loop_type = SPEED_LOOP | CURRENT_LOOP,
             .reverse_flag = MOTOR_DIRECTION_REVERSE,
         },
@@ -97,6 +98,7 @@ void ChassisInit()
         .controller_setting_init_config = {
             .angle_feedback_source = MOTOR_FEED,
             .speed_feedback_source = MOTOR_FEED,
+            .outer_loop_type = SPEED_LOOP,
             .close_loop_type = SPEED_LOOP | CURRENT_LOOP,
             .reverse_flag = MOTOR_DIRECTION_REVERSE,
         },
@@ -118,6 +120,7 @@ void ChassisInit()
         .controller_setting_init_config = {
             .angle_feedback_source = MOTOR_FEED,
             .speed_feedback_source = MOTOR_FEED,
+            .outer_loop_type = SPEED_LOOP,
             .close_loop_type = SPEED_LOOP | CURRENT_LOOP,
             .reverse_flag = MOTOR_DIRECTION_REVERSE,
         },
@@ -139,6 +142,7 @@ void ChassisInit()
         .controller_setting_init_config = {
             .angle_feedback_source = MOTOR_FEED,
             .speed_feedback_source = MOTOR_FEED,
+            .outer_loop_type = SPEED_LOOP,
             .close_loop_type = SPEED_LOOP | CURRENT_LOOP,
             .reverse_flag = MOTOR_DIRECTION_REVERSE,
         },
@@ -198,7 +202,6 @@ static void EstimateSpeed()
     // 根据电机速度和imu的速度解算
     // chassis_feedback_data.vx vy wz
     //  ...
-    
 }
 
 void ChassisTask()
@@ -208,6 +211,7 @@ void ChassisTask()
     SubGetMessage(chassis_sub, &chassis_cmd_recv);
 
     // 根据控制模式设定旋转速度
+    // 后续增加不同状态的过渡模式?
     switch (chassis_cmd_recv.chassis_mode)
     {
     case CHASSIS_NO_FOLLOW:
@@ -220,7 +224,10 @@ void ChassisTask()
         // chassis_cmd_recv.wz // 当前维持定值,后续增加不规则的变速策略
         break;
     case CHASSIS_ZERO_FORCE:
-        DJIMotorStop(); // 如果出现重要模块离线或遥控器设置为急停,让电机停止
+        DJIMotorStop(motor_lf); // 如果出现重要模块离线或遥控器设置为急停,让电机停止
+        DJIMotorStop(motor_rf);
+        DJIMotorStop(motor_lb);
+        DJIMotorStop(motor_rb);
         break;
     default:
         break;
@@ -242,11 +249,11 @@ void ChassisTask()
     // 根据电机的反馈速度计算
     EstimateSpeed();
 
-    //获取裁判系统数据
-    // 我方颜色id小于7是红色,大于7是蓝色,注意这里发送的是对方的颜色, 0:blue , 1:red
-    chassis_feedback_data.enemy_color = referee_data->GameRobotStat.robot_id > 7 ? 1 : 0; // 
+    // 获取裁判系统数据
+    //  我方颜色id小于7是红色,大于7是蓝色,注意这里发送的是对方的颜色, 0:blue , 1:red
+    chassis_feedback_data.enemy_color = referee_data->GameRobotStat.robot_id > 7 ? 1 : 0; //
     chassis_feedback_data.bullet_speed = referee_data->GameRobotStat.shooter_id1_17mm_speed_limit;
-    chassis_feedback_data.rest_heat=referee_data->PowerHeatData.shooter_heat0;
+    chassis_feedback_data.rest_heat = referee_data->PowerHeatData.shooter_heat0;
 
     // 推送反馈消息
     PubPushMessage(chassis_pub, &chassis_feedback_data);

application/gimbal/gimbal.c

@@ -43,6 +43,7 @@ void GimbalInit()
         .controller_setting_init_config = {
             .angle_feedback_source = MOTOR_FEED,
             .speed_feedback_source = MOTOR_FEED,
+            .outer_loop_type=ANGLE_LOOP,
             .close_loop_type = ANGLE_LOOP | SPEED_LOOP,
             .reverse_flag = MOTOR_DIRECTION_REVERSE,
         },
@@ -69,6 +70,7 @@ void GimbalInit()
         .controller_setting_init_config = {
             .angle_feedback_source = MOTOR_FEED,
             .speed_feedback_source = MOTOR_FEED,
+            .outer_loop_type=ANGLE_LOOP,
             .close_loop_type = ANGLE_LOOP | SPEED_LOOP,
             .reverse_flag = MOTOR_DIRECTION_REVERSE,
         },
@@ -101,7 +103,8 @@ void GimbalTask()
     switch (gimbal_cmd_recv.gimbal_mode)
     {
     case GIMBAL_ZERO_FORCE:
-        DJIMotorStop();
+        DJIMotorStop(yaw_motor);
+        DJIMotorStop(pitch_motor);
         break;
     case GIMBAL_GYRO_MODE:
         DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, OTHER_FEED);

application/robot_def.h

@@ -4,17 +4,17 @@
  * @author Even
  * @version 0.1
  * @date 2022-12-02
- * 
+ *
  * @copyright Copyright (c) HNU YueLu EC 2022 all rights reserved
- * 
+ *
  */
 
 #ifndef ROBOT_DEF_H
 #define ROBOT_DEF_H
 
 #define PI 3.14159f
-#define RAD_2_ANGLE (180.0f/PI)
-#define ANGLE_2_RAD (PI/180.0f)
+#define RAD_2_ANGLE (180.0f / PI)
+#define ANGLE_2_RAD (PI / 180.0f)
 
 #include "ins_task.h"
 #include "master_process.h"
@@ -29,7 +29,6 @@
 /* 重要参数定义,注意根据不同机器人进行修改 */
 #define YAW_MID_ECD
 
-
 #if (defined(ONE_BOARD) && defined(CHASSIS_BOARD)) || \
     (defined(ONE_BOARD) && defined(GIMBAL_BOARD)) ||  \
     (defined(CHASSIS_BOARD) && defined(GIMBAL_BOARD))
@@ -55,7 +54,7 @@ typedef enum
 // 底盘模式设置
 /**
  * @brief 后续考虑修改为云台跟随底盘,而不是让底盘去追云台,云台的惯量比底盘小.
- * 
+ *
  */
 typedef enum
 {
@@ -68,9 +67,9 @@ typedef enum
 // 云台模式设置
 typedef enum
 {
-    GIMBAL_ZERO_FORCE,  // 电流零输入
-    GIMBAL_FREE_MODE,   // 云台自由运动模式,即与底盘分离(底盘此时应为NO_FOLLOW)反馈值为电机total_angle
-    GIMBAL_GYRO_MODE,   // 云台陀螺仪反馈模式,反馈值为陀螺仪pitch,total_yaw_angle,底盘可以为小陀螺和跟随模式
+    GIMBAL_ZERO_FORCE, // 电流零输入
+    GIMBAL_FREE_MODE,  // 云台自由运动模式,即与底盘分离(底盘此时应为NO_FOLLOW)反馈值为电机total_angle
+    GIMBAL_GYRO_MODE,  // 云台陀螺仪反馈模式,反馈值为陀螺仪pitch,total_yaw_angle,底盘可以为小陀螺和跟随模式
 } gimbal_mode_e;
 
 // 发射模式设置
@@ -83,11 +82,12 @@ typedef enum
 typedef enum
 {
     LID_CLOSE, // 弹舱盖打开
-    LID_ON,    // 弹舱盖关闭
+    LID_OPEN,  // 弹舱盖关闭
 } lid_mode_e;
 
 typedef enum
 {
+    SHOOT_STOP,     // 停止整个发射模块,后续可能隔离出来
     LOAD_STOP,      // 停止发射
     LOAD_REVERSE,   // 反转
     LOAD_1_BULLET,  // 单发
@@ -98,13 +98,9 @@ typedef enum
 // 功率限制,从裁判系统获取
 typedef struct
 { // 功率控制
-    
+
 } Chassis_Power_Data_s;
 
-
-
-
-
 /* ----------------CMD应用发布的控制数据,应当由gimbal/chassis/shoot订阅---------------- */
 /**
  * @brief 对于双板情况,遥控器和pc在云台,裁判系统在底盘
@@ -120,8 +116,8 @@ typedef struct
     float offset_angle; // 底盘和归中位置的夹角
     chassis_mode_e chassis_mode;
 
-    //UI部分
-    // ...
+    // UI部分
+    //  ...
 
 } Chassis_Ctrl_Cmd_s;
 
@@ -137,17 +133,15 @@ typedef struct
 
 // cmd发布的发射控制数据,由shoot订阅
 typedef struct
-{ // 发射弹速控制
+{ 
     loader_mode_e load_mode;
     lid_mode_e lid_mode;
     shoot_mode_e shoot_mode;
-    Bullet_Speed_e bullet_speed;
+    Bullet_Speed_e bullet_speed; // 弹速枚举
     uint8_t rest_heat;
+    float shoot_rate; //连续发射的射频,unit per s,发/秒
 } Shoot_Ctrl_Cmd_s;
 
-
-
-
 /* ----------------gimbal/shoot/chassis发布的反馈数据----------------*/
 /**
  * @brief 由cmd订阅,其他应用也可以根据需要获取.
@@ -165,11 +159,11 @@ typedef struct
     // float real_vy;
     // float real_wz;
 
-    uint8_t rest_heat; //剩余枪口热量
-    Bullet_Speed_e bullet_speed; //弹速限制
-    Enemy_Color_e enemy_color; // 0 for blue, 1 for red
+    uint8_t rest_heat;           // 剩余枪口热量
+    Bullet_Speed_e bullet_speed; // 弹速限制
+    Enemy_Color_e enemy_color;   // 0 for blue, 1 for red
 
-    //是否需要剩余电量?(电容)
+    // 是否需要剩余电量?(电容)
 
 } Chassis_Upload_Data_s;
 

application/shoot/shoot.c

@@ -2,18 +2,27 @@
 #include "robot_def.h"
 #include "dji_motor.h"
 #include "message_center.h"
+#include "bsp_dwt.h"
 
-/* 对于双发射机构的机器人,将下面的数据封装成结构体即可,生成两份shoot应用实例
- */
+#define ONE_BULLET_DELTA_ANGLE 0 // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出
+#define REDUCTION_RATIO 49.0f    // 拨盘电机的减速比,英雄需要修改为3508的19.0f
+#define NUM_PER_CIRCLE 1         // 拨盘一圈的装载量
+
+/* 对于双发射机构的机器人,将下面的数据封装成结构体即可,生成两份shoot应用实例 */
 static dji_motor_instance *friction_l; // 左摩擦轮
 static dji_motor_instance *friction_r; // 右摩擦轮
 static dji_motor_instance *loader;     // 拨盘电机
+// static servo_instance *lid; 需要增加弹舱盖
 
 static Publisher_t *shoot_pub;
 static Shoot_Ctrl_Cmd_s shoot_cmd_recv; // 来自gimbal_cmd的发射控制信息
 static Subscriber_t *shoot_sub;
 static Shoot_Upload_Data_s shoot_feedback_data; // 来自gimbal_cmd的发射控制信息
 
+// 定时,计算冷却用
+static uint32_t INS_DWT_Count = 0;
+static float dt = 0, t = 0;
+
 void ShootInit()
 {
     // 左摩擦轮
@@ -38,6 +47,8 @@ void ShootInit()
         .controller_setting_init_config = {
             .angle_feedback_source = MOTOR_FEED,
             .speed_feedback_source = MOTOR_FEED,
+
+            .outer_loop_type = SPEED_LOOP,
             .close_loop_type = SPEED_LOOP | CURRENT_LOOP,
             .reverse_flag = MOTOR_DIRECTION_REVERSE,
         },
@@ -64,11 +75,12 @@ void ShootInit()
         .controller_setting_init_config = {
             .angle_feedback_source = MOTOR_FEED,
             .speed_feedback_source = MOTOR_FEED,
+            .outer_loop_type = SPEED_LOOP,
             .close_loop_type = SPEED_LOOP | CURRENT_LOOP,
             .reverse_flag = MOTOR_DIRECTION_REVERSE,
         },
         .motor_type = M3508};
-    //  拨盘电机
+    // 拨盘电机
     Motor_Init_Config_s loader_config = {
         .can_init_config = {
             .can_handle = &hcan1,
@@ -76,9 +88,13 @@ void ShootInit()
         },
         .controller_param_init_config = {
             .angle_PID = {
+                // 如果启用位置环来控制发弹,需要较大的I值保证输出力矩的线性度否则出现接近拨出的力矩大幅下降
                 .Kd = 10,
                 .Ki = 1,
                 .Kd = 2,
+            },
+            .angle_PID = {
+
             },
             .speed_PID = {
 
@@ -88,12 +104,13 @@ void ShootInit()
             },
         },
         .controller_setting_init_config = {
-            .angle_feedback_source = MOTOR_FEED,
-            .speed_feedback_source = MOTOR_FEED,
-            .close_loop_type = SPEED_LOOP | CURRENT_LOOP,
-            .reverse_flag = MOTOR_DIRECTION_REVERSE,
+            .angle_feedback_source = MOTOR_FEED, .speed_feedback_source = MOTOR_FEED,
+            .outer_loop_type = SPEED_LOOP, // 初始化成SPEED_LOOP,让拨盘停在原地,防止拨盘上电时乱转
+            .close_loop_type = ANGLE_LOOP | SPEED_LOOP | CURRENT_LOOP,
+            .reverse_flag = MOTOR_DIRECTION_REVERSE, // 注意方向设置为拨盘的拨出的击发方向
         },
-        .motor_type = M2006};
+        .motor_type = M2006 // 英雄使用m3508
+    };
 
     friction_l = DJIMotorInit(&left_friction_config);
     friction_r = DJIMotorInit(&right_friction_config);
@@ -105,4 +122,76 @@ void ShootInit()
 
 void ShootTask()
 {
+    // 从cmd获取控制数据
+    SubGetMessage(shoot_sub, &shoot_cmd_recv);
+
+    // 根据控制模式进行电机参考值设定和模式切换
+    switch (shoot_cmd_recv.load_mode)
+    {
+    // 停止三个电机
+    case SHOOT_STOP:
+        DJIMotorStop(friction_l);
+        DJIMotorStop(friction_r);
+        DJIMotorStop(loader);
+        break;
+    // 停止拨盘
+    case LOAD_STOP:
+        DJIMotorOuterLoop(loader, SPEED_LOOP);
+        DJIMotorSetRef(loader, 0);
+        break;
+    // 单发模式,根据鼠标按下的时间,触发一次之后需要进入不响应输入的状态(否则按下的时间内可能多次进入)
+    // 激活能量机关/干扰对方用,英雄用.
+    case LOAD_1_BULLET:
+        DJIMotorOuterLoop(loader, ANGLE_LOOP);
+        DJIMotorSetRef(loader, loader->motor_measure.total_angle + ONE_BULLET_DELTA_ANGLE); // 增加一发弹丸
+        break;
+    // 三连发,如果不需要后续可能删除
+    case LOAD_3_BULLET:
+        DJIMotorOuterLoop(loader, ANGLE_LOOP);
+        DJIMotorSetRef(loader, loader->motor_measure.total_angle + 3 * ONE_BULLET_DELTA_ANGLE); // 增加3发
+        break;
+    // 连发模式,对速度闭环,射频后续修改为可变
+    case LOAD_BURSTFIRE:
+        DJIMotorOuterLoop(loader, SPEED_LOOP);
+        DJIMotorSetRef(loader, shoot_cmd_recv.shoot_rate * 360 * REDUCTION_RATIO / NUM_PER_CIRCLE);
+        // x颗/秒换算成速度: 已知一圈的载弹量,由此计算出1s需要转的角度,注意换算角速度
+        break;
+    // 拨盘反转,对速度闭环,后续增加卡弹检测(通过裁判系统剩余热量反馈)
+    // 可能需要从switch-case中独立出来
+    case LOAD_REVERSE:
+        DJIMotorOuterLoop(loader, SPEED_LOOP);
+        // ...
+        break;
+    default:
+        break;
+    }
+
+    // 根据收到的弹速设置设定摩擦轮参考值,需实测后填入
+    switch (shoot_cmd_recv.bullet_speed)
+    {
+    case SMALL_AMU_15:
+        DJIMotorSetRef(friction_l, 0);
+        DJIMotorSetRef(friction_l, 0);
+        break;
+    case SMALL_AMU_18:
+        DJIMotorSetRef(friction_l, 0);
+        DJIMotorSetRef(friction_l, 0);
+        break;
+    case SMALL_AMU_30:
+        DJIMotorSetRef(friction_l, 0);
+        DJIMotorSetRef(friction_l, 0);
+        break;
+    default:
+        break;
+    }
+
+    // 开关弹舱盖
+    if (shoot_cmd_recv.lid_mode == LID_CLOSE)
+    {
+        //...
+    }
+    else if (shoot_cmd_recv.lid_mode == LID_OPEN)
+    {
+        //...
+    }
 }