云台跟随，连续发射

.vscode/launch.json

@@ -43,7 +43,7 @@
             "interface": "swd",
             "svdFile": "STM32F407.svd",
             "rtos": "FreeRTOS",
-            "preLaunchTask": "build task",//先运行Build任务,取消注释即可使用
+            //"preLaunchTask": "build task",//先运行Build任务,取消注释即可使用
             "liveWatch": {
                 "enabled": true,
                 "samplesPerSecond": 4

.vscode/settings.json

@@ -5,7 +5,10 @@
         "stdlib.h": "c",
         "bsp_can.h": "c",
         "math.h": "c",
-        "user_lib.h": "c"
+        "user_lib.h": "c",
+        "servo_motor.h": "c",
+        "main.h": "c",
+        "buzzer.h": "c"
     },
     "C_Cpp.errorSquiggles": "disabled"
 }

application/chassis/balance.c

@@ -19,18 +19,18 @@
 #include "lqr_calc.h"
 #include "speed_estimation.h"
 #include "fly_detection.h"
-
+#include "buzzer.h"
 // 计时变量
 static uint32_t balance_dwt_cnt;
 static float del_t;
 
 // 底盘拥有的实例模块
 static INS_t *Chassis_IMU_data;
-static RC_ctrl_t *rc_data;  // 底盘单独调试用
+static RC_ctrl_t *rc_data; // 底盘单独调试用
-static Chassis_Ctrl_Cmd_s chassis_cmd_recv; 
+static Chassis_Ctrl_Cmd_s chassis_cmd_recv;
-
+static Chassis_Upload_Data_s chassis_feedback_data; // 底盘反馈数据
 // 四个关节电机和两个驱动轮电机
-static HTMotorInstance *lf, *lb, *rf, *rb, *joint[4];   // 指针数组方便传参和调试
+static HTMotorInstance *lf, *lb, *rf, *rb, *joint[4]; // 指针数组方便传参和调试
 static LKMotorInstance *l_driven, *r_driven, *driven[2];
 
 // 两个腿的参数,0为左腿,1为右腿
@@ -38,23 +38,35 @@ static LinkNPodParam l_side, r_side;
 static ChassisParam chassis;
 
 // 综合运动补偿的PID控制器
-static PIDInstance leglen_pid_l, leglen_pid_r;     // 用PD模拟弹簧, 不要积分(弹簧是无积分二阶系统), 增益不可过大否则抗外界冲击响应时太"硬"
+static PIDInstance leglen_pid_l, leglen_pid_r; // 用PD模拟弹簧, 不要积分(弹簧是无积分二阶系统), 增益不可过大否则抗外界冲击响应时太"硬"
-static PIDInstance roll_compensate_pid;            // roll轴补偿,用于保持机体水平
+static PIDInstance roll_compensate_pid;        // roll轴补偿,用于保持机体水平
-static PIDInstance steer_p_pid, steer_v_pid;       // 转向PID,有转向指令时使用IMU的加速度反馈积分以获取速度和位置状态量
+static PIDInstance steer_p_pid, steer_v_pid;   // 转向PID,有转向指令时使用IMU的加速度反馈积分以获取速度和位置状态量
-static PIDInstance anti_crash_pid;                 // 抗劈叉,将输出以相反的方向叠加到左右腿的上
+static PIDInstance anti_crash_pid;             // 抗劈叉,将输出以相反的方向叠加到左右腿的上
 
 // 底盘状态
 static Robot_Status_e chassis_status;
 
-static referee_info_t* referee_data; // 用于获取裁判系统的数据
+static referee_info_t *referee_data;       // 用于获取裁判系统的数据
 static Referee_Interactive_info_t ui_data; // UI数据，将底盘中的数据传入此结构体的对应变量中，UI会自动检测是否变化，对应显示UI
 
+static CANCommInstance *cmd_can_comm; // 底盘CAN通信实例
+
 void BalanceInit()
 {
     rc_data = RemoteControlInit(&huart3);
     Chassis_IMU_data = INS_Init();
     referee_data = UITaskInit(&huart6, &ui_data); // 裁判系统初始化,会同时初始化UI
 
+    CANComm_Init_Config_s comm_conf = {
+        .can_config = {
+            .can_handle = &hcan2,
+            .tx_id = 0x311,
+            .rx_id = 0x312,
+        },
+        .recv_data_len = sizeof(Chassis_Ctrl_Cmd_s),
+        .send_data_len = sizeof(Chassis_Upload_Data_s),
+    };
+    cmd_can_comm = CANCommInit(&comm_conf);
     // 关节电机
     Motor_Init_Config_s joint_conf = {
         // 写一个,剩下的修改方向和id即可
@@ -173,7 +185,7 @@ void BalanceInit()
 
     // 状态初始化
     l_side.target_len = r_side.target_len = 0.12;
-    chassis.vel_cov = 100;      // 速度协方差初始化
+    chassis.vel_cov = 100; // 速度协方差初始化
     chassis_status = ROBOT_READY;
     DWT_GetDeltaT(&balance_dwt_cnt);
 }
@@ -191,7 +203,7 @@ static uint8_t JointMotorIsLost()
 {
     for (uint8_t i = 0; i < JOINT_CNT; i++)
     {
-        if(joint[i]->motor_daemon->temp_count == 0)
+        if (joint[i]->motor_daemon->temp_count == 0)
             return 1;
     }
 
@@ -201,9 +213,9 @@ static uint8_t JointMotorIsLost()
 // 检查驱动轮电机是否离线
 static uint8_t DrivenMotorIsLost()
 {
-    for(uint8_t i = 0; i < DRIVEN_CNT; i++)
+    for (uint8_t i = 0; i < DRIVEN_CNT; i++)
     {
-        if(driven[i]->daemon->temp_count == 0)
+        if (driven[i]->daemon->temp_count == 0)
             return 1;
     }
 
@@ -212,7 +224,7 @@ static uint8_t DrivenMotorIsLost()
 
 /* 切换底盘遥控器控制和云台双板控制 */
 static void ControlSwitch()
-{ 
+{
     // 根据裁判系统底盘输出电压设定底盘状态
     float chassis_vol = referee_data->PowerHeatData.chassis_voltage * 0.001;
     if (chassis_vol < 15.0f || JointMotorIsLost() || DrivenMotorIsLost())
@@ -232,21 +244,22 @@ static void ControlSwitch()
         }
         else
         {
-            chassis_cmd_recv.chassis_mode = CHASSIS_FREE_DEBUG;                        // 自由转动&前后
+            chassis_cmd_recv.chassis_mode = CHASSIS_FREE_DEBUG;              // 自由转动&前后
-            chassis_cmd_recv.vx = 0.003 * (float)rc_data[TEMP].rc.rocker_r1;           // speed x, unit m/s
+            chassis_cmd_recv.vx = 0.003 * (float)rc_data[TEMP].rc.rocker_r1; // speed x, unit m/s
             chassis_cmd_recv.delta_leglen = -0.0000005f * (float)rc_data[TEMP].rc.dial;
             chassis_cmd_recv.offset_angle -= 0.000005 * (float)rc_data[TEMP].rc.rocker_r_;
         }
     }
     else
-        chassis_cmd_recv.chassis_mode = CHASSIS_ZERO_FORCE; // 皆离线,急停
+    {
+        chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(cmd_can_comm);
+    }
 }
 
-
 /* 腿缩回复位,只允许驱动轮电机移动 */
 static void ResetChassis()
 {
-    EnableAllMotor();   // 打开全部电机,关节复位到起始角度,驱动电机响应速度输入以从墙角或固连中脱身
+    EnableAllMotor(); // 打开全部电机,关节复位到起始角度,驱动电机响应速度输入以从墙角或固连中脱身
 
     // 目标速度置0
     chassis.target_v = 0;
@@ -278,7 +291,7 @@ static void ResetChassis()
         for (uint8_t i = 0; i < JOINT_CNT; i++)
             HTMotorOuterLoop(joint[i], OPEN_LOOP); // 改回直接开环扭矩输入,让电调对扭矩闭环
 
-        return;                                    // 退出函数不再执行关节指令
+        return; // 退出函数不再执行关节指令
     }
     else
         chassis_status = ROBOT_STOP;
@@ -291,7 +304,6 @@ static void ResetChassis()
     }
 }
 
-
 // 工作状态设定
 static void WokingStateSet()
 {
@@ -334,8 +346,11 @@ static void WokingStateSet()
     chassis.target_v += sign(chassis_cmd_recv.vx - chassis.target_v) * MAX_ACC_REF * del_t;
 
     // 角度输入
-    chassis.target_yaw = chassis_cmd_recv.offset_angle; 
+    if (chassis_cmd_recv.chassis_mode == CHASSIS_FREE_DEBUG)
-
+    {
+        chassis.target_yaw = chassis_cmd_recv.offset_angle;
+    }
+    chassis.target_yaw = chassis.yaw + chassis_cmd_recv.offset_angle*DEGREE_2_RAD;
     // TODO 转向速度限幅
 
     // TODO 最大dist误差限幅
@@ -343,7 +358,6 @@ static void WokingStateSet()
     // TODO 最大速度误差限幅
 }
 
-
 /**
  * @brief 将电机和imu的数据组装为LinkNPodParam结构体和chassisParam结构体
  *
@@ -352,7 +366,7 @@ static void WokingStateSet()
  *
  */
 static void ParamAssemble()
-{ 
+{
     // 机体参数,视为平面刚体
     chassis.pitch = Chassis_IMU_data->Pitch * DEGREE_2_RAD;
     chassis.pitch_w = Chassis_IMU_data->Gyro[0];
@@ -375,16 +389,11 @@ static void ParamAssemble()
     r_side.w_ecd = -r_driven->measure.speed_rads;
 }
 
-
 static void SynthesizeMotion() /* 腿部控制:抗劈叉; 轮子控制:转向 */
 {
-    if (chassis_cmd_recv.chassis_mode == CHASSIS_FREE_DEBUG)
-    {
-        // 双环控制
-        float p_ref = PIDCalculate(&steer_p_pid, chassis.yaw, chassis.target_yaw);
-        PIDCalculate(&steer_v_pid, chassis.wz, p_ref);
-    }
 
+    float p_ref = PIDCalculate(&steer_p_pid, chassis.yaw, chassis.target_yaw);
+    PIDCalculate(&steer_v_pid, chassis.wz, p_ref);
     l_side.T_wheel -= steer_v_pid.Output;
     r_side.T_wheel += steer_v_pid.Output;
 
@@ -396,7 +405,6 @@ static void SynthesizeMotion() /* 腿部控制:抗劈叉; 轮子控制:转向 */
     r_side.T_hip -= anti_crash_pid.Output - swerving_speed_ff;
 }
 
-
 static void LegControl() /* 腿长控制和Roll补偿 */
 {
     PIDCalculate(&roll_compensate_pid, chassis.roll, 0);
@@ -423,7 +431,7 @@ static void WattLimitSet() /* 设定运动模态的输出 */
 void BalanceTask()
 {
     del_t = DWT_GetDeltaT(&balance_dwt_cnt);
-
+    BuzzerOn();
     // 切换遥控器控制or云台板控制
     ControlSwitch();
     // 设置目标参数和工作模式
@@ -457,4 +465,5 @@ void BalanceTask()
 
     // 运动模态,电机输出映射和限幅
     WattLimitSet();
+    CANCommSend(cmd_can_comm, (void *)&chassis_feedback_data);
 }

application/cmd/robot_cmd.c

@@ -9,18 +9,238 @@
 #include "general_def.h"
 #include "dji_motor.h"
 #include "bmi088.h"
+#include "user_lib.h"
+#include "can_comm.h"
 // bsp
 #include "bsp_dwt.h"
 #include "bsp_log.h"
+// 私有宏,自动将编码器转换成角度值
+#define PTICH_HORIZON_ANGLE (PITCH_HORIZON_ECD * ECD_ANGLE_COEF_DJI) // pitch水平时电机的角度,0-360
 
+float yaw_align_angle = 0.0f, yaw_chassis_align_ecd = 2716;
+static Publisher_t *chassis_cmd_pub;             // 底盘控制消息发布者
+static Subscriber_t *chassis_feed_sub;           // 底盘反馈信息订阅者
+static Chassis_Ctrl_Cmd_s chassis_cmd_send;      // 发送给底盘应用的信息,包括控制信息和UI绘制相关
+static Chassis_Upload_Data_s chassis_fetch_data; // 从底盘应用接收的反馈信息信息,底盘功率枪口热量与底盘运动状态等
 
+static RC_ctrl_t *rc_data;              // 遥控器数据,初始化时返回
+static Vision_Recv_s *vision_recv_data; // 视觉接收数据指针,初始化时返回
+static Vision_Send_s vision_send_data;  // 视觉发送数据
+
+static Publisher_t *gimbal_cmd_pub;            // 云台控制消息发布者
+static Subscriber_t *gimbal_feed_sub;          // 云台反馈信息订阅者
+static Gimbal_Ctrl_Cmd_s gimbal_cmd_send;      // 传递给云台的控制信息
+static Gimbal_Upload_Data_s gimbal_fetch_data; // 从云台获取的反馈信息
+
+static Publisher_t *shoot_cmd_pub;           // 发射控制消息发布者
+static Subscriber_t *shoot_feed_sub;         // 发射反馈信息订阅者
+static Shoot_Ctrl_Cmd_s shoot_cmd_send;      // 传递给发射的控制信息
+static Shoot_Upload_Data_s shoot_fetch_data; // 从发射获取的反馈信息
+
+static Robot_Status_e robot_state; // 机器人整体工作状态
+
+static Work_Mode_e vision_work_mode; // 视觉工作模式
+
+static CANCommInstance *cmd_can_comm; // 底盘CAN通信实例
 void RobotCMDInit()
 {
+    rc_data = RemoteControlInit(&huart3);   // 修改为对应串口,注意如果是自研板dbus协议串口需选用添加了反相器的那个
+    vision_recv_data = VisionInit(&huart1); // 视觉通信串口
 
+    gimbal_cmd_pub = PubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));
+    gimbal_feed_sub = SubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));
+    shoot_cmd_pub = PubRegister("shoot_cmd", sizeof(Shoot_Ctrl_Cmd_s));
+    shoot_feed_sub = SubRegister("shoot_feed", sizeof(Shoot_Upload_Data_s));
+
+    CANComm_Init_Config_s comm_conf = {
+        .can_config = {
+            .can_handle = &hcan1,
+            .tx_id = 0x312,
+            .rx_id = 0x311,
+        },
+        .recv_data_len = sizeof(Chassis_Upload_Data_s),
+        .send_data_len = sizeof(Chassis_Ctrl_Cmd_s),
+    };
+    cmd_can_comm = CANCommInit(&comm_conf);
+
+    gimbal_cmd_send.pitch = 0;
+    gimbal_cmd_send.yaw = 0;
+    gimbal_cmd_send.gimbal_mode = GIMBAL_ZERO_FORCE;
+    chassis_cmd_send.chassis_mode = CHASSIS_ZERO_FORCE;
+
+    robot_state = ROBOT_STOP; // 启动时机器人进入工作模式,后续加入所有应用初始化完成之后再进入
 }
 
+/**
+ * @brief 根据gimbal app传回的当前电机角度计算和零位的误差
+ *        单圈绝对角度的范围是0~360,说明文档中有图示
+ *
+ */
+static void CalcOffsetAngle()
+{
+    // 别名angle提高可读性,不然太长了不好看,虽然基本不会动这个函数
+    static float angle;
+    yaw_align_angle = yaw_chassis_align_ecd * ECD_ANGLE_COEF_DJI; // 从底盘获取的yaw电机对齐角度
+    angle = gimbal_fetch_data.yaw_motor_single_round_angle;       // 从云台获取的当前yaw电机单圈角度
+    if (yaw_chassis_align_ecd > 4096)                             // 如果大于180度
+    {
+        if (angle > yaw_align_angle)
+            chassis_cmd_send.offset_angle = angle - yaw_align_angle;
+        else if (angle <= yaw_align_angle && angle >= yaw_align_angle - 180.0f)
+            chassis_cmd_send.offset_angle = angle - yaw_align_angle;
+        else
+            chassis_cmd_send.offset_angle = angle - yaw_align_angle + 360.0f;
+    }
+    else
+    { // 小于180度
+        if (angle > yaw_align_angle && angle <= 180.0f + yaw_align_angle)
+            chassis_cmd_send.offset_angle = angle - yaw_align_angle;
+        else if (angle > 180.0f + yaw_align_angle)
+            chassis_cmd_send.offset_angle = angle - yaw_align_angle - 360.0f;
+        else
+            chassis_cmd_send.offset_angle = angle - yaw_align_angle;
+    }
+}
 
+/**
+ * @brief 控制输入为遥控器(调试时)的模式和控制量设置
+ *
+ */
+static void RemoteControlSet()
+{
+    shoot_cmd_send.bullet_speed = 30;
+    // // 云台参数,确定云台控制数据
+    // if (switch_is_mid(rc_data[TEMP].rc.switch_left)) // 左侧开关状态为[中],视觉模式
+    // {
+    //     gimbal_cmd_send.yaw = ( vision_recv_data->yaw == 0 ? gimbal_cmd_send.yaw : vision_recv_data->yaw );
+    //     gimbal_cmd_send.pitch =( vision_recv_data->pitch == 0 ? gimbal_cmd_send.pitch : vision_recv_data->pitch );
+    // }
+    // 左侧开关状态为[下],或视觉未识别到目标,纯遥控器拨杆控制
+    chassis_cmd_send.vx = 0.0f;
+    if (abs(rc_data[TEMP].rc.rocker_r1) > 300)
+    {
+        chassis_cmd_send.vx = 0.003f * (float)rc_data[TEMP].rc.rocker_r1; // _水平方向
+        yaw_chassis_align_ecd = 2716;
+    }
+    else if (abs(rc_data[TEMP].rc.rocker_r_) > 300)
+    {
+        chassis_cmd_send.vx = 0.003f * (float)rc_data[TEMP].rc.rocker_r_; // _水平方向
+        yaw_chassis_align_ecd = 765;
+    }
+
+    gimbal_cmd_send.yaw -= 0.001f * (float)rc_data[TEMP].rc.rocker_l_;
+    gimbal_cmd_send.pitch -= 0.0004f * (float)rc_data[TEMP].rc.rocker_l1;
+    // 摇杆控制的软件限位
+    gimbal_cmd_send.pitch = float_constrain(gimbal_cmd_send.pitch, -35.0f, 30.0f);
+    // gimbal_cmd_send.yaw = float_constrain(gimbal_cmd_send.yaw, -180.0f, 180.0f);
+
+    // 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整
+
+    // 摩擦轮控制,拨轮向上打为负,向下为正
+    if (rc_data[TEMP].rc.dial < -100) // 向上超过100,打开摩擦轮
+        shoot_cmd_send.friction_mode = FRICTION_ON;
+    else
+        shoot_cmd_send.friction_mode = FRICTION_OFF;
+    // 拨弹控制,遥控器固定为一种拨弹模式,可自行选择
+    if (rc_data[TEMP].rc.dial < -400)
+        shoot_cmd_send.load_mode = LOAD_BURSTFIRE;
+    else
+        shoot_cmd_send.load_mode = LOAD_STOP;
+    // // 射频控制,固定每秒1发,后续可以根据左侧拨轮的值大小切换射频,
+    // if( rc_data[TEMP].rc.switch_left == 3)
+    // {
+    //     shoot_cmd_send.friction_mode = FRICTION_ON;
+    //     if(vision_recv_data->fire_mode ==AUTO_AIM)
+    //         shoot_cmd_send.load_mode = LOAD_1_BULLET;
+    //     else
+    //         shoot_cmd_send.load_mode = LOAD_STOP;
+    // }
+    shoot_cmd_send.shoot_rate = 15;
+}
+
+/**
+ * @brief 输入为键鼠时模式和控制量设置
+ *
+ */
+static void MouseKeySet()
+{
+}
+
+/**
+ * @brief  紧急停止,包括遥控器左上侧拨轮打满/重要模块离线/双板通信失效等
+ *         停止的阈值'300'待修改成合适的值,或改为开关控制.
+ *
+ * @todo   后续修改为遥控器离线则电机停止(关闭遥控器急停),通过给遥控器模块添加daemon实现
+ *
+ */
+static void EmergencyHandler()
+{
+
+    if (switch_is_down(rc_data[TEMP].rc.switch_left) || switch_is_mid(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[下],遥控器控制
+    {
+        if (rc_data[TEMP].rc.dial > 300 || robot_state == ROBOT_STOP) // 还需添加重要应用和模块离线的判断
+        {
+            robot_state = ROBOT_STOP;
+            gimbal_cmd_send.gimbal_mode = GIMBAL_ZERO_FORCE;
+            chassis_cmd_send.chassis_mode = CHASSIS_ZERO_FORCE;
+            shoot_cmd_send.shoot_mode = SHOOT_OFF;
+            shoot_cmd_send.friction_mode = FRICTION_OFF;
+
+            gimbal_cmd_send.yaw = gimbal_fetch_data.gimbal_imu_data.YawTotalAngle; // 急停时设定值保持与实际值同步，避免恢复时疯转
+            gimbal_cmd_send.pitch = gimbal_fetch_data.gimbal_imu_data.Pitch;
+        }
+        // 遥控器右侧开关为[上],恢复正常运行
+        if (switch_is_up(rc_data[TEMP].rc.switch_right))
+        {
+            robot_state = ROBOT_READY;
+            shoot_cmd_send.shoot_mode = SHOOT_ON;
+            gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
+            chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
+        }
+    }
+    else if (switch_is_up(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[上],键盘控制
+    {
+        switch (rc_data[TEMP].key_count[KEY_PRESS_WITH_CTRL][Key_C] % 2) // ctrl+c 进入急停
+        {
+        case 0:
+            robot_state = ROBOT_READY;
+            shoot_cmd_send.shoot_mode = SHOOT_ON;
+            break;
+
+        default:
+            robot_state = ROBOT_STOP;
+            gimbal_cmd_send.gimbal_mode = GIMBAL_ZERO_FORCE;
+            chassis_cmd_send.chassis_mode = CHASSIS_ZERO_FORCE;
+            shoot_cmd_send.shoot_mode = SHOOT_OFF;
+            shoot_cmd_send.friction_mode = FRICTION_OFF;
+            shoot_cmd_send.load_mode = LOAD_STOP;
+
+            gimbal_cmd_send.yaw = gimbal_fetch_data.gimbal_imu_data.YawTotalAngle; // 急停时设定值保持与实际值同步，避免恢复时疯转
+            gimbal_cmd_send.pitch = gimbal_fetch_data.gimbal_imu_data.Pitch;
+            break;
+        }
+    }
+}
+/* 机器人核心控制任务,200Hz频率运行(必须高于视觉发送频率) */
 void RobotCMDTask()
 {
-  
+    chassis_fetch_data = *(Chassis_Upload_Data_s *)CANCommGet(cmd_can_comm);
+
+    SubGetMessage(shoot_feed_sub, &shoot_fetch_data);
+    SubGetMessage(gimbal_feed_sub, &gimbal_fetch_data);
+
+    // 根据gimbal的反馈值计算云台和底盘正方向的夹角,不需要传参,通过static私有变量完成
+    CalcOffsetAngle();
+    // 根据遥控器左侧开关,确定当前使用的控制模式为遥控器调试还是键鼠
+    if (switch_is_down(rc_data[TEMP].rc.switch_left) || switch_is_mid(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[下],遥控器控制
+        RemoteControlSet();
+    if (switch_is_up(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[上],键盘控制
+        MouseKeySet();
+
+    EmergencyHandler(); // 处理模块离线和遥控器急停等紧急情况
+
+    CANCommSend(cmd_can_comm, (void *)&chassis_cmd_send);
+
+    PubPushMessage(shoot_cmd_pub, (void *)&shoot_cmd_send);
+    PubPushMessage(gimbal_cmd_pub, (void *)&gimbal_cmd_send);
 }

application/gimbal/gimbal.c

@@ -4,16 +4,169 @@
 #include "ins_task.h"
 #include "message_center.h"
 #include "general_def.h"
+#include "buzzer.h"
 #include "bmi088.h"
 
+static INS_t *gimba_IMU_data; // 云台IMU数据
+static DJIMotorInstance *yaw_motor, *pitch_motor;
+
+static Publisher_t *gimbal_pub;                   // 云台应用消息发布者(云台反馈给cmd)
+static Subscriber_t *gimbal_sub;                  // cmd控制消息订阅者
+static Gimbal_Upload_Data_s gimbal_feedback_data; // 回传给cmd的云台状态信息
+static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv;         // 来自cmd的控制信息
 
 void GimbalInit()
-{   
+{
+    gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源
+    // YAW
+    Motor_Init_Config_s yaw_config = {
+        .can_init_config = {
+            .can_handle = &hcan1,
+            .tx_id = 4,
+        },
+        .controller_param_init_config = {
+            .angle_PID = {
+                .Kp = 0.5, //0.55
+                .Ki = 0.0,//0.05
+                .Kd = 0.0,//0.02
+                .CoefA =0.0,//0.5
+                .CoefB = 0.0,//0.6
+                .Output_LPF_RC = 0,
+                .DeadBand = 0.0,//0.02
+                .Derivative_LPF_RC=0.0,//0.008
+                .Improve = PID_Trapezoid_Intergral |PID_ChangingIntegrationRate| PID_Integral_Limit |PID_Derivative_On_Measurement |  PID_OutputFilter |PID_DerivativeFilter,
+                .IntegralLimit = 0.0,
 
+                .MaxOut = 400,
+            },
+            .speed_PID = {
+                .Kp = 15000,//22000
+                .Ki = 0,//
+                .Kd =0,
+                // .CoefA = 0.8,
+                // .CoefB = 0.1,
+                .Output_LPF_RC = 0.0,//0.002
+                .Improve = PID_Trapezoid_Intergral |PID_Integral_Limit |PID_Derivative_On_Measurement |  PID_OutputFilter,
+                .IntegralLimit = 0,
+                .MaxOut = 20000,
+            },
+            .other_angle_feedback_ptr = &gimba_IMU_data->YawTotalAngle,
+            // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明
+            .other_speed_feedback_ptr=&gimba_IMU_data->Gyro[2],
+        },
+        .controller_setting_init_config = {
+            .angle_feedback_source = OTHER_FEED,
+            .speed_feedback_source = OTHER_FEED,
+            .outer_loop_type = ANGLE_LOOP,
+            .close_loop_type = ANGLE_LOOP | SPEED_LOOP,
+            .motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
+            .feedforward_flag = SPEED_FEEDFORWARD,
+            
+        },
+        .motor_type = GM6020};
+     // PITCH
+    Motor_Init_Config_s pitch_config = {
+        .can_init_config = {
+            .can_handle = &hcan2,
+            .tx_id = 3,
+        },
+        .controller_param_init_config = {
+            .angle_PID = {
+                .Kp =0.4,//0.4
+                .Ki = 0.0,//0.15
+                .Kd = 0.0,//0.006
+                .CoefA = 0.0,//0.5
+                .CoefB = 0.0,//0.6
+                .DeadBand = 0.0,//0.005
+                .Output_LPF_RC = 0.0,//0.01
+                .Improve = PID_Trapezoid_Intergral |PID_ChangingIntegrationRate| PID_Integral_Limit| PID_OutputFilter|PID_DerivativeFilter,
+                .IntegralLimit =1,//1
+                .MaxOut = 600,//600
+            },
+            .speed_PID = {
+                .Kp=10000,//14000
+                .Ki =0,//0
+                .Kd =0.0,//0.0005
+                .CoefA =0,//1500
+                .CoefB =0,//2000
+                .Output_LPF_RC = 0.0,//0.005
+                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit  | PID_OutputFilter,
+                .IntegralLimit =3000,//3000
+                .MaxOut = 20000,//20000
+            },
+            .other_angle_feedback_ptr = &gimba_IMU_data->Pitch,
+            // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明
+            .other_speed_feedback_ptr = (&gimba_IMU_data->Gyro[0]),
+        },
+            .controller_setting_init_config = {
+            .angle_feedback_source = OTHER_FEED,
+            .speed_feedback_source = OTHER_FEED,
+            .outer_loop_type = ANGLE_LOOP,
+            .close_loop_type = ANGLE_LOOP | SPEED_LOOP,
+            .motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
+            .feedback_reverse_flag = FEEDBACK_DIRECTION_NORMAL,
+        },
+        .motor_type = GM6020,
+    };
+    // 电机对total_angle闭环,上电时为零,会保持静止,收到遥控器数据再动
+    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));
 }
 
 /* 机器人云台控制核心任务,后续考虑只保留IMU控制,不再需要电机的反馈 */
 void GimbalTask()
 {
-   
+    BuzzerOn();
+    // 获取云台控制数据
+    // 后续增加未收到数据的处理
+    SubGetMessage(gimbal_sub, &gimbal_cmd_recv);
+    // @todo:现在已不再需要电机反馈,实际上可以始终使用IMU的姿态数据来作为云台的反馈,yaw电机的offset只是用来跟随底盘
+    // 根据控制模式进行电机反馈切换和过渡,视觉模式在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: // 后续只保留此模式
+       // DJIMotorSetFeedfoward(yaw_motor,SPEED_FEEDFORWARD);
+        DJIMotorEnable(yaw_motor);
+        DJIMotorEnable(pitch_motor);
+        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); // yaw和pitch会在robot_cmd中处理好多圈和单圈
+        DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch);
+        break;
+    // 云台自由模式,使用编码器反馈,底盘和云台分离,仅云台旋转,一般用于调整云台姿态(英雄吊射等)/能量机关
+    case GIMBAL_FREE_MODE: // 后续删除,或加入云台追地盘的跟随模式(响应速度更快)
+        DJIMotorEnable(yaw_motor);
+        DJIMotorEnable(pitch_motor);
+        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); // yaw和pitch会在robot_cmd中处理好多圈和单圈
+        DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch);
+        break;
+    default:
+        break;
+    }
+
+    // 在合适的地方添加pitch重力补偿前馈力矩
+    // 根据IMU姿态/pitch电机角度反馈计算出当前配重下的重力矩
+    // ...
+
+    // 设置反馈数据,主要是imu和yaw的ecd
+    gimbal_feedback_data.gimbal_imu_data = *gimba_IMU_data;
+    gimbal_feedback_data.yaw_motor_single_round_angle = yaw_motor->measure.angle_single_round;
+
+    // 推送消息
+    PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data);
 }

application/robot_def.h

@@ -19,14 +19,14 @@
 /* 开发板类型定义,烧录时注意不要弄错对应功能;修改定义后需要重新编译,只能存在一个定义! */
 // #define ONE_BOARD // 单板控制整车
 #define CHASSIS_BOARD //底盘板
-// #define GIMBAL_BOARD  //云台板
+//#define GIMBAL_BOARD  //云台板
 
 #define VISION_USE_VCP  // 使用虚拟串口发送视觉数据
 // #define VISION_USE_UART // 使用串口发送视觉数据
 
 /* 机器人重要参数定义,注意根据不同机器人进行修改,浮点数需要以.0或f结尾,无符号以u结尾 */
 // 云台参数
-#define YAW_CHASSIS_ALIGN_ECD 2711  // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改
+#define YAW_CHASSIS_ALIGN_ECD 2716  // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改
 #define YAW_ECD_GREATER_THAN_4096 0 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度
 #define PITCH_HORIZON_ECD 3412      // 云台处于水平位置时编码器值,若对云台有机械改动需要修改
 #define PITCH_MAX_ANGLE 0           // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
@@ -35,8 +35,8 @@
 // 发射参数
 #define ONE_BULLET_DELTA_ANGLE 36    // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出
 #define REDUCTION_RATIO_LOADER 49.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f
-#define NUM_PER_CIRCLE 10            // 拨盘一圈的装载量
+#define NUM_PER_CIRCLE 12           // 拨盘一圈的装载量
-
+#define LOAD_RATIO 2.5f //中心供弹需要机械减速比
 
 #define GYRO2GIMBAL_DIR_YAW 1   // 陀螺仪数据相较于云台的yaw的方向,1为相同,-1为相反
 #define GYRO2GIMBAL_DIR_PITCH 1 // 陀螺仪数据相较于云台的pitch的方向,1为相同,-1为相反
@@ -203,7 +203,7 @@ typedef struct
 
 typedef struct
 {
-    attitude_t gimbal_imu_data;
+    INS_t gimbal_imu_data;
     uint16_t yaw_motor_single_round_angle;
 } Gimbal_Upload_Data_s;
 

application/robot_task.h

@@ -93,14 +93,12 @@ __attribute__((noreturn)) void StartDAEMONTASK(void const *argument)
 {
     static float daemon_dt;
     static float daemon_start;
-    BuzzerInit();
     LOGINFO("[freeRTOS] Daemon Task Start");
     for (;;)
     {
         // 100Hz
         daemon_start = DWT_GetTimeline_ms();
         DaemonTask();
-        BuzzerTask();
         daemon_dt = DWT_GetTimeline_ms() - daemon_start;
         if (daemon_dt > 10)
             LOGERROR("[freeRTOS] Daemon Task is being DELAY! dt = [%f]", &daemon_dt);

application/shoot/shoot.c

@@ -5,15 +5,203 @@
 #include "message_center.h"
 #include "bsp_dwt.h"
 #include "general_def.h"
+#include "servo_motor.h"
+/* 对于双发射机构的机器人,将下面的数据封装成结构体即可,生成两份shoot应用实例 */
+static DJIMotorInstance *friction_l; // 左摩擦轮
+static DJIMotorInstance *friction_r; // 右摩擦轮
+static DJIMotorInstance *loader;     // 拨盘电机
+static ServoInstance *lid_L; //需要增加弹舱盖
+static ServoInstance *lid_R; //需要增加弹舱盖
 
+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的发射控制信息
 
+// dwt定时,计算冷却用
+static float hibernate_time = 0, dead_time = 0;
+static float load_angle_set = 0;
 void ShootInit()
 {
+    // 左摩擦轮
+    Motor_Init_Config_s friction_config = {
+        .can_init_config = {
+            .can_handle = &hcan2,
+        },
+        .controller_param_init_config = {
+            .speed_PID = {
+                .Kp = 1,//20
+                .Ki = 0,//1
+                .Kd = 0,
+                .Derivative_LPF_RC = 0.02,
+                .Improve = PID_Integral_Limit| PID_Trapezoid_Intergral | PID_DerivativeFilter,
+                .IntegralLimit = 10000,
+                .MaxOut = 15000,
+            },
+            .current_PID = {
+                .Kp = 1.5,//0.7
+                .Ki = 0,//0.1
+                .Kd = 0,
+                .Improve = PID_Integral_Limit,
+                .IntegralLimit = 10000,
+                .MaxOut = 15000,
+            },
+            
+        },
+        .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 ,
+            .motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
+
+        },
+        .motor_type = M3508};
+        friction_config.can_init_config.tx_id = 1,
+        friction_l = DJIMotorInit(&friction_config);
+        
+        friction_config.can_init_config.tx_id = 2; // 右摩擦轮,改txid和方向就行
+        friction_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
+        friction_r = DJIMotorInit(&friction_config); 
     
+    // 拨盘电机
+    Motor_Init_Config_s loader_config = {
+        .can_init_config = {
+            .can_handle = &hcan1,
+            .tx_id = 3,
+        },
+        .controller_param_init_config = {
+            .angle_PID = {
+                // 如果启用位置环来控制发弹,需要较大的I值保证输出力矩的线性度否则出现接近拨出的力矩大幅下降
+                .Kp = 10,//10
+                .Ki = 0,
+                .Kd = 0,
+                .MaxOut = 20000,
+            },
+            .speed_PID = {
+                .Kp = 0.5,//0
+                .Ki = 0.1,//0
+                .Kd = 0.002,
+                .Improve = PID_Integral_Limit,
+                .IntegralLimit = 200,
+                .MaxOut = 10000,
+            },
+            .current_PID = {
+                .Kp = 1.5,//0
+                .Ki = 0,//0
+                .Kd = 0,
+                .Improve = PID_Integral_Limit,
+                .IntegralLimit = 5000,
+                .MaxOut = 10000,
+            },
+        },
+        .controller_setting_init_config = {
+            .angle_feedback_source = MOTOR_FEED, .speed_feedback_source = MOTOR_FEED,
+            .outer_loop_type = SPEED_LOOP, // 初始化成SPEED_LOOP,让拨盘停在原地,防止拨盘上电时乱转
+            .close_loop_type = SPEED_LOOP,
+            .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, // 注意方向设置为拨盘的拨出的击发方向
+        },
+        .motor_type = M2006 // 英雄使用m3508
+    };
+    loader = DJIMotorInit(&loader_config);
+
+    shoot_pub = PubRegister("shoot_feed", sizeof(Shoot_Upload_Data_s));
+    shoot_sub = SubRegister("shoot_cmd", sizeof(Shoot_Ctrl_Cmd_s));
 }
 
 /* 机器人发射机构控制核心任务 */
 void ShootTask()
 {
-    
+    // 从cmd获取控制数据
+    SubGetMessage(shoot_sub, &shoot_cmd_recv);
+
+    // 对shoot mode等于SHOOT_STOP的情况特殊处理,直接停止所有电机(紧急停止)
+    if (shoot_cmd_recv.shoot_mode == SHOOT_OFF)
+    {
+        DJIMotorStop(friction_l);
+        DJIMotorStop(friction_r);
+        DJIMotorStop(loader);
+    }
+    else // 恢复运行
+    {
+        DJIMotorEnable(friction_l);
+        DJIMotorEnable(friction_r);
+        DJIMotorEnable(loader);
+    }
+    if (shoot_cmd_recv.friction_mode == FRICTION_ON)
+    {
+        // 根据收到的弹速设置设定摩擦轮电机参考值,需实测后填入
+        switch (shoot_cmd_recv.bullet_speed)
+        {
+        case SMALL_AMU_15:
+            DJIMotorSetRef(friction_l, 26800);
+            DJIMotorSetRef(friction_r, 26800);
+            break;
+        case SMALL_AMU_18:
+            DJIMotorSetRef(friction_l, 30000);
+            DJIMotorSetRef(friction_r, 30000);
+            break;
+        case SMALL_AMU_30:
+            DJIMotorSetRef(friction_l, 46500);
+            DJIMotorSetRef(friction_r, 46500);
+            break;
+        default: // 当前为了调试设定的默认值4000,因为还没有加入裁判系统无法读取弹速.
+            DJIMotorSetRef(friction_l, 24500);
+            DJIMotorSetRef(friction_r, 24500);
+            break;
+        } 
+    }
+    else // 关闭摩擦轮
+    {
+        DJIMotorSetRef(friction_l, 0);
+        DJIMotorSetRef(friction_r, 0);
+    }
+    // 如果上一次触发单发或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;
+        // 单发模式,根据鼠标按下的时间,触发一次之后需要进入不响应输入的状态(否则按下的时间内可能多次进入,导致多次发射)
+        case LOAD_1_BULLET: // 激活能量机关/干扰对方用,英雄用.
+            load_angle_set = loader->measure.total_angle + ONE_BULLET_DELTA_ANGLE * 36;
+            DJIMotorOuterLoop(loader, ANGLE_LOOP);  // 切换到角度环
+            DJIMotorSetRef(loader, load_angle_set); // 控制量增加一发弹丸的角度
+            hibernate_time = DWT_GetTimeline_ms();  // 记录触发指令的时间
+            dead_time = 500;                        // 完成1发弹丸发射的时间
+            break;
+        // 三连发,如果不需要后续可能删除
+        case LOAD_3_BULLET:
+            load_angle_set = loader->measure.total_angle + ONE_BULLET_DELTA_ANGLE * 36 *3 ;
+            DJIMotorOuterLoop(loader, ANGLE_LOOP);                                                  // 切换到速度环
+            DJIMotorSetRef(loader, load_angle_set); // 增加3发
+            hibernate_time = DWT_GetTimeline_ms();                                                  // 记录触发指令的时间
+            dead_time = 1800;                                                                        // 完成3发弹丸发射的时间
+            break;
+        // 连发模式,对速度闭环,射频后续修改为可变,目前固定为1Hz
+        case LOAD_BURSTFIRE:
+            DJIMotorOuterLoop(loader, SPEED_LOOP); 
+            DJIMotorSetRef(loader, shoot_cmd_recv.shoot_rate * 360 * REDUCTION_RATIO_LOADER * LOAD_RATIO/ NUM_PER_CIRCLE);
+            // x颗/秒换算成速度: 已知一圈的载弹量,由此计算出1s需要转的角度,注意换算角速度(DJIMotor的速度单位是angle per second)
+            break;
+        // 拨盘反转,对速度闭环,后续增加卡弹检测(通过裁判系统剩余热量反馈和电机电流)
+        // 也有可能需要从switch-case中独立出来
+        case LOAD_REVERSE:
+            DJIMotorOuterLoop(loader, SPEED_LOOP);
+            // ...
+            break;
+        default:
+            while (1)
+                ; // 未知模式,停止运行,检查指针越界,内存溢出等问题
+        }
+
+    // 反馈数据,目前暂时没有要设定的反馈数据,后续可能增加应用离线监测以及卡弹反馈
+    PubPushMessage(shoot_pub, (void *)&shoot_feedback_data);
 }

bsp/bsp_init.h

@@ -5,7 +5,7 @@
 #include "bsp_log.h"
 #include "bsp_dwt.h"
 #include "bsp_usb.h"
-
+#include "buzzer.h"
 /**
  * @brief bsp层初始化统一入口,这里仅初始化必须的bsp组件,其他组件的初始化在各自的模块中进行
  *        需在实时系统启动前调用,目前由RobotoInit()调用
@@ -17,6 +17,7 @@ void BSPInit()
 {
     DWT_Init(168);
     BSPLogInit();
+    BuzzerInit();
 }
 
 

modules/alarm/buzzer.c

@@ -1,93 +1,31 @@
-#include "bsp_pwm.h"
 #include "buzzer.h"
-#include "bsp_dwt.h"
+#include "main.h"
-#include "string.h"
 
-static PWMInstance *buzzer;
+extern TIM_HandleTypeDef htim4;
-// static uint8_t idx;
+static uint8_t tmp_warning_level = 0;
-static BuzzzerInstance *buzzer_list[BUZZER_DEVICE_CNT] = {0};
 
-/**
- * @brief 蜂鸣器初始化
- *
- */
 void BuzzerInit()
 {
-    PWM_Init_Config_s buzzer_config = {
+    HAL_TIM_PWM_Start(&htim4, TIM_CHANNEL_3);
-        .htim = &htim4,
+    __HAL_TIM_SetCompare(&htim4, TIM_CHANNEL_3, 0);
-        .channel = TIM_CHANNEL_3,
-        .dutyratio = 0,
-        .period = 0.001,
-    };
-    buzzer = PWMRegister(&buzzer_config);
 }
 
-BuzzzerInstance *BuzzerRegister(Buzzer_config_s *config)
+void BuzzerOn( )
 {
-    if (config->alarm_level > BUZZER_DEVICE_CNT) // 超过最大实例数,考虑增加或查看是否有内存泄漏
+    static int16_t temp = 4000 ;
-        while (1)
+    if(temp < 1000)
-            ;
+    {    
-    BuzzzerInstance *buzzer_temp = (BuzzzerInstance *)malloc(sizeof(BuzzzerInstance));
+        BuzzerOff();
-    memset(buzzer_temp, 0, sizeof(BuzzzerInstance));
+        return;
-
-    buzzer_temp->alarm_level = config->alarm_level;
-    buzzer_temp->loudness = config->loudness;
-    buzzer_temp->octave = config->octave;
-    buzzer_temp->alarm_state = ALARM_OFF;
-
-    buzzer_list[config->alarm_level] = buzzer_temp;
-    return buzzer_temp;
-}
-
-void AlarmSetStatus(BuzzzerInstance *buzzer, AlarmState_e state)
-{
-    buzzer->alarm_state = state;
-}
-
-void BuzzerTask()
-{
-    BuzzzerInstance *buzz;
-    for (size_t i = 0; i < BUZZER_DEVICE_CNT; ++i)
-    {
-        buzz = buzzer_list[i];
-        if (buzz->alarm_level > ALARM_LEVEL_LOW)
-        {
-            continue;
-        }
-        if (buzz->alarm_state == ALARM_OFF)
-        {
-            PWMSetDutyRatio(buzzer, 0);
-        }
-        else
-        {
-            PWMSetDutyRatio(buzzer, buzz->loudness);
-            switch (buzz->octave)
-            {
-            case OCTAVE_1:
-                PWMSetPeriod(buzzer, (float)1 / DoFreq);
-                break;
-            case OCTAVE_2:
-                PWMSetPeriod(buzzer, (float)1 / ReFreq);
-                break;
-            case OCTAVE_3:
-                PWMSetPeriod(buzzer, (float)1 / MiFreq);
-                break;
-            case OCTAVE_4:
-                PWMSetPeriod(buzzer, (float)1 / FaFreq);
-                break;
-            case OCTAVE_5:
-                PWMSetPeriod(buzzer, (float)1 / SoFreq);
-                break;
-            case OCTAVE_6:
-                PWMSetPeriod(buzzer, (float)1 / LaFreq);
-                break;
-            case OCTAVE_7:
-                PWMSetPeriod(buzzer, (float)1 / SiFreq);
-                break;
-            default:
-                break;
-            }
-            break;
-        }
     }
+    __HAL_TIM_PRESCALER(&htim4,(int)(temp/1000));
+    __HAL_TIM_SetCompare(&htim4, TIM_CHANNEL_3, 10000);
+    temp -= 5 ;
+    
+}
+
+void BuzzerOff(void)
+{
+    __HAL_TIM_SetCompare(&htim4, TIM_CHANNEL_3, 0);
+    tmp_warning_level = 0;
 }

modules/alarm/buzzer.h

@@ -1,60 +1,10 @@
-#ifndef BUZZER_H
+#ifndef BSP_BUZZER_H
-#define BUZZER_H
+#define BSP_BUZZER_H
-#include "bsp_pwm.h"
-#define BUZZER_DEVICE_CNT 5
-
-#define  DoFreq  523
-#define  ReFreq  587
-#define  MiFreq  659
-#define  FaFreq  698
-#define  SoFreq  784
-#define  LaFreq  880
-#define  SiFreq  988
-
-typedef enum
-{
-    OCTAVE_1 = 0,
-    OCTAVE_2,
-    OCTAVE_3,
-    OCTAVE_4,
-    OCTAVE_5,
-    OCTAVE_6,
-    OCTAVE_7,
-    OCTAVE_8,
-}octave_e;
-
-typedef enum
-{
-    ALARM_LEVEL_HIGH = 0,
-    ALARM_LEVEL_ABOVE_MEDIUM,
-    ALARM_LEVEL_MEDIUM,
-    ALARM_LEVEL_BELOW_MEDIUM,
-    ALARM_LEVEL_LOW,
-}AlarmLevel_e;
-
-typedef enum
-{
-    ALARM_OFF = 0,
-    ALARM_ON,
-}AlarmState_e;
-typedef struct
-{
-    AlarmLevel_e alarm_level;
-    octave_e octave;
-    float loudness;
-}Buzzer_config_s;
-
-typedef struct
-{
-    float loudness;
-    octave_e octave;
-    AlarmLevel_e alarm_level;
-    AlarmState_e alarm_state;
-}BuzzzerInstance;
 
+#include <stdint.h>
 
 void BuzzerInit();
-void BuzzerTask();
+extern void BuzzerOn();
-BuzzzerInstance *BuzzerRegister(Buzzer_config_s *config);
+extern void BuzzerOff(void);
-void AlarmSetStatus(BuzzzerInstance *buzzer, AlarmState_e state);
+
-#endif // !BUZZER_H
+#endif

modules/motor/motor_task.c

@@ -4,16 +4,21 @@
 #include "dji_motor.h"
 #include "step_motor.h"
 #include "servo_motor.h"
-
+#include "dji_motor.h"
+#include "robot_def.h"
 void MotorControlTask()
 {
     // static uint8_t cnt = 0; 设定不同电机的任务频率
     // if(cnt%5==0) //200hz
     // if(cnt%10==0) //100hz
-    // DJIMotorControl();
+    #ifdef GIMBAL_BOARD
-
+    DJIMotorControl();
-    /* 如果有对应的电机则取消注释,可以加入条件编译或者register对应的idx判断是否注册了电机 */
+    #endif // DEBUG
+    #ifdef CHASSIS_BOARD
     LKMotorControl();
+    #endif // DEBUG
+    /* 如果有对应的电机则取消注释,可以加入条件编译或者register对应的idx判断是否注册了电机 */
+    //LKMotorControl();
 
     // legacy support
     // 由于ht04电机的反馈方式为接收到一帧消息后立刻回传,以此方式连续发送可能导致总线拥塞