新增通用定义头文件,主要内容是角度转换.统一控制的输入,为后续model-based控制做准备

Makefile

@@ -121,7 +121,8 @@ modules/master_machine/seasky_protocol.c \
 modules/motor/dji_motor.c \
 modules/motor/HT04.c \
 modules/motor/LK9025.c \
-modules/mode/step_motor.c \
+modules/motor/step_motor.c \
+modules/motor/servo_motor.c \
 modules/motor/motor_task.c \
 modules/referee/crc.c \
 modules/referee/referee.c \
@@ -228,7 +229,8 @@ C_INCLUDES =  \
 -Imodules/super_cap \
 -Imodules/can_comm \
 -Imodules/message_center \
--Imodules/monitor
+-Imodules/monitor \
+-Imodules/
 
 # compile gcc flags
 ASFLAGS = $(MCU) $(AS_DEFS) $(AS_INCLUDES) $(OPT) -Wall -fdata-sections -ffunction-sections

application/chassis/chassis.c

@@ -17,6 +17,7 @@
 #include "super_cap.h"
 #include "message_center.h"
 #include "referee.h"
+#include "general_def.h"
 
 #include "arm_math.h"
 

application/cmd/robot_cmd.c

@@ -4,6 +4,7 @@
 #include "ins_task.h"
 #include "master_process.h"
 #include "message_center.h"
+#include "general_def.h"
 
 /* gimbal_cmd应用包含的模块实例指针和交互信息存储*/
 #ifndef ONE_BOARD

application/gimbal/gimbal.c

@@ -3,10 +3,15 @@
 #include "dji_motor.h"
 #include "ins_task.h"
 #include "message_center.h"
+#include "general_def.h"
 
-// 需要将控制值统一修改为角度制而不是编码器ECD值
+/* 根据每个机器人进行设定,若对云台有改动需要修改 */
-#define YAW_ALIGN_ECD 0 // 云台和底盘对齐指向相同方向时的电机编码器值
+#define YAW_CHASSIS_ALIGN_ECD 0 // 云台和底盘对齐指向相同方向时的电机编码器值
-#define PITCH_HORIZON_ECD 0 // 云台处于水平位置时编码器值
+#define PITCH_HORIZON_ECD 0     // 云台处于水平位置时编码器值
+
+// 自动将编码器转换成角度值
+#define YAW_CHASSIS_ALIGN_ANGLE
+#define PTICH_HORIZON_ALIGN_ANGLE
 
 static attitude_t *Gimbal_IMU_data;     // 云台IMU数据
 static dji_motor_instance *yaw_motor;   // yaw电机
@@ -43,7 +48,7 @@ void GimbalInit()
         .controller_setting_init_config = {
             .angle_feedback_source = MOTOR_FEED,
             .speed_feedback_source = MOTOR_FEED,
-            .outer_loop_type=ANGLE_LOOP,
+            .outer_loop_type = ANGLE_LOOP,
             .close_loop_type = ANGLE_LOOP | SPEED_LOOP,
             .reverse_flag = MOTOR_DIRECTION_REVERSE,
         },
@@ -70,7 +75,7 @@ void GimbalInit()
         .controller_setting_init_config = {
             .angle_feedback_source = MOTOR_FEED,
             .speed_feedback_source = MOTOR_FEED,
-            .outer_loop_type=ANGLE_LOOP,
+            .outer_loop_type = ANGLE_LOOP,
             .close_loop_type = ANGLE_LOOP | SPEED_LOOP,
             .reverse_flag = MOTOR_DIRECTION_REVERSE,
         },
@@ -84,8 +89,8 @@ void GimbalInit()
 }
 
 // /**
-//  * @brief 
+//  * @brief
-//  * 
+//  *
 //  */
 // static void TransitionMode()
 // {
@@ -111,17 +116,17 @@ void GimbalTask()
         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(yaw_motor, gimbal_cmd_recv.yaw);
-        DJIMotorSetRef(pitch_motor,gimbal_cmd_recv.pitch);
+        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(yaw_motor, YAW_ALIGN_ECD);
-        DJIMotorSetRef(pitch_motor,PITCH_HORIZON_ECD);
+        DJIMotorSetRef(pitch_motor, PITCH_HORIZON_ECD);
         break;
     default:
         break;
@@ -142,9 +147,9 @@ void GimbalTask()
     */
 
     // 获取反馈数据
-    gimbal_feedback_data.gimbal_imu_data=*Gimbal_IMU_data;
+    gimbal_feedback_data.gimbal_imu_data = *Gimbal_IMU_data;
-    gimbal_feedback_data.yaw_motor_ecd=pitch_motor->motor_measure.ecd;
+    gimbal_feedback_data.yaw_motor_ecd = pitch_motor->motor_measure.ecd;
-    
+
     // 推送消息
-    PubPushMessage(gimbal_pub,&gimbal_feedback_data);
+    PubPushMessage(gimbal_pub, &gimbal_feedback_data);
 }

application/robot_def.h

@@ -12,10 +12,6 @@
 #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)
-
 #include "ins_task.h"
 #include "master_process.h"
 #include "stdint-gcc.h"

application/shoot/shoot.c

@@ -3,6 +3,7 @@
 #include "dji_motor.h"
 #include "message_center.h"
 #include "bsp_dwt.h"
+#include "general_def.h"
 
 #define ONE_BULLET_DELTA_ANGLE 0 // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出
 #define REDUCTION_RATIO 49.0f    // 拨盘电机的减速比,英雄需要修改为3508的19.0f
@@ -19,9 +20,8 @@ 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的发射控制信息
 
-// 定时,计算冷却用
+// dwt定时,计算冷却用
-static uint32_t INS_DWT_Count = 0;
+static float hibernate_time = 0, dead_time = 0;
-static float dt = 0, t = 0;
 
 void ShootInit()
 {
@@ -125,30 +125,39 @@ void ShootTask()
     // 从cmd获取控制数据
     SubGetMessage(shoot_sub, &shoot_cmd_recv);
 
-    // 根据控制模式进行电机参考值设定和模式切换
+    // 对shoot mode等于SHOOT_STOP的情况特殊处理,直接停止所有电机
-    switch (shoot_cmd_recv.load_mode)
+    if (shoot_cmd_recv.load_mode == SHOOT_STOP)
     {
-    // 停止三个电机
-    case SHOOT_STOP:
         DJIMotorStop(friction_l);
         DJIMotorStop(friction_r);
         DJIMotorStop(loader);
-        break;
+    }
+
+    // 如果上一次触发单发或3发指令的时间加上不应期仍然大于当前时间(尚未休眠完毕),直接返回即可
+    if (hibernate_time + dead_time > DWT_GetTimeline_ms())
+        return;
+
+    // 若不在休眠状态,根据控制模式进行电机参考值设定和模式切换
+    switch (shoot_cmd_recv.load_mode)
+    {
     // 停止拨盘
     case LOAD_STOP:
         DJIMotorOuterLoop(loader, SPEED_LOOP);
         DJIMotorSetRef(loader, 0);
         break;
-    // 单发模式,根据鼠标按下的时间,触发一次之后需要进入不响应输入的状态(否则按下的时间内可能多次进入)
+    // 单发模式,根据鼠标按下的时间,触发一次之后需要进入不响应输入的状态(否则按下的时间内可能多次进入)F
-    // 激活能量机关/干扰对方用,英雄用.
+    case LOAD_1_BULLET: // 激活能量机关/干扰对方用,英雄用.
-    case LOAD_1_BULLET:
         DJIMotorOuterLoop(loader, ANGLE_LOOP);
-        DJIMotorSetRef(loader, loader->motor_measure.total_angle + ONE_BULLET_DELTA_ANGLE); // 增加一发弹丸
+        DJIMotorSetRef(loader, loader->motor_measure.total_angle + ONE_BULLET_DELTA_ANGLE); // 增加一发弹丸的角度
+        hibernate_time = DWT_GetTimeline_ms();                                              // 记录触发指令的时间
+        dead_time = 150;                                                                    // 完成1发弹丸发射的时间
         break;
     // 三连发,如果不需要后续可能删除
     case LOAD_3_BULLET:
         DJIMotorOuterLoop(loader, ANGLE_LOOP);
         DJIMotorSetRef(loader, loader->motor_measure.total_angle + 3 * ONE_BULLET_DELTA_ANGLE); // 增加3发
+        hibernate_time = DWT_GetTimeline_ms();                                                  // 记录触发指令的时间
+        dead_time = 300;                                                                        // 完成3发弹丸发射的时间
         break;
     // 连发模式,对速度闭环,射频后续修改为可变
     case LOAD_BURSTFIRE:

application/shoot/shoot.h

@@ -5,4 +5,4 @@ void ShootInit();
 
 void ShootTask();
 
-#endif //SHOOT_H
+#endif // SHOOT_H

modules/general_def.h

@@ -0,0 +1,13 @@
+#ifndef GENERAL_DEF_H
+#define GENERAL_DEF_H
+
+#define PI 3.1415926535f
+#define PI2 (PI * 2.0f) // 2 pi
+
+#define RAD_2_ANGLE (180.0f / PI)
+#define ANGLE_2_RAD (PI / 180.0f)
+
+#define RPM_2_ANGLE_PER_SEC (360.0f/60.0f) // ×360°/60sec
+
+
+#endif // !GENERAL_DEF_H

modules/motor/dji_motor.c

@@ -1,6 +1,6 @@
 #include "dji_motor.h"
+#include "general_def.h"
 
-#define PI2 (3.141592f * 2)  // 2pi
 #define ECD_ANGLE_COEF 0.043945f // 360/8192,将编码器值转化为角度制
 
 static uint8_t idx = 0; // register idx,是该文件的全局电机索引,在注册时使用
@@ -129,20 +129,24 @@ static void DecodeDJIMotor(can_instance *_instance)
         if (dji_motor_info[i]->motor_can_instance == _instance)
         {
             rxbuff = _instance->rx_buff;
-            measure = &dji_motor_info[i]->motor_measure;
+            measure = &dji_motor_info[i]->motor_measure; // measure要多次使用,保存指针减小访存开销
+
             // resolve data and apply filter to current and speed
             measure->last_ecd = measure->ecd;
             measure->ecd = (uint16_t)(rxbuff[0] << 8 | rxbuff[1]);
-            // 增加滤波
+            measure->angle_single_round = ECD_ANGLE_COEF* measure->ecd;
-            measure->speed_rpm = (1 - SPEED_SMOOTH_COEF) * measure->speed_rpm + SPEED_SMOOTH_COEF * (int16_t)(rxbuff[2] << 8 | rxbuff[3]);
+            measure->speed_angle_per_sec = (1 - SPEED_SMOOTH_COEF) * measure->speed_angle_per_sec + 
-            measure->given_current = (1 - CURRENT_SMOOTH_COEF) * measure->given_current + CURRENT_SMOOTH_COEF * (uint16_t)(rxbuff[4] << 8 | rxbuff[5]);
+                                           RPM_2_ANGLE_PER_SEC * SPEED_SMOOTH_COEF * (int16_t)(rxbuff[2] << 8 | rxbuff[3]);
+            measure->given_current = (1 - CURRENT_SMOOTH_COEF) * measure->given_current + 
+                                     RPM_2_ANGLE_PER_SEC * CURRENT_SMOOTH_COEF * (uint16_t)(rxbuff[4] << 8 | rxbuff[5]);
             measure->temperate = rxbuff[6];
-            // multi round calc
+
-            if (measure->ecd - measure->last_ecd > 4096)
+            // multi rounds calc,计算的前提是两次采样间电机转过的角度小于180°
+            if (measure->ecd - measure->last_ecd > 4096) 
                 measure->total_round--;
             else if (measure->ecd - measure->last_ecd < -4096)
                 measure->total_round++;
-            measure->total_angle = measure->total_round * 360 + measure->ecd * ECD_ANGLE_COEF; // @todo simplify the calculation
+            measure->total_angle = measure->total_round * 360 + measure->angle_single_round; 
             break;
         }
     }
@@ -164,8 +168,10 @@ dji_motor_instance *DJIMotorInit(Motor_Init_Config_s *config)
     PID_Init(&dji_motor_info[idx]->motor_controller.angle_PID, &config->controller_param_init_config.angle_PID);
     dji_motor_info[idx]->motor_controller.other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr;
     dji_motor_info[idx]->motor_controller.other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr;
+    
     // group motors, because 4 motors share the same CAN control message
     MotorSenderGrouping(&config->can_init_config);
+
     // register motor to CAN bus
     config->can_init_config.can_module_callback = DecodeDJIMotor; // set callback
     dji_motor_info[idx]->motor_can_instance = CANRegister(&config->can_init_config);
@@ -173,7 +179,6 @@ dji_motor_instance *DJIMotorInit(Motor_Init_Config_s *config)
     return dji_motor_info[idx++];
 }
 
-// 改变反馈来源
 void DJIMotorChangeFeed(dji_motor_instance *motor, Closeloop_Type_e loop, Feedback_Source_e type)
 {
     if (loop == ANGLE_LOOP)
@@ -186,12 +191,28 @@ void DJIMotorChangeFeed(dji_motor_instance *motor, Closeloop_Type_e loop, Feedba
     }
 }
 
+void DJIMotorStop(dji_motor_instance *motor)
+{
+    motor->stop_flag = MOTOR_STOP;
+}
+
+void DJIMotorEnable(dji_motor_instance *motor)
+{
+    motor->stop_flag = MOTOR_ENALBED;
+}
+
+void DJIMotorOuterLoop(dji_motor_instance *motor, Closeloop_Type_e outer_loop)
+{
+    motor->motor_settings.outer_loop_type = outer_loop;
+}
+
 // 设置参考值
 void DJIMotorSetRef(dji_motor_instance *motor, float ref)
 {
     motor->motor_controller.pid_ref = ref;
 }
-// 计算三环PID,发送控制报文
+
+// 为所有电机实例计算三环PID,发送控制报文
 void DJIMotorControl()
 {
     // 预先通过静态变量定义避免反复释放分配栈空间,直接保存一次指针引用从而减小访存的开销
@@ -231,7 +252,7 @@ void DJIMotorControl()
                 if (motor_setting->speed_feedback_source == OTHER_FEED)
                     pid_measure = *motor_controller->other_speed_feedback_ptr;
                 else
-                    pid_measure = motor_measure->speed_rpm;
+                    pid_measure = motor_measure->speed_angle_per_sec;
                 // 更新pid_ref进入下一个环
                 motor_controller->pid_ref = PID_Calculate(&motor_controller->speed_PID, pid_measure, motor_controller->pid_ref);
             }
@@ -272,18 +293,3 @@ void DJIMotorControl()
         }
     }
 }
-
-void DJIMotorStop(dji_motor_instance *motor)
-{
-    motor->stop_flag = MOTOR_STOP;
-}
-
-void DJIMotorEnable(dji_motor_instance *motor)
-{
-    motor->stop_flag = MOTOR_ENALBED;
-}
-
-void DJIMotorOuterLoop(dji_motor_instance *motor, Closeloop_Type_e outer_loop)
-{
-    motor->motor_settings.outer_loop_type = outer_loop;
-}

modules/motor/dji_motor.h

@@ -22,19 +22,20 @@
 #define DJI_MOTOR_CNT 12
 
 /* 滤波系数设置为1的时候即关闭滤波 */
-#define SPEED_SMOOTH_COEF 0.85f   // better to be greater than 0.8
+#define SPEED_SMOOTH_COEF 0.9f    // better to be greater than 0.85
-#define CURRENT_SMOOTH_COEF 0.98f // this coef must be greater than 0.95
+#define CURRENT_SMOOTH_COEF 0.98f // this coef *must* be greater than 0.95
 
 /* DJI电机CAN反馈信息*/
 typedef struct
 {
-    uint16_t ecd; // 0-8192
+    uint16_t ecd;              // 0-8191,刻度总共有8192格
-    uint16_t last_ecd;
+    uint16_t last_ecd;         // 上一次读取的编码器值
-    int16_t speed_rpm;     // rounds per minute
+    float angle_single_round;  // 单圈角度
-    int16_t given_current; // 实际电流
+    float speed_angle_per_sec; // 度/秒 rounds per minute
-    uint8_t temperate;
+    int16_t given_current;     // 实际电流
-    int16_t total_round; // 总圈数,注意方向
+    uint8_t temperate;         // 温度 Celsius
-    int32_t total_angle; // 总角度,注意方向
+    int16_t total_round;       // 总圈数,注意方向
+    int32_t total_angle;       // 总角度,注意方向
 } dji_motor_measure;
 
 /**
@@ -59,9 +60,8 @@ typedef struct
     uint8_t sender_group;
     uint8_t message_num;
 
-    Motor_Type_e motor_type;
+    Motor_Type_e motor_type;        // 电机类型
-
+    Motor_Working_Type_e stop_flag; // 启停标志
-    Motor_Working_Type_e stop_flag;
 
 } dji_motor_instance;
 
@@ -111,22 +111,21 @@ void DJIMotorControl();
  * @brief 停止电机,注意不是将设定值设为零,而是直接给电机发送的电流值置零
  *
  */
-void DJIMotorStop(dji_motor_instance* motor);
+void DJIMotorStop(dji_motor_instance *motor);
 
 /**
  * @brief 启动电机,此时电机会响应设定值
  *        初始化时不需要此函数,因为stop_flag的默认值为0
  *
  */
-void DJIMotorEnable(dji_motor_instance* motor);
+void DJIMotorEnable(dji_motor_instance *motor);
 
 /**
  * @brief 修改电机闭环目标(外层闭环)
- * 
+ *
  * @param motor  要修改的电机实例指针
  * @param outer_loop 外层闭环类型
  */
 void DJIMotorOuterLoop(dji_motor_instance *motor, Closeloop_Type_e outer_loop);
 
-
 #endif // !DJI_MOTOR_H

modules/motor/dji_motor.md

@@ -15,9 +15,17 @@ dji_motor模块对DJI智能电机，包括M2006，M3508以及GM6020进行了详
 
 **==设定值的单位==**
 
-1. 位置环为角度，角度制。
+1. ==位置环为**角度制**（0-360，total_angle可以为任意值）==
-2. 速度环为角速度，单位为度/每秒
+
-3. 
+2. ==速度环为角速度，单位为**度/每秒**（deg/sec）==
+
+3. ==电流环为mA==
+
+4. ==GM6020的输入设定为**力矩**，待测量（-30000~30000）==
+
+   ==M3508的输入设定为-20A~20A （-16384~16384）==
+
+   ==M2006的输入设定为-10A~10A （-10000~10000）==
 
 如果你希望更改电机的反馈来源，比如进入小陀螺模式/视觉模式（这时候你想要云台保持静止，使用IMU的yaw角度值作为反馈来源），只需要调用`DJIMotorChangeFeed()`，电机便可立刻切换反馈数据来源至IMU。
 
@@ -419,6 +427,12 @@ static void DecodeDJIMotor(can_instance *_instance)
 - `DecodeDJIMotor()`是解析电机反馈报文的函数，在`DJIMotorInit()`中会将其注册到该电机实例对应的`can_instance`中（即`can_instance`的`can_module_callback()`）。这样，当该电机的反馈报文到达时，`bsp_can.c`中的回调函数会调用解包函数进行反馈数据解析。
 
   该函数还会对电流和速度反馈值进行滤波，消除高频噪声；同时计算多圈角度和单圈绝对角度。
+  
+  **电机反馈的电流值为说明书中的映射值，需转换为实际值。**
+  
+  **反馈的速度单位是rpm（转每分钟），转换为角度每秒。**
+  
+  **反馈的位置是编码器值（0~8191），转换为角度。**
 
 ## 使用范例