275 lines
11 KiB
C
275 lines
11 KiB
C
#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, // 20
|
||
.Ki = 0.2f, // 1
|
||
.Kd = 0,
|
||
.Improve = PID_Integral_Limit,
|
||
.IntegralLimit = 10000,
|
||
.MaxOut = 15000,
|
||
},
|
||
.current_PID = {
|
||
.Kp = 0, // 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,
|
||
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
|
||
},
|
||
.motor_type = M3508};
|
||
friction_config.can_init_config.tx_id = 2,
|
||
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 = 1
|
||
},
|
||
.controller_param_init_config = {
|
||
.angle_PID = {
|
||
// 如果启用位置环来控制发弹,需要较大的I值保证输出力矩的线性度否则出现接近拨出的力矩大幅下降
|
||
.Kp = 500, // 10
|
||
.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 = ANGLE_LOOP | SPEED_LOOP,
|
||
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL, // 注意方向设置为拨盘的拨出的击发方向
|
||
},
|
||
.motor_type = M2006 // 英雄使用m3508，碳板用2006
|
||
};
|
||
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 < 200) // 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.load_mode == LOAD_BURSTFIRE)
|
||
{
|
||
//if(real_angle < reference_angle * 0.2f)
|
||
if(abs(loader->measure.real_current)>=9500)
|
||
{
|
||
//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 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);
|
||
}
|
||
|
||
// 如果上一次触发单发或3发指令的时间加上不应期仍然大于当前时间(尚未休眠完毕),直接返回即可
|
||
// 单发模式主要提供给能量机关激活使用(以及英雄的射击大部分处于单发)
|
||
if (hibernate_time + dead_time > DWT_GetTimeline_ms())
|
||
return;
|
||
|
||
// 若不在休眠状态,根据robotCMD传来的控制模式进行拨盘电机参考值设定和模式切换
|
||
switch (shoot_cmd_recv.load_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 = 100; // 翻转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, 39000);
|
||
DJIMotorSetRef(friction_r, 39000);
|
||
// break;
|
||
// }
|
||
}
|
||
else // 关闭摩擦轮
|
||
{
|
||
DJIMotorSetRef(friction_l, 0);
|
||
DJIMotorSetRef(friction_r, 0);
|
||
}
|
||
|
||
// 开关弹舱盖
|
||
if (shoot_cmd_recv.lid_mode == LID_CLOSE)
|
||
{
|
||
Servo_Motor_FreeAngle_Set(lid,95);
|
||
//...
|
||
}
|
||
else if (shoot_cmd_recv.lid_mode == LID_OPEN)
|
||
{
|
||
Servo_Motor_FreeAngle_Set(lid,20);
|
||
}
|
||
//卡弹检测
|
||
stalled_detect();
|
||
|
||
|
||
// DJIMotorEnable(friction_l);
|
||
// DJIMotorEnable(friction_r);
|
||
// DJIMotorSetRef(friction_l, 30000);
|
||
// DJIMotorSetRef(friction_r, 30000);
|
||
|
||
// 反馈数据,目前暂时没有要设定的反馈数据,后续可能增加应用离线监测以及卡弹反馈
|
||
PubPushMessage(shoot_pub, (void *)&shoot_feedback_data);
|
||
} |