// #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, *friction_r, *loader; // 拨盘电机
// static ServoInstance *lid;
//
// static Publisher_t *shoot_pub;
// static Shoot_Ctrl_Cmd_s shoot_cmd_recv; // 来自cmd的发射控制信息
// static Subscriber_t *shoot_sub;
// static Shoot_Upload_Data_s shoot_feedback_data; // 来自cmd的发射控制信息
//
// // dwt定时,计算冷却用
// static float hibernate_time = 0,last_hibernate_time = 0, dead_time = 0;
//
// void ShootInit()
// {
//     // // 左摩擦轮
//     // Motor_Init_Config_s friction_config = {
//     //         .can_init_config = {
//     //                 .can_handle = &hcan2,
//     //         },
//     //         .controller_param_init_config = {
//     //                 .speed_PID = {
//     //                         .Kp = 1.5f,
//     //                         .Ki = 0.2f,
//     //                         .Kd = 0,
//     //                         .Improve = PID_Integral_Limit,
//     //                         .IntegralLimit = 10000,
//     //                         .MaxOut = 15000,
//     //                 },
//     //                 .current_PID = {
//     //                         .Kp = 0,
//     //                         .Ki = 0,
//     //                         .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,
//     //         },
//     //         .motor_type = M3508};
//     // friction_config.can_init_config.tx_id = 2,
//     // friction_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
//     // friction_l = DJIMotorInit(&friction_config);
//     //
//     // friction_config.can_init_config.tx_id = 3; // 右摩擦轮,改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 = &hcan2,
//                     .tx_id = 5
//             },
//             .controller_param_init_config = {
//                     .angle_PID = {
//                             // 如果启用位置环来控制发弹,需要较大的I值保证输出力矩的线性度否则出现接近拨出的力矩大幅下降
//                             .Kp = 500,
//                             .Ki = 100,
//                             .Kd = 0,
//                             .MaxOut = 5000,
//                     },
//                     .speed_PID = {
//                             .Kp = 2, // 3
//                             .Ki = 0, // 1
//                             .Kd = 0,
//                             .Improve = PID_Integral_Limit,
//                             .IntegralLimit = 5000,
//                             .MaxOut = 10000,
//                     },
//                     .current_PID = {
//                             .Kp = 0, // 0.7
//                             .Ki = 0, // 0.1
//                             .Kd = 0,
//                             .Improve = PID_Integral_Limit,
//                             .IntegralLimit = 5000,
//                             .MaxOut = 5000,
//                     },
//             },
//             .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 | ANGLE_LOOP,
//                     .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, // 注意方向设置为拨盘的拨出的击发方向
//                     //.feedback_reverse_flag = FEEDBACK_DIRECTION_REVERSE,
//             },
//             .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));
//
//     // Servo_Init_Config_s lid_config = {
//     //         .htim = &htim1,
//     //         .Channel = TIM_CHANNEL_1,
//     //         .Servo_type = Servo180,
//     //         .Servo_Angle_Type = Free_Angle_mode,
//     // };
//
//     // lid = ServoInit(&lid_config);
//
// //    SPI_Init_Config_s rgb_config = {
// //           .spi_handle = ;
// //           .GPIOx = GPIOx;
// //           .cs_pin = cs_pin;
// //           .spi_work_mode = spi_work_mode;
// //           .callback = callback;
// //           .id = 1;
// //    }
// }
//
// //卡弹检测
// void stalled_detect()
// {
//     static float last_detect_time = 0,detect_time = 0;
//     static float last_total_angle = 0;
//     static uint8_t stalled_cnt = 0;
//
//     detect_time = DWT_GetTimeline_ms();
//
//     //last_detect_time + 200 > detect_time
//     if(detect_time - last_detect_time < 100)  // 200ms 检测一次
//         return;
// //    reference_angle = (detect_time - last_detect_time) * loader->motor_controller.speed_PID.Ref * 1e-3f;
// //    real_angle = loader->measure.total_angle - last_total_angle;
//     if(shoot_cmd_recv.loader_mode == LOAD_BURSTFIRE)
//     {
//         //if(real_angle < reference_angle * 0.2f)
//         if(abs(loader->measure.real_current)>=6500)
//         {
//             //stalled_cnt ++;
//             shoot_feedback_data.stalled_flag = 1;
//
//         }
//         last_detect_time = DWT_GetTimeline_ms();
//     }
// //    last_detect_time = DWT_GetTimeline_ms();
// //    last_total_angle = loader->measure.total_angle;
// }
//
// // 热量控制
// //void Heat_control()
// //{
// //    if(shoot_cmd_recv.heat>=(0.9*shoot_cmd_recv.heat_limit))
// //    {
// //        DJIMotorStop(loader);
// //    }
// //    else
// //    {
// //        DJIMotorEnable(loader);
// //    }
// //}
//
// /* 机器人发射机构控制核心任务 */
// void ShootTask()
// {
//     //从cmd获取控制数据
//     SubGetMessage(shoot_sub, &shoot_cmd_recv);
//
//     // 对shoot mode等于SHOOT_STOP的情况特殊处理,直接停止所有电机(紧急停止)
//     if (shoot_cmd_recv.shoot_mode == SHOOT_OFF)
//     {
//         DJIMotorSetRef(friction_l, 0);
//         DJIMotorSetRef(friction_r, 0);
//         DJIMotorStop(loader);
//     }
//     else // 恢复运行
//     {
//         DJIMotorEnable(friction_l);
//         DJIMotorEnable(friction_r);
//         DJIMotorEnable(loader);
//     }
//
//     // 如果上一次触发单发或3发指令的时间加上不应期仍然大于当前时间(尚未休眠完毕),直接返回即可
//     // 单发模式主要提供给能量机关激活使用(以及英雄的射击大部分处于单发)
//     if (hibernate_time + dead_time > DWT_GetTimeline_ms())
//         return;
//
//     // 若不在休眠状态,根据robotCMD传来的控制模式进行拨盘电机参考值设定和模式切换
//     switch (shoot_cmd_recv.loader_mode)
//     {
//         // 停止拨盘
//         case LOAD_STOP:
//             DJIMotorOuterLoop(loader, SPEED_LOOP); // 切换到速度环
//             DJIMotorSetRef(loader, 0);             // 同时设定参考值为0,这样停止的速度最快
//             break;
//             // 单发模式,根据鼠标按下的时间,触发一次之后需要进入不响应输入的状态(否则按下的时间内可能多次进入,导致多次发射)
//         case LOAD_1_BULLET:                                                                     // 激活能量机关/干扰对方用,英雄用.
//             DJIMotorOuterLoop(loader, ANGLE_LOOP);                              // 切换到角度环
//             DJIMotorSetRef(loader, loader->measure.total_angle - ONE_BULLET_DELTA_ANGLE); // 控制量增加一发弹丸的角度
//             hibernate_time = DWT_GetTimeline_ms();                                              // 记录触发指令的时间
//             dead_time = 150;                                                                    // 完成1发弹丸发射的时间
//             break;
//             // 三连发,如果不需要后续可能删除
//         case LOAD_3_BULLET:
//             DJIMotorOuterLoop(loader, ANGLE_LOOP);                                     // 切换到角度环
//             DJIMotorSetRef(loader, loader->measure.total_angle - 3 * ONE_BULLET_DELTA_ANGLE); // 增加3发
//             hibernate_time = DWT_GetTimeline_ms();                                                  // 记录触发指令的时间
//             dead_time = 1000;                                                                       // 完成3发弹丸发射的时间
//             break;
//             // 连发模式,对速度闭环,射频后续修改为可变,目前固定为1Hz
//         case LOAD_BURSTFIRE:
//             DJIMotorOuterLoop(loader, SPEED_LOOP);
//             DJIMotorSetRef(loader, -(shoot_cmd_recv.shoot_rate * 360 * REDUCTION_RATIO_LOADER / 8));
//
//             // x颗/秒换算成速度: 已知一圈的载弹量,由此计算出1s需要转的角度,注意换算角速度(DJIMotor的速度单位是angle per second)
//             break;
//             // 拨盘反转,对速度闭环,后续增加卡弹检测(通过裁判系统剩余热量反馈和电机电流)
//             // 也有可能需要从switch-case中独立出来
//         case LOAD_REVERSE:
//             DJIMotorOuterLoop(loader, SPEED_LOOP);
//             DJIMotorSetRef(loader, -8 * 360 * REDUCTION_RATIO_LOADER / 8);             // 固定速度反转
//             hibernate_time = DWT_GetTimeline_ms();                                              // 记录触发指令的时间
//             dead_time = 300;                                                                    // 翻转500ms
//             shoot_feedback_data.stalled_flag = 0 ; //清除堵转标志
//             // ...
//             break;
//         default:
//             while (1)
//                 ; // 未知模式,停止运行,检查指针越界,内存溢出等问题
//     }
//
//     // 确定是否开启摩擦轮,后续可能修改为键鼠模式下始终开启摩擦轮(上场时建议一直开启)
// //     if (shoot_cmd_recv.friction_mode == FRICTION_ON)
// //     {
// //         // 根据收到的弹速设置设定摩擦轮电机参考值,需实测后填入
// // //        switch (shoot_cmd_recv.bullet_speed)
// // //        {
// // //            case SMALL_AMU_15:
// // //                DJIMotorSetRef(friction_l, 0);
// // //                DJIMotorSetRef(friction_r, 0);
// // //                break;
// // //            case SMALL_AMU_18:
// // //                DJIMotorSetRef(friction_l, 0);
// // //                DJIMotorSetRef(friction_r, 0);
// // //                break;
// // //            case SMALL_AMU_30:
// // //                DJIMotorSetRef(friction_l, 0);
// // //                DJIMotorSetRef(friction_r, 0);
// // //                break;
// //         //default: // 当前为了调试设定的默认值4000,因为还没有加入裁判系统无法读取弹速.
// //         DJIMotorSetRef(friction_l, 40000);
// //         DJIMotorSetRef(friction_r, 40000);
// // //                break;
// // //        }
// //     }
// //     else // 关闭摩擦轮
// //     {
// //         DJIMotorSetRef(friction_l, 0);
// //         DJIMotorSetRef(friction_r, 0);
// //     }
//
//     // // 开关弹舱盖
//     // if (shoot_cmd_recv.lid_mode == LID_CLOSE)
//     // {
//     //     Servo_Motor_FreeAngle_Set(lid,107);// 小107 大115
//     // }
//     // else if (shoot_cmd_recv.lid_mode == LID_OPEN)
//     // {
//     //     Servo_Motor_FreeAngle_Set(lid,10);
//     // }
//
//     // 开关热控
//     // if(shoot_cmd_recv.heat_mode == HEAT_OPEN)
//     // {
//     //     if(shoot_cmd_recv.heat>=(0.75*shoot_cmd_recv.heat_limit))
//     //     {
//     //         DJIMotorStop(loader);
//     //     }
//     //     else
//     //     {
//     //         DJIMotorEnable(loader);
//     //     }
//     // }
//     // else if(shoot_cmd_recv.heat_mode == HEAT_CLOSE) {
//     //     DJIMotorEnable(loader);
//     // }
//     //卡弹检测
//     stalled_detect();
//     // 热量控制
//     //Heat_control();
//
//     // 反馈数据,目前暂时没有要设定的反馈数据,后续可能增加应用离线监测以及卡弹反馈
//     PubPushMessage(shoot_pub, (void *)&shoot_feedback_data);
// }