#include "to_stretch.h" #include "robot_def.h" #include "dji_motor.h" #include "message_center.h" #include "referee_task.h" #include "general_def.h" #include "bsp_dwt.h" #include "referee_UI.h" #include "arm_math.h" #include "user_lib.h" //to_stretch static Publisher_t *to_stretch_pub; // 用于发布伸缩的数据 static Subscriber_t *to_stretch_sub; // 用于订阅伸缩的控制命令 static To_stretch_Ctrl_Cmd_s to_stretch_cmd_recv; // 伸缩接收到的控制命令 static To_stretch_Upload_Data_s to_stretch_feedback_data; // 伸缩回传的反馈数据 static DJIMotorInstance *motor_lu, *motor_ru, *motor_ld, *motor_rd, *tuchuan; float gravity_feedforward = 5000; float l_protract = 0,r_protract = 0; float protract_x = 0; //前伸距离 float l_lift = 0,r_lift = 0; float lift_z = 0; //抬升高度 float ld_offset = 0,rd_offset = 0; float lu_offset = 0,ru_offset = 0; float kp=3,ki=2,kd=0;//调试用 first_order_filter_type_t lu_filter,ru_filter; static float lu_spd,ru_spd; PIDInstance protract_position_loop;//前伸位置环 PIDInstance lift_position_loop;//抬升位置环 void To_stretchInit() { // 上下 Motor_Init_Config_s ud_config = { .can_init_config = { .can_handle = &hcan1, }, .controller_param_init_config = { .speed_PID = { .Kp = 3, .Ki = 2, .Kd = 0, .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, .IntegralLimit = 5000, .MaxOut = 13000, }, .current_feedforward_ptr = &gravity_feedforward, }, .controller_setting_init_config = { .angle_feedback_source = MOTOR_FEED, .speed_feedback_source = MOTOR_FEED, .outer_loop_type = SPEED_LOOP, .close_loop_type = SPEED_LOOP, .feedforward_flag = CURRENT_FEEDFORWARD, }, .motor_type = M3508, }; // 前后 Motor_Init_Config_s fb_config = { .can_init_config = { .can_handle = &hcan1, }, .controller_param_init_config = { .angle_PID = { .Kp = 10.0f, .Ki = 0, .Kd = 0, .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, .IntegralLimit = 100, .MaxOut = 5000, }, .speed_PID = { .Kp = 2, .Ki = 0, .Kd = 0, .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, .IntegralLimit = 2500, .MaxOut = 5000, }, .other_angle_feedback_ptr = &protract_x, }, .controller_setting_init_config = { .angle_feedback_source = OTHER_FEED, .speed_feedback_source = MOTOR_FEED, .outer_loop_type = SPEED_LOOP, .close_loop_type = SPEED_LOOP, }, .motor_type = M3508, }; //上下 ud_config.can_init_config.tx_id = 5; ud_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL; ud_config.controller_setting_init_config.feedback_reverse_flag = FEEDBACK_DIRECTION_NORMAL; ud_config.controller_param_init_config.other_speed_feedback_ptr = &lu_spd; motor_lu = DJIMotorInit(&ud_config); ud_config.can_init_config.tx_id = 6; ud_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE; ud_config.controller_setting_init_config.feedback_reverse_flag = FEEDBACK_DIRECTION_REVERSE; ud_config.controller_param_init_config.other_speed_feedback_ptr = &ru_spd; motor_ru = DJIMotorInit(&ud_config); //前后 fb_config.can_init_config.tx_id = 7; fb_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL; fb_config.controller_setting_init_config.feedback_reverse_flag = FEEDBACK_DIRECTION_NORMAL; motor_ld = DJIMotorInit(&fb_config); fb_config.can_init_config.tx_id = 8; fb_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE; fb_config.controller_setting_init_config.feedback_reverse_flag = FEEDBACK_DIRECTION_REVERSE; motor_rd = DJIMotorInit(&fb_config); const float spd_filter_num = 0.02f; first_order_filter_init(&lu_filter,5e-3f,&spd_filter_num); first_order_filter_init(&ru_filter,5e-3f,&spd_filter_num); PID_Init_Config_s protract_pid_config = { .Kp = 2500.0f, .Ki = 0, .Kd = 0, .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, .IntegralLimit = 100, .MaxOut = 15000, }; PIDInit(&protract_position_loop,&protract_pid_config); PID_Init_Config_s lift_pid_config = { .Kp = 2500.0f, .Ki = 0, .Kd = 0, .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, .IntegralLimit = 100, .MaxOut = 15000, }; PIDInit(&lift_position_loop,&lift_pid_config); // 图传电机 Motor_Init_Config_s tuchuan_config = { .can_init_config = { .can_handle = &hcan2, .tx_id = 1, }, .controller_param_init_config = { .angle_PID = { .Kp = 20, .Ki = 10, .Kd = 0, .MaxOut = 5000, }, .speed_PID = { .Kp = 2.0f, .Ki = 1.0f, .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, .close_loop_type = SPEED_LOOP, .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, }, .motor_type = M2006 }; tuchuan = DJIMotorInit(&tuchuan_config); to_stretch_sub = SubRegister("to_stretch_cmd", sizeof(To_stretch_Ctrl_Cmd_s)); to_stretch_pub = PubRegister("to_stretch_feed", sizeof(To_stretch_Upload_Data_s)); } /* 机器人伸缩控制核心任务 */ void To_stretchTask() { motor_lu->motor_controller.speed_PID.Kp=motor_ru->motor_controller.speed_PID.Kp=kp; motor_lu->motor_controller.speed_PID.Ki=motor_ru->motor_controller.speed_PID.Ki=ki; motor_lu->motor_controller.speed_PID.Kd=motor_ru->motor_controller.speed_PID.Kd=kd; motor_ru->motor_controller.speed_PID.Kp=motor_ru->motor_controller.speed_PID.Kp=kp; motor_ru->motor_controller.speed_PID.Ki=motor_ru->motor_controller.speed_PID.Ki=ki; motor_ru->motor_controller.speed_PID.Kd=motor_ru->motor_controller.speed_PID.Kd=kd; static uint8_t init_flag = FALSE; if(init_flag == FALSE) { ld_offset = motor_ld->measure.total_angle ; rd_offset = motor_rd->measure.total_angle ; lu_offset = motor_lu->measure.total_angle ; ru_offset = motor_ru->measure.total_angle ; init_flag = TRUE; } l_protract = -(motor_ld->measure.total_angle - ld_offset) / 19.0f * DEGREE_2_RAD * 20.0f; //单位mm r_protract = (motor_rd->measure.total_angle - rd_offset) / 19.0f * DEGREE_2_RAD * 20.0f; l_lift = (motor_lu->measure.total_angle - lu_offset) / 19.0f * DEGREE_2_RAD * 20.0f; //单位mm r_lift = -(motor_ru->measure.total_angle - ru_offset) / 19.0f * DEGREE_2_RAD * 20.0f; protract_x = (l_protract+r_protract)/2; lift_z = (l_lift+r_lift)/2; SubGetMessage(to_stretch_sub, &to_stretch_cmd_recv); first_order_filter_cali(&lu_filter,motor_lu->measure.speed_aps); lu_spd = lu_filter.out; first_order_filter_cali(&ru_filter, -motor_ru->measure.speed_aps); ru_spd = ru_filter.out; float fb_speed_set = PIDCalculate(&protract_position_loop,protract_x,to_stretch_cmd_recv.fb); float ud_speed_set = PIDCalculate(&lift_position_loop,lift_z,to_stretch_cmd_recv.ud); if (to_stretch_cmd_recv.to_stretch_mode == TO_STRETCH_ZERO_FORCE) { // 如果出现重要模块离线或遥控器设置为急停,让电机停止 DJIMotorStop(motor_lu); DJIMotorStop(motor_ru); DJIMotorStop(motor_ld); DJIMotorStop(motor_rd); DJIMotorStop(tuchuan); } else { // 正常工作 DJIMotorEnable(motor_lu); DJIMotorEnable(motor_ru); DJIMotorEnable(motor_ld); DJIMotorEnable(motor_rd); DJIMotorEnable(tuchuan); } // 根据模式设定伸缩+图传运动形式 switch (to_stretch_cmd_recv.to_stretch_mode) { case TO_STRETCH_ZERO_FORCE: DJIMotorStop(motor_lu); DJIMotorStop(motor_ru); DJIMotorStop(motor_ld); DJIMotorStop(motor_rd); DJIMotorStop(tuchuan); break; case TO_STRETCH_MODE: DJIMotorEnable(motor_lu); DJIMotorEnable(motor_ru); DJIMotorEnable(motor_ld); DJIMotorEnable(motor_rd); DJIMotorSetRef(motor_lu, ud_speed_set); DJIMotorSetRef(motor_ru, -ud_speed_set); DJIMotorSetRef(motor_ld, -fb_speed_set); DJIMotorSetRef(motor_rd, fb_speed_set); DJIMotorSetRef(tuchuan, to_stretch_cmd_recv.tc); break; case TO_STRETCH_KEEP: DJIMotorEnable(motor_lu); DJIMotorEnable(motor_ru); DJIMotorEnable(motor_ld); DJIMotorEnable(motor_rd); DJIMotorEnable(tuchuan); DJIMotorSetRef(tuchuan, to_stretch_cmd_recv.tc); break; default: break; } // 推送反馈消息 to_stretch_feedback_data.protract_x = protract_x; to_stretch_feedback_data.lift_z = lift_z; #ifdef ONE_BOARD PubPushMessage(to_stretch_pub, (void *)&to_stretch_feedback_data); #endif #ifdef CHASSIS_BOARD CANCommSend(chasiss_can_comm, (void *)&chassis_feedback_data); #endif // CHASSIS_BOARD }