From 0c0d1201ea69da7bdd0ad05f940ef079cfa13bd0 Mon Sep 17 00:00:00 2001 From: chenfu <2412777093@qq.com> Date: Mon, 20 May 2024 22:22:24 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BA=91=E5=8F=B0=E8=B7=9F=E9=9A=8F=EF=BC=8C?= =?UTF-8?q?=E8=BF=9E=E7=BB=AD=E5=8F=91=E5=B0=84?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .vscode/launch.json | 2 +- .vscode/settings.json | 5 +- application/chassis/balance.c | 79 ++++++------ application/cmd/robot_cmd.c | 222 +++++++++++++++++++++++++++++++++- application/gimbal/gimbal.c | 157 +++++++++++++++++++++++- application/robot_def.h | 10 +- application/robot_task.h | 2 - application/shoot/shoot.c | 190 ++++++++++++++++++++++++++++- bsp/bsp_init.h | 3 +- modules/alarm/buzzer.c | 104 ++++------------ modules/alarm/buzzer.h | 64 ++-------- modules/motor/motor_task.c | 13 +- 12 files changed, 658 insertions(+), 193 deletions(-) diff --git a/.vscode/launch.json b/.vscode/launch.json index 49cd04b..036e9d4 100644 --- a/.vscode/launch.json +++ b/.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 diff --git a/.vscode/settings.json b/.vscode/settings.json index fc2b44b..b4dfa76 100644 --- a/.vscode/settings.json +++ b/.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" } \ No newline at end of file diff --git a/application/chassis/balance.c b/application/chassis/balance.c index 95332e0..c408787 100644 --- a/application/chassis/balance.c +++ b/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 Chassis_Ctrl_Cmd_s chassis_cmd_recv; - +static RC_ctrl_t *rc_data; // 底盘单独调试用 +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 roll_compensate_pid; // roll轴补偿,用于保持机体水平 -static PIDInstance steer_p_pid, steer_v_pid; // 转向PID,有转向指令时使用IMU的加速度反馈积分以获取速度和位置状态量 -static PIDInstance anti_crash_pid; // 抗劈叉,将输出以相反的方向叠加到左右腿的上 +static PIDInstance leglen_pid_l, leglen_pid_r; // 用PD模拟弹簧, 不要积分(弹簧是无积分二阶系统), 增益不可过大否则抗外界冲击响应时太"硬" +static PIDInstance roll_compensate_pid; // roll轴补偿,用于保持机体水平 +static PIDInstance steer_p_pid, steer_v_pid; // 转向PID,有转向指令时使用IMU的加速度反馈积分以获取速度和位置状态量 +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.vx = 0.003 * (float)rc_data[TEMP].rc.rocker_r1; // speed x, unit m/s + 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.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); } \ No newline at end of file diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index 54e6c36..e86b434 100644 --- a/application/cmd/robot_cmd.c +++ b/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); } diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index cb060fb..aae4b61 100644 --- a/application/gimbal/gimbal.c +++ b/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); } \ No newline at end of file diff --git a/application/robot_def.h b/application/robot_def.h index cbdba27..a0f9334 100644 --- a/application/robot_def.h +++ b/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; diff --git a/application/robot_task.h b/application/robot_task.h index 466b430..378d966 100644 --- a/application/robot_task.h +++ b/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); diff --git a/application/shoot/shoot.c b/application/shoot/shoot.c index 7def34a..57353c5 100644 --- a/application/shoot/shoot.c +++ b/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); } \ No newline at end of file diff --git a/bsp/bsp_init.h b/bsp/bsp_init.h index 73cecf8..de8f892 100644 --- a/bsp/bsp_init.h +++ b/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(); } diff --git a/modules/alarm/buzzer.c b/modules/alarm/buzzer.c index 3051ee4..67f30ef 100644 --- a/modules/alarm/buzzer.c +++ b/modules/alarm/buzzer.c @@ -1,93 +1,31 @@ -#include "bsp_pwm.h" #include "buzzer.h" -#include "bsp_dwt.h" -#include "string.h" +#include "main.h" -static PWMInstance *buzzer; -// static uint8_t idx; -static BuzzzerInstance *buzzer_list[BUZZER_DEVICE_CNT] = {0}; +extern TIM_HandleTypeDef htim4; +static uint8_t tmp_warning_level = 0; -/** - * @brief 蜂鸣器初始化 - * - */ void BuzzerInit() { - PWM_Init_Config_s buzzer_config = { - .htim = &htim4, - .channel = TIM_CHANNEL_3, - .dutyratio = 0, - .period = 0.001, - }; - buzzer = PWMRegister(&buzzer_config); + HAL_TIM_PWM_Start(&htim4, TIM_CHANNEL_3); + __HAL_TIM_SetCompare(&htim4, TIM_CHANNEL_3, 0); } -BuzzzerInstance *BuzzerRegister(Buzzer_config_s *config) +void BuzzerOn( ) { - if (config->alarm_level > BUZZER_DEVICE_CNT) // 超过最大实例数,考虑增加或查看是否有内存泄漏 - while (1) - ; - BuzzzerInstance *buzzer_temp = (BuzzzerInstance *)malloc(sizeof(BuzzzerInstance)); - memset(buzzer_temp, 0, sizeof(BuzzzerInstance)); - - 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; - } + static int16_t temp = 4000 ; + if(temp < 1000) + { + BuzzerOff(); + return; } + __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; } diff --git a/modules/alarm/buzzer.h b/modules/alarm/buzzer.h index e63054c..198f6c0 100644 --- a/modules/alarm/buzzer.h +++ b/modules/alarm/buzzer.h @@ -1,60 +1,10 @@ -#ifndef BUZZER_H -#define 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; +#ifndef BSP_BUZZER_H +#define BSP_BUZZER_H +#include void BuzzerInit(); -void BuzzerTask(); -BuzzzerInstance *BuzzerRegister(Buzzer_config_s *config); -void AlarmSetStatus(BuzzzerInstance *buzzer, AlarmState_e state); -#endif // !BUZZER_H +extern void BuzzerOn(); +extern void BuzzerOff(void); + +#endif diff --git a/modules/motor/motor_task.c b/modules/motor/motor_task.c index 18e388d..4d404f9 100644 --- a/modules/motor/motor_task.c +++ b/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(); - - /* 如果有对应的电机则取消注释,可以加入条件编译或者register对应的idx判断是否注册了电机 */ + #ifdef GIMBAL_BOARD + DJIMotorControl(); + #endif // DEBUG + #ifdef CHASSIS_BOARD LKMotorControl(); + #endif // DEBUG + /* 如果有对应的电机则取消注释,可以加入条件编译或者register对应的idx判断是否注册了电机 */ + //LKMotorControl(); // legacy support // 由于ht04电机的反馈方式为接收到一帧消息后立刻回传,以此方式连续发送可能导致总线拥塞