264 lines
11 KiB
C
264 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"
|
||
|
|
#include "user_lib.h"
|
||
|
|
|
||
|
|
/* 对于双发射机构的机器人,将下面的数据封装成结构体即可,生成两份shoot应用实例 */
|
||
|
|
static DJIMotorInstance *TimingBeltMotor_l, *TimingBeltMotor_r, *SpeedControlMotor,*Load_motor;
|
||
|
|
static ServoInstance *Trigger,*load_hit_servo,*load_rf,*load_rb,*load_lf,*load_lb;//扳机
|
||
|
|
|
||
|
|
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的发射控制信息
|
||
|
|
|
||
|
|
static float test;
|
||
|
|
|
||
|
|
void ShootInit()
|
||
|
|
{
|
||
|
|
// 两个同步带轮
|
||
|
|
Motor_Init_Config_s TimingBeltMotor_config = {
|
||
|
|
.can_init_config = {
|
||
|
|
.can_handle = &hcan1,
|
||
|
|
},
|
||
|
|
.controller_param_init_config = {
|
||
|
|
.angle_PID = {
|
||
|
|
.Kp = 8.0f,//1.2
|
||
|
|
.Ki = 0.0f,//0
|
||
|
|
.Kd = 0.04f,//0.05
|
||
|
|
.DeadBand = 0.0f,
|
||
|
|
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
|
||
|
|
.IntegralLimit = 100,
|
||
|
|
.MaxOut = 7000,
|
||
|
|
},
|
||
|
|
.speed_PID = {
|
||
|
|
.Kp = 3,//5000, //4000
|
||
|
|
.Ki = 1.5f,//100.0f, //100
|
||
|
|
.Kd = 0.001f,//0.03f, //0
|
||
|
|
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
|
||
|
|
.IntegralLimit = 10000,
|
||
|
|
.MaxOut = 15000,
|
||
|
|
},
|
||
|
|
},
|
||
|
|
.controller_setting_init_config = {
|
||
|
|
.angle_feedback_source = MOTOR_FEED,
|
||
|
|
.speed_feedback_source = MOTOR_FEED,
|
||
|
|
.outer_loop_type = ANGLE_LOOP,
|
||
|
|
.close_loop_type = SPEED_LOOP|ANGLE_LOOP,
|
||
|
|
.motor_reverse_flag=MOTOR_DIRECTION_REVERSE,
|
||
|
|
},
|
||
|
|
.motor_type = M3508};
|
||
|
|
TimingBeltMotor_config.can_init_config.tx_id = 1,//左同步带
|
||
|
|
TimingBeltMotor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
|
||
|
|
TimingBeltMotor_l = DJIMotorInit(&TimingBeltMotor_config);
|
||
|
|
|
||
|
|
TimingBeltMotor_config.can_init_config.tx_id = 2; // 右同步带轮
|
||
|
|
TimingBeltMotor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
|
||
|
|
TimingBeltMotor_r = DJIMotorInit(&TimingBeltMotor_config);
|
||
|
|
|
||
|
|
// 发射速度控制轮
|
||
|
|
Motor_Init_Config_s SpeedControlMotor_config = {
|
||
|
|
.can_init_config = {
|
||
|
|
.can_handle = &hcan1,
|
||
|
|
.tx_id = 3
|
||
|
|
},
|
||
|
|
.controller_param_init_config = {
|
||
|
|
.angle_PID = {
|
||
|
|
.Kp = 20.0f,//1.2
|
||
|
|
.Ki = 5.0f,//0
|
||
|
|
.Kd = 0.001f,//0.05
|
||
|
|
.DeadBand = 0.0f,
|
||
|
|
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
|
||
|
|
.IntegralLimit = 500,
|
||
|
|
.MaxOut = 80000,
|
||
|
|
},
|
||
|
|
.speed_PID = {
|
||
|
|
.Kp = 2.0f, //4000
|
||
|
|
.Ki = 0.001f, //100
|
||
|
|
.Kd = 0, //0
|
||
|
|
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
|
||
|
|
.IntegralLimit = 10000,
|
||
|
|
.MaxOut = 15000,
|
||
|
|
},
|
||
|
|
},
|
||
|
|
.controller_setting_init_config = {
|
||
|
|
.angle_feedback_source = MOTOR_FEED,
|
||
|
|
.speed_feedback_source = MOTOR_FEED,
|
||
|
|
.outer_loop_type = ANGLE_LOOP, // 初始化成SPEED_LOOP,让拨盘停在原地,防止拨盘上电时乱转
|
||
|
|
.close_loop_type = SPEED_LOOP | ANGLE_LOOP,
|
||
|
|
.motor_reverse_flag = MOTOR_DIRECTION_REVERSE, // 注意方向设置为拨盘的拨出的击发方向
|
||
|
|
//.feedback_reverse_flag = FEEDBACK_DIRECTION_REVERSE,
|
||
|
|
},
|
||
|
|
.motor_type = M2006 // 英雄使用m3508
|
||
|
|
};
|
||
|
|
//装弹2006
|
||
|
|
Motor_Init_Config_s Load_motor_config = {
|
||
|
|
.can_init_config = {
|
||
|
|
.can_handle = &hcan1,
|
||
|
|
.tx_id = 4
|
||
|
|
},
|
||
|
|
.controller_param_init_config = {
|
||
|
|
.angle_PID = {
|
||
|
|
.Kp = 17.0f,//1.2
|
||
|
|
.Ki = 0.0f,//0
|
||
|
|
.Kd = 0.000f,//0.05
|
||
|
|
.DeadBand = 0.0f,
|
||
|
|
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
|
||
|
|
.IntegralLimit = 500,
|
||
|
|
.MaxOut = 80000,
|
||
|
|
},
|
||
|
|
.speed_PID = {
|
||
|
|
.Kp = 2.0f, //4000
|
||
|
|
.Ki = 0.001f, //100
|
||
|
|
.Kd = 0.00, //0
|
||
|
|
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
|
||
|
|
.IntegralLimit = 10000,
|
||
|
|
.MaxOut = 15000,
|
||
|
|
},
|
||
|
|
},
|
||
|
|
.controller_setting_init_config = {
|
||
|
|
.angle_feedback_source = MOTOR_FEED,
|
||
|
|
.speed_feedback_source = MOTOR_FEED,
|
||
|
|
.outer_loop_type = ANGLE_LOOP, // 初始化成SPEED_LOOP,让拨盘停在原地,防止拨盘上电时乱转
|
||
|
|
.close_loop_type = SPEED_LOOP | ANGLE_LOOP,
|
||
|
|
.motor_reverse_flag = MOTOR_DIRECTION_REVERSE, // 注意方向设置为拨盘的拨出的击发方向
|
||
|
|
//.feedback_reverse_flag = FEEDBACK_DIRECTION_REVERSE,
|
||
|
|
|
||
|
|
},
|
||
|
|
|
||
|
|
.motor_type = M2006 // 英雄使用m3508
|
||
|
|
};
|
||
|
|
//扳机舵机的控制
|
||
|
|
Servo_Init_Config_s trigger_config = {
|
||
|
|
.htim = &htim8,
|
||
|
|
.Channel = TIM_CHANNEL_1,
|
||
|
|
.Servo_type = Servo180,
|
||
|
|
.Servo_Angle_Type = Free_Angle_mode,
|
||
|
|
};
|
||
|
|
//装弹4+1个舵机的控制
|
||
|
|
Servo_Init_Config_s load_hit_servo_config = {
|
||
|
|
.htim = &htim8,
|
||
|
|
.Channel = TIM_CHANNEL_2,
|
||
|
|
.Servo_type = Servo180,
|
||
|
|
.Servo_Angle_Type = Free_Angle_mode,
|
||
|
|
};
|
||
|
|
Servo_Init_Config_s load_rf_config = {
|
||
|
|
.htim = &htim1,
|
||
|
|
.Channel = TIM_CHANNEL_1,
|
||
|
|
.Servo_type = Servo180,
|
||
|
|
.Servo_Angle_Type = Free_Angle_mode,
|
||
|
|
};
|
||
|
|
Servo_Init_Config_s load_rb_config = {
|
||
|
|
.htim = &htim1,
|
||
|
|
.Channel = TIM_CHANNEL_2,
|
||
|
|
.Servo_type = Servo180,
|
||
|
|
.Servo_Angle_Type = Free_Angle_mode,
|
||
|
|
};
|
||
|
|
Servo_Init_Config_s load_lf_config = {
|
||
|
|
.htim = &htim1,
|
||
|
|
.Channel = TIM_CHANNEL_3,
|
||
|
|
.Servo_type = Servo180,
|
||
|
|
.Servo_Angle_Type = Free_Angle_mode,
|
||
|
|
};
|
||
|
|
Servo_Init_Config_s load_lb_config = {
|
||
|
|
.htim = &htim1,
|
||
|
|
.Channel = TIM_CHANNEL_4,
|
||
|
|
.Servo_type = Servo180,
|
||
|
|
.Servo_Angle_Type = Free_Angle_mode,
|
||
|
|
};
|
||
|
|
SpeedControlMotor = DJIMotorInit(&SpeedControlMotor_config);
|
||
|
|
Load_motor = DJIMotorInit(&Load_motor_config);
|
||
|
|
|
||
|
|
Trigger = ServoInit(&trigger_config);
|
||
|
|
load_hit_servo=ServoInit(&load_hit_servo_config);
|
||
|
|
|
||
|
|
load_rf = ServoInit(&load_rf_config);//四边形结构舵机
|
||
|
|
load_rb=ServoInit(&load_rb_config);
|
||
|
|
load_lf=ServoInit(&load_lf_config);
|
||
|
|
load_lb=ServoInit(&load_lb_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)
|
||
|
|
{
|
||
|
|
/* DJIMotorSetRef(friction_l, 0);
|
||
|
|
DJIMotorSetRef(friction_r, 0);
|
||
|
|
DJIMotorStop(loader);*/
|
||
|
|
//扳机止动
|
||
|
|
DJIMotorStop(TimingBeltMotor_l);
|
||
|
|
DJIMotorStop(TimingBeltMotor_r);
|
||
|
|
DJIMotorStop(SpeedControlMotor);
|
||
|
|
DJIMotorStop(Load_motor);
|
||
|
|
|
||
|
|
}
|
||
|
|
else // 恢复运行
|
||
|
|
{
|
||
|
|
|
||
|
|
DJIMotorEnable(TimingBeltMotor_l);
|
||
|
|
DJIMotorEnable(TimingBeltMotor_r);
|
||
|
|
DJIMotorEnable(SpeedControlMotor);
|
||
|
|
|
||
|
|
DJIMotorSetRef(TimingBeltMotor_r,shoot_cmd_recv.TimingBeltMotorAngle);
|
||
|
|
DJIMotorSetRef(TimingBeltMotor_l,shoot_cmd_recv.TimingBeltMotorAngle);
|
||
|
|
DJIMotorSetRef(SpeedControlMotor,shoot_cmd_recv.SpeedControlMotorAngle);
|
||
|
|
DJIMotorSetRef(Load_motor,shoot_cmd_recv.TimingBeltMotorAngle);
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
// 扳机开关
|
||
|
|
if (shoot_cmd_recv.trigger_mode == TRIGGER_ON)
|
||
|
|
{
|
||
|
|
Servo_Motor_FreeAngle_Set(Trigger,62);//角度待验证
|
||
|
|
}
|
||
|
|
else if (shoot_cmd_recv.trigger_mode == TRIGGER_OFF)
|
||
|
|
{
|
||
|
|
Servo_Motor_FreeAngle_Set(Trigger,78);//角度待验证
|
||
|
|
}
|
||
|
|
|
||
|
|
if (shoot_cmd_recv.load_servo_mode == LOAD_TRIGGER_ON)
|
||
|
|
{
|
||
|
|
Servo_Motor_FreeAngle_Set(load_hit_servo,45);//角度待验证
|
||
|
|
}
|
||
|
|
else if (shoot_cmd_recv.load_servo_mode == LOAD_TRIGGER_OFF)
|
||
|
|
{
|
||
|
|
Servo_Motor_FreeAngle_Set(load_hit_servo,90);//角度待验证
|
||
|
|
}
|
||
|
|
|
||
|
|
if (shoot_cmd_recv.load_structure_mode == LOAD_STRUCTURE_ON)//往下
|
||
|
|
{
|
||
|
|
Servo_Motor_FreeAngle_Set(load_rf,180);//四个舵机的角度要设置好
|
||
|
|
Servo_Motor_FreeAngle_Set(load_rb,180);
|
||
|
|
Servo_Motor_FreeAngle_Set(load_lf,0);
|
||
|
|
Servo_Motor_FreeAngle_Set(load_lb,0);
|
||
|
|
}
|
||
|
|
else if (shoot_cmd_recv.load_structure_mode == LOAD_STRUCTURE_OFF)//往上
|
||
|
|
{
|
||
|
|
Servo_Motor_FreeAngle_Set(load_rf,45);//四个舵机的角度要设置好
|
||
|
|
Servo_Motor_FreeAngle_Set(load_rb,46);
|
||
|
|
Servo_Motor_FreeAngle_Set(load_lf,135);
|
||
|
|
Servo_Motor_FreeAngle_Set(load_lb,135);//角度待验证TODO未在cmd中编写控制方式
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
// 反馈数据,目前暂时没有要设定的反馈数据,后续可能增加应用离线监测以及卡弹反馈
|
||
|
|
PubPushMessage(shoot_pub, (void *)&shoot_feedback_data);
|
||
|
|
}
|