#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); }