diff --git a/.vscode/launch.json b/.vscode/launch.json index 036e9d4..49cd04b 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/application/chassis/balance.c b/application/chassis/balance.c index a26b707..af240a9 100644 --- a/application/chassis/balance.c +++ b/application/chassis/balance.c @@ -20,6 +20,7 @@ #include "speed_estimation.h" #include "fly_detection.h" #include "buzzer.h" + // 计时变量 static uint32_t balance_dwt_cnt; static float del_t; @@ -29,6 +30,7 @@ static INS_t *Chassis_IMU_data; 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 LKMotorInstance *l_driven, *r_driven, *driven[2]; @@ -68,6 +70,7 @@ void BalanceInit() .send_data_len = sizeof(Chassis_Upload_Data_s), }; cmd_can_comm = CANCommInit(&comm_conf); + // 关节电机 Motor_Init_Config_s joint_conf = { // 写一个,剩下的修改方向和id即可 @@ -256,7 +259,6 @@ static void ControlSwitch() // chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(cmd_can_comm); // } chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(cmd_can_comm); - } /* 腿缩回复位,只允许驱动轮电机移动 */ @@ -353,7 +355,8 @@ static void WokingStateSet() { chassis.target_yaw = chassis_cmd_recv.offset_angle; } - chassis.target_yaw = chassis.yaw + chassis_cmd_recv.offset_angle*DEGREE_2_RAD; + chassis.target_yaw = chassis.yaw + chassis_cmd_recv.offset_angle * DEGREE_2_RAD; + // TODO 转向速度限幅 // TODO 最大dist误差限幅 @@ -395,12 +398,12 @@ static void ParamAssemble() static void SynthesizeMotion() /* 腿部控制:抗劈叉; 轮子控制:转向 */ { if(chassis_cmd_recv.chassis_mode == CHASSIS_FREE_DEBUG || - chassis_cmd_recv.chassis_mode == CHASSIS_FOLLOW_GIMBAL_YAW) + chassis_cmd_recv.chassis_mode == CHASSIS_FOLLOW_GIMBAL_YAW) // 底盘跟随 { float p_ref = PIDCalculate(&steer_p_pid, chassis.yaw, chassis.target_yaw); PIDCalculate(&steer_v_pid, chassis.wz, p_ref); } - else if (chassis_cmd_recv.chassis_mode == CHASSIS_ROTATE) + else if (chassis_cmd_recv.chassis_mode == CHASSIS_ROTATE) // 小陀螺 { PIDCalculate(&steer_v_pid, chassis.wz, 6); } @@ -441,6 +444,7 @@ static void WattLimitSet() /* 设定运动模态的输出 */ void BalanceTask() { del_t = DWT_GetDeltaT(&balance_dwt_cnt); + BuzzerOn(); // 切换遥控器控制or云台板控制 ControlSwitch(); diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index e3b2f6e..4bf7239 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -14,10 +14,7 @@ // 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绘制相关 @@ -39,11 +36,10 @@ 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通信实例 static chassis_direction_e chassis_direction; // 底盘方向 + void RobotCMDInit() { rc_data = RemoteControlInit(&huart3); // 修改为对应串口,注意如果是自研板dbus协议串口需选用添加了反相器的那个 @@ -84,8 +80,9 @@ void RobotCMDInit() */ static void CalcOffsetAngle() { - //@todo:相差一整圈时会出问题，待修复 + // @todo:相差一整圈时会出问题，待修复 // 别名angle提高可读性,不然太长了不好看,虽然基本不会动这个函数 + uint16_t yaw_chassis_align_ecd; if(chassis_direction == CHASSIS_ALIGN) { yaw_chassis_align_ecd = 2716; @@ -94,9 +91,11 @@ static void CalcOffsetAngle() { yaw_chassis_align_ecd = 765; } - static float angle; + + static float angle, yaw_align_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) @@ -107,7 +106,7 @@ static void CalcOffsetAngle() chassis_cmd_send.offset_angle = angle - yaw_align_angle + 360.0f; } else - { // 小于180度 + { // 小于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) @@ -123,16 +122,22 @@ static void CalcOffsetAngle() */ static void RemoteControlSet() { - shoot_cmd_send.bullet_speed = 30; gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; - chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW; - // // 云台参数,确定云台控制数据 - // 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 ); - // } - // 左侧开关状态为[下],或视觉未识别到目标,纯遥控器拨杆控制 + + // 云台参数,确定云台控制数据 + gimbal_cmd_send.yaw -= 0.001f * (float)rc_data[TEMP].rc.rocker_l_; + gimbal_cmd_send.pitch -= 0.0006f * (float)rc_data[TEMP].rc.rocker_l1; + + // 摇杆控制的软件限位 + gimbal_cmd_send.pitch = float_constrain(gimbal_cmd_send.pitch, PITCH_MIN_ANGLE, PITCH_MAX_ANGLE); + + if (switch_is_down(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 ); + } + + // 底盘参数 if (abs(rc_data[TEMP].rc.rocker_r1) > 500) { chassis_direction = CHASSIS_ALIGN; @@ -142,24 +147,23 @@ static void RemoteControlSet() chassis_direction = CHASSIS_SIDLE; } - // 右拨杆拨下去,底盘旋转 + // 右侧开关状态为[下],底盘旋转 if (switch_is_down(rc_data[TEMP].rc.switch_right)) { chassis_cmd_send.chassis_mode = CHASSIS_ROTATE; + chassis_cmd_send.vx = 0; + } + else + { + chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW; + chassis_cmd_send.vx = 0.003f * (-(float)rc_data[TEMP].rc.rocker_r_ + (float)rc_data[TEMP].rc.rocker_r1); } + // 左侧开关状态为[中]，腿长控制 if (switch_is_mid(rc_data[TEMP].rc.switch_left)) { chassis_cmd_send.delta_leglen = -0.000001f * (abs(rc_data[TEMP].rc.dial) > 100 ? (float)rc_data[TEMP].rc.dial : 0); } - chassis_cmd_send.vx = 0.003f * ((float)rc_data[TEMP].rc.rocker_r_ + (float)rc_data[TEMP].rc.rocker_r1); - - gimbal_cmd_send.yaw -= 0.001f * (float)rc_data[TEMP].rc.rocker_l_; - gimbal_cmd_send.pitch -= 0.0006f * (float)rc_data[TEMP].rc.rocker_l1; - // 摇杆控制的软件限位 - gimbal_cmd_send.pitch = float_constrain(gimbal_cmd_send.pitch, PITCH_MIN_ANGLE, PITCH_MAX_ANGLE); - // gimbal_cmd_send.yaw = float_constrain(gimbal_cmd_send.yaw, -90.0f, 90+.0f); - // 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整 // 摩擦轮控制,拨轮向上打为负,向下为正 if (switch_is_down(rc_data[TEMP].rc.switch_left)) @@ -168,6 +172,7 @@ static void RemoteControlSet() 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; @@ -175,15 +180,8 @@ static void RemoteControlSet() 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.bullet_speed = 30; shoot_cmd_send.shoot_rate = 15; } @@ -200,7 +198,15 @@ static void MouseKeySet() gimbal_cmd_send.pitch = float_constrain(gimbal_cmd_send.pitch, PITCH_MIN_ANGLE, PITCH_MAX_ANGLE); - chassis_cmd_send.vx = BALANCE_MAX_SPEED * (float)(rc_data[TEMP].key[KEY_PRESS].w - rc_data[TEMP].key[KEY_PRESS].s - rc_data[TEMP].key[KEY_PRESS].a + rc_data[TEMP].key[KEY_PRESS].d); + if(chassis_cmd_send.chassis_mode == CHASSIS_FOLLOW_GIMBAL_YAW) + { + chassis_cmd_send.vx = BALANCE_MAX_SPEED * (float)(rc_data[TEMP].key[KEY_PRESS].w - rc_data[TEMP].key[KEY_PRESS].s + - rc_data[TEMP].key[KEY_PRESS].a + rc_data[TEMP].key[KEY_PRESS].d); + } + else if (chassis_cmd_send.chassis_mode == CHASSIS_ROTATE) + { + chassis_cmd_send.vx = 0; + } chassis_cmd_send.delta_leglen = (float)(rc_data[TEMP].key[KEY_PRESS].e - rc_data[TEMP].key[KEY_PRESS].c) * 0.001f; @@ -233,6 +239,16 @@ static void MouseKeySet() break; } + if(shoot_cmd_send.friction_mode == FRICTION_ON) + { + if (rc_data[TEMP].mouse.press_l) + shoot_cmd_send.load_mode = LOAD_BURSTFIRE; + else + shoot_cmd_send.load_mode = LOAD_STOP; + } + else + shoot_cmd_send.load_mode = LOAD_STOP; + if (rc_data[TEMP].mouse.press_r) { gimbal_cmd_send.yaw = (vision_recv_data->yaw == 0 ? gimbal_cmd_send.yaw : vision_recv_data->yaw); @@ -249,7 +265,6 @@ static void MouseKeySet() */ 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 && switch_is_down(rc_data[TEMP].rc.switch_left)) || robot_state == ROBOT_STOP) // 还需添加重要应用和模块离线的判断 @@ -259,6 +274,7 @@ static void EmergencyHandler() 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; @@ -270,7 +286,6 @@ static void EmergencyHandler() shoot_cmd_send.shoot_mode = SHOOT_ON; gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW; - chassis_cmd_send.direction = CHASSIS_ALIGN; } } else if (switch_is_up(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[上],键盘控制 @@ -291,7 +306,6 @@ static void EmergencyHandler() shoot_cmd_send.shoot_mode = SHOOT_OFF; shoot_cmd_send.friction_mode = FRICTION_OFF; shoot_cmd_send.load_mode = LOAD_STOP; - chassis_cmd_send.direction = CHASSIS_ALIGN; gimbal_cmd_send.yaw = gimbal_fetch_data.gimbal_imu_data.YawTotalAngle; // 急停时设定值保持与实际值同步，避免恢复时疯转 gimbal_cmd_send.pitch = gimbal_fetch_data.gimbal_imu_data.Pitch; diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index 9b1851c..a809443 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -36,7 +36,6 @@ void GimbalInit() .Derivative_LPF_RC=0,//0.01 .Improve = PID_Trapezoid_Intergral |PID_ChangingIntegrationRate| PID_Integral_Limit |PID_Derivative_On_Measurement | PID_OutputFilter |PID_DerivativeFilter, .IntegralLimit = 4.0, - .MaxOut = 20, }, .speed_PID = { @@ -61,7 +60,6 @@ void GimbalInit() .close_loop_type = ANGLE_LOOP | SPEED_LOOP, .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, .feedforward_flag = SPEED_FEEDFORWARD, - }, .motor_type = GM6020}; // PITCH diff --git a/application/robot_def.h b/application/robot_def.h index f837b1b..0a0afbd 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -18,8 +18,8 @@ /* 开发板类型定义,烧录时注意不要弄错对应功能;修改定义后需要重新编译,只能存在一个定义! */ // #define ONE_BOARD // 单板控制整车 -#define CHASSIS_BOARD //底盘板 -// #define GIMBAL_BOARD //云台板 +// #define CHASSIS_BOARD //底盘板 +#define GIMBAL_BOARD //云台板 #define VISION_USE_VCP // 使用虚拟串口发送视觉数据 // #define VISION_USE_UART // 使用串口发送视觉数据 @@ -34,10 +34,10 @@ #define PITCH_MIN_ANGLE (-30.0f) // 云台竖直方向最小角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度) // 发射参数 -#define ONE_BULLET_DELTA_ANGLE 36 // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出 -#define REDUCTION_RATIO_LOADER 49.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f +#define ONE_BULLET_DELTA_ANGLE 75 // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出 +#define REDUCTION_RATIO_LOADER 36.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f #define NUM_PER_CIRCLE 12 // 拨盘一圈的装载量 -#define LOAD_RATIO 2.5f //中心供弹需要机械减速比 +#define LOAD_RATIO 2.5f // 中心供弹需要机械减速比 #define GYRO2GIMBAL_DIR_YAW 1 // 陀螺仪数据相较于云台的yaw的方向,1为相同,-1为相反 #define GYRO2GIMBAL_DIR_PITCH 1 // 陀螺仪数据相较于云台的pitch的方向,1为相同,-1为相反 diff --git a/application/shoot/shoot.c b/application/shoot/shoot.c index 57353c5..9416679 100644 --- a/application/shoot/shoot.c +++ b/application/shoot/shoot.c @@ -1,17 +1,15 @@ #include "shoot.h" #include "robot_def.h" - #include "dji_motor.h" #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的发射控制信息 @@ -21,6 +19,7 @@ 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() { // 左摩擦轮 @@ -46,16 +45,13 @@ void ShootInit() .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, @@ -157,50 +153,50 @@ void ShootTask() 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) - ; // 未知模式,停止运行,检查指针越界,内存溢出等问题 - } + 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; + 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 * 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); diff --git a/modules/motor/motor_task.c b/modules/motor/motor_task.c index 4d404f9..f727257 100644 --- a/modules/motor/motor_task.c +++ b/modules/motor/motor_task.c @@ -11,12 +11,14 @@ void MotorControlTask() // static uint8_t cnt = 0; 设定不同电机的任务频率 // if(cnt%5==0) //200hz // if(cnt%10==0) //100hz + #ifdef GIMBAL_BOARD - DJIMotorControl(); + DJIMotorControl(); #endif // DEBUG #ifdef CHASSIS_BOARD - LKMotorControl(); + LKMotorControl(); #endif // DEBUG + /* 如果有对应的电机则取消注释,可以加入条件编译或者register对应的idx判断是否注册了电机 */ //LKMotorControl();