更改了宏定义的文件

.vscode/settings.json

@@ -35,7 +35,8 @@
         "compare": "c",
         "*.tcc": "c",
         "type_traits": "c",
-        "typeinfo": "c"
+        "typeinfo": "c",
+        "general_def.h": "c"
     },
     "C_Cpp.default.configurationProvider": "ms-vscode.makefile-tools",
 }

application/chassis/chassis.c

@@ -18,18 +18,12 @@
 #include "message_center.h"
 #include "referee.h"
 #include "general_def.h"
+#include "bsp_dwt.h"
 
 #include "arm_math.h"
 
-/* 需要根据机器人底盘修改的参数,单位为mm(毫米) */
-#define WHEEL_BASE 300           // 纵向轴距(前进后退方向)
-#define TRACK_WIDTH 300          // 横向轮距(左右平移方向)
-#define CENTER_GIMBAL_OFFSET_X 0 // 云台旋转中心距底盘几何中心的距离,前后方向,云台位于正中心时默认设为0
-#define CENTER_GIMBAL_OFFSET_Y 0 // 云台旋转中心距底盘几何中心的距离,左右方向,云台位于正中心时默认设为0
-#define RADIUS_WHEEL 60          // 轮子半径
-#define REDUCTION_RATIO 19.0f    // 电机减速比,因为编码器量测的是转子的速度而不是输出轴的速度故需进行转换
 
-/* 自动计算的参数 */
+/* 根据robot_def.h中的macro自动计算的参数 */
 #define HALF_WHEEL_BASE (WHEEL_BASE / 2.0f)
 #define HALF_TRACK_WIDTH (TRACK_WIDTH / 2.0f)
 #define PERIMETER_WHEEL (RADIUS_WHEEL * 2 * PI)
@@ -54,6 +48,9 @@ static Chassis_Ctrl_Cmd_s chassis_cmd_recv;
 static Subscriber_t *chassis_sub;
 static Chassis_Upload_Data_s chassis_feedback_data;
 
+/* 用于自旋变速策略的时间变量,后续考虑查表加速 */
+static float t;
+
 /* 私有函数计算的中介变量,设为静态避免参数传递的开销 */
 static float chassis_vx, chassis_vy;     // 将云台系的速度投影到底盘
 static float vt_lf, vt_rf, vt_lb, vt_rb; // 底盘速度解算后的临时输出,待进行限幅
@@ -173,8 +170,8 @@ static void MecanumCalculate()
 {
     vt_lf = -chassis_vx - chassis_vy - chassis_cmd_recv.wz * LF_CENTER;
     vt_rf = -chassis_vx + chassis_vy - chassis_cmd_recv.wz * RF_CENTER;
-    vt_lb = chassis_vx + chassis_vy - chassis_cmd_recv.wz * LB_CENTER;
+    vt_lb =  chassis_vx + chassis_vy - chassis_cmd_recv.wz * LB_CENTER;
-    vt_rb = chassis_vx - chassis_vy - chassis_cmd_recv.wz * RB_CENTER;
+    vt_rb =  chassis_vx - chassis_vy - chassis_cmd_recv.wz * RB_CENTER;
 }
 
 /**
@@ -215,31 +212,31 @@ void ChassisTask()
     // 后续增加不同状态的过渡模式?
     switch (chassis_cmd_recv.chassis_mode)
     {
-    case CHASSIS_NO_FOLLOW:
-        chassis_cmd_recv.wz = 0; // 云台任意移动模式,不旋转,一般用于调整云台姿态
-        break;
-    case CHASSIS_FOLLOW_GIMBAL_YAW:
-        chassis_cmd_recv.wz = 0.05f * powf(chassis_cmd_recv.wz, 2.0f); // 跟随,不开pid,以误差角平方为输出
-        break;
-    case CHASSIS_ROTATE:
-        // chassis_cmd_recv.wz // 当前维持定值,后续增加不规则的变速策略
-        break;
     case CHASSIS_ZERO_FORCE:
         DJIMotorStop(motor_lf); // 如果出现重要模块离线或遥控器设置为急停,让电机停止
         DJIMotorStop(motor_rf);
         DJIMotorStop(motor_lb);
         DJIMotorStop(motor_rb);
         break;
+    case CHASSIS_NO_FOLLOW:
+        chassis_cmd_recv.wz = 0; // 底盘不旋转,但维持全向机动,一般用于调整云台姿态
+        break;
+    case CHASSIS_FOLLOW_GIMBAL_YAW:
+        chassis_cmd_recv.wz = 0.05f * powf(chassis_cmd_recv.wz, 2.0f); // 跟随,不单独设置pid,以误差角平方为速度输出
+        break;
+    case CHASSIS_ROTATE:
+        // chassis_cmd_recv.wz=sin(t) // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略
+        break;
     default:
         break;
     }
 
     // 根据云台和底盘的角度offset将控制量映射到底盘坐标系上
     // 底盘逆时针旋转为角度正方向;云台命令的方向以云台指向的方向为x,采用右手系(x指向正北时y在正东)
-    chassis_vx = chassis_cmd_recv.vx * arm_cos_f32(chassis_cmd_recv.offset_angle) -
+    chassis_vx = chassis_cmd_recv.vx * arm_cos_f32(chassis_cmd_recv.offset_angle * ANGLE_2_RAD) -
-                 chassis_cmd_recv.vy * arm_sin_f32(chassis_cmd_recv.offset_angle);
+                 chassis_cmd_recv.vy * arm_sin_f32(chassis_cmd_recv.offset_angle * ANGLE_2_RAD);
-    chassis_vy = chassis_cmd_recv.vx * arm_sin_f32(chassis_cmd_recv.offset_angle) -
+    chassis_vy = chassis_cmd_recv.vx * arm_sin_f32(chassis_cmd_recv.offset_angle * ANGLE_2_RAD) -
-                 chassis_cmd_recv.vy * arm_cos_f32(chassis_cmd_recv.offset_angle);
+                 chassis_cmd_recv.vy * arm_cos_f32(chassis_cmd_recv.offset_angle * ANGLE_2_RAD);
 
     // 根据控制模式进行正运动学解算,计算底盘输出
     MecanumCalculate();
@@ -251,8 +248,9 @@ void ChassisTask()
     EstimateSpeed();
 
     // 获取裁判系统数据
-    //  我方颜色id小于7是红色,大于7是蓝色,注意这里发送的是对方的颜色, 0:blue , 1:red
+    // 我方颜色id小于7是红色,大于7是蓝色,注意这里发送的是对方的颜色, 0:blue , 1:red
-    chassis_feedback_data.enemy_color = referee_data->GameRobotStat.robot_id > 7 ? 1 : 0; //
+    chassis_feedback_data.enemy_color = referee_data->GameRobotStat.robot_id > 7 ? 1 : 0; 
+    // 当前只做了17mm的数据获取,后续根据robot_def中的宏切换双枪管和英雄42mm的情况
     chassis_feedback_data.bullet_speed = referee_data->GameRobotStat.shooter_id1_17mm_speed_limit;
     chassis_feedback_data.rest_heat = referee_data->PowerHeatData.shooter_heat0;
 

application/gimbal/gimbal.c

@@ -5,13 +5,9 @@
 #include "message_center.h"
 #include "general_def.h"
 
-/* 根据每个机器人进行设定,若对云台有改动需要修改 */
-#define YAW_CHASSIS_ALIGN_ECD 0 // 云台和底盘对齐指向相同方向时的电机编码器值
-#define PITCH_HORIZON_ECD 0     // 云台处于水平位置时编码器值
-
 // 自动将编码器转换成角度值
-#define YAW_CHASSIS_ALIGN_ANGLE
+#define YAW_CHASSIS_ALIGN_ANGLE (YAW_CHASSIS_ALIGN_ECD * ECD_ANGLE_COEF)
-#define PTICH_HORIZON_ALIGN_ANGLE
+#define PTICH_HORIZON_ALIGN_ANGLE (PITCH_HORIZON_ECD * ECD_ANGLE_COEF)
 
 static attitude_t *Gimbal_IMU_data;     // 云台IMU数据
 static dji_motor_instance *yaw_motor;   // yaw电机
@@ -81,6 +77,7 @@ void GimbalInit()
         },
         .motor_type = GM6020};
 
+    // 电机对total_angle闭环,上电时为零,会保持静止,收到遥控器数据再动
     yaw_motor = DJIMotorInit(&yaw_config);
     pitch_motor = DJIMotorInit(&pitch_config);
 
@@ -104,29 +101,31 @@ 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(yaw_motor);
         DJIMotorStop(pitch_motor);
         break;
+    // 使用陀螺仪的反馈,底盘根据yaw电机的offset跟随云台或视觉模式采用
     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(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈
         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, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈
-        DJIMotorSetRef(pitch_motor, PITCH_HORIZON_ECD);
+        DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch);
         break;
     default:
         break;
@@ -146,9 +145,9 @@ void GimbalTask()
     }
     */
 
-    // 获取反馈数据
+    // 设置反馈数据
     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_single_round_angle = pitch_motor->motor_measure.angle_single_round;
 
     // 推送消息
     PubPushMessage(gimbal_pub, &gimbal_feedback_data);

application/robot_def.h

@@ -23,7 +23,21 @@
 // #define GIMBAL_BOARD  //云台板
 
 /* 重要参数定义,注意根据不同机器人进行修改 */
-#define YAW_MID_ECD
+// 云台参数
+#define YAW_CHASSIS_ALIGN_ECD 0 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改
+#define PITCH_HORIZON_ECD 0     // 云台处于水平位置时编码器值,若对云台有机械改动需要修改
+// 发射参数
+#define ONE_BULLET_DELTA_ANGLE 0     // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出
+#define REDUCTION_RATIO_LOADER 49.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f
+#define NUM_PER_CIRCLE 1             // 拨盘一圈的装载量
+// 机器人底盘修改的参数,单位为mm(毫米)
+#define WHEEL_BASE 300              // 纵向轴距(前进后退方向)
+#define TRACK_WIDTH 300             // 横向轮距(左右平移方向)
+#define CENTER_GIMBAL_OFFSET_X 0    // 云台旋转中心距底盘几何中心的距离,前后方向,云台位于正中心时默认设为0
+#define CENTER_GIMBAL_OFFSET_Y 0    // 云台旋转中心距底盘几何中心的距离,左右方向,云台位于正中心时默认设为0
+#define RADIUS_WHEEL 60             // 轮子半径
+#define REDUCTION_RATIO_WHEEL 19.0f // 电机减速比,因为编码器量测的是转子的速度而不是输出轴的速度故需进行转换
+
 
 #if (defined(ONE_BOARD) && defined(CHASSIS_BOARD)) || \
     (defined(ONE_BOARD) && defined(GIMBAL_BOARD)) ||  \
@@ -129,13 +143,13 @@ 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; // 弹速枚举
     uint8_t rest_heat;
-    float shoot_rate; //连续发射的射频,unit per s,发/秒
+    float shoot_rate; // 连续发射的射频,unit per s,发/秒
 } Shoot_Ctrl_Cmd_s;
 
 /* ----------------gimbal/shoot/chassis发布的反馈数据----------------*/
@@ -166,7 +180,7 @@ typedef struct
 typedef struct
 {
     attitude_t gimbal_imu_data;
-    uint16_t yaw_motor_ecd;
+    uint16_t yaw_motor_single_round_angle;
 } Gimbal_Upload_Data_s;
 
 typedef struct

application/shoot/shoot.c

@@ -5,9 +5,6 @@
 #include "bsp_dwt.h"
 #include "general_def.h"
 
-#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; // 左摩擦轮
@@ -137,7 +134,7 @@ void ShootTask()
     if (hibernate_time + dead_time > DWT_GetTimeline_ms())
         return;
 
-    // 若不在休眠状态,根据控制模式进行电机参考值设定和模式切换
+    // 若不在休眠状态,根据控制模式进行拨盘电机参考值设定和模式切换
     switch (shoot_cmd_recv.load_mode)
     {
     // 停止拨盘
@@ -162,7 +159,7 @@ void ShootTask()
     // 连发模式,对速度闭环,射频后续修改为可变
     case LOAD_BURSTFIRE:
         DJIMotorOuterLoop(loader, SPEED_LOOP);
-        DJIMotorSetRef(loader, shoot_cmd_recv.shoot_rate * 360 * REDUCTION_RATIO / NUM_PER_CIRCLE);
+        DJIMotorSetRef(loader, shoot_cmd_recv.shoot_rate * 360 * REDUCTION_RATIO_WHEEL / NUM_PER_CIRCLE);
         // x颗/秒换算成速度: 已知一圈的载弹量,由此计算出1s需要转的角度,注意换算角速度
         break;
     // 拨盘反转,对速度闭环,后续增加卡弹检测(通过裁判系统剩余热量反馈)
@@ -175,7 +172,7 @@ void ShootTask()
         break;
     }
 
-    // 根据收到的弹速设置设定摩擦轮参考值,需实测后填入
+    // 根据收到的弹速设置设定摩擦轮电机参考值,需实测后填入
     switch (shoot_cmd_recv.bullet_speed)
     {
     case SMALL_AMU_15:

modules/motor/dji_motor.c

@@ -1,8 +1,6 @@
 #include "dji_motor.h"
 #include "general_def.h"
 
-#define ECD_ANGLE_COEF 0.043945f // 360/8192,将编码器值转化为角度制
-
 static uint8_t idx = 0; // register idx,是该文件的全局电机索引,在注册时使用
 
 /* DJI电机的实例,此处仅保存指针,内存的分配将通过电机实例初始化时通过malloc()进行 */
@@ -134,19 +132,19 @@ static void DecodeDJIMotor(can_instance *_instance)
             // 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->angle_single_round = ECD_ANGLE_COEF * measure->ecd;
-            measure->speed_angle_per_sec = (1 - SPEED_SMOOTH_COEF) * measure->speed_angle_per_sec + 
+            measure->speed_angle_per_sec = (1 - SPEED_SMOOTH_COEF) * measure->speed_angle_per_sec +
                                            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 + 
+            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 rounds calc,计算的前提是两次采样间电机转过的角度小于180°
-            if (measure->ecd - measure->last_ecd > 4096) 
+            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->angle_single_round; 
+            measure->total_angle = measure->total_round * 360 + measure->angle_single_round;
             break;
         }
     }
@@ -168,7 +166,7 @@ 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);
 
@@ -251,13 +249,13 @@ void DJIMotorControl()
             {
                 if (motor_setting->speed_feedback_source == OTHER_FEED)
                     pid_measure = *motor_controller->other_speed_feedback_ptr;
-                else
+                else // MOTOR_FEED
                     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);
             }
 
-            // 计算电流环,只要启用了电流环就计算,不管外层闭环是什么,并且电流只有电机自身的反馈
+            // 计算电流环,只要启用了电流环就计算,不管外层闭环是什么,并且电流只有电机自身传感器的反馈
             if (motor_setting->close_loop_type & CURRENT_LOOP)
             {
                 motor_controller->pid_ref = PID_Calculate(&motor_controller->current_PID, motor_measure->given_current, motor_controller->pid_ref);

modules/motor/dji_motor.h

@@ -24,6 +24,8 @@
 /* 滤波系数设置为1的时候即关闭滤波 */
 #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 ECD_ANGLE_COEF 0.043945f // 360/8192,将编码器值转化为角度制
+
 
 /* DJI电机CAN反馈信息*/
 typedef struct

modules/motor/motor_def.h

@@ -37,8 +37,8 @@ typedef enum
 /* 反馈来源设定,若设为OTHER_FEED则需要指定数据来源指针,详见Motor_Controller_s*/
 typedef enum
 {
-    MOTOR_FEED = 0,
+    MOTOR_FEED,
-    OTHER_FEED = 1
+    OTHER_FEED,
 } Feedback_Source_e;
 
 /* 电机正反转标志 */