#ifdef __cplusplus extern "C" { #endif #include "arm.h" #include "robot_def.h" #include "dji_motor.h" #include "dmmotor.h" #include "ins_task.h" #include "message_center.h" #include "general_def.h" #include "bmi088.h" #include "user_lib.h" #include "bsp_log.h" #include "referee_VT.h" #ifdef __cplusplus } #endif #include "matrix.h" #include "robotics.h" static DJIMotorInstance *Joint1LeftMotor,*Joint1RightMotor,*Joint2Motor,*Joint56Motor,*Joint65Motor;//定义电机 static DMMotorInstance *Joint3Motor,*Joint4Motor; float arm_gravity_feedforward = 0; float q_set[6] ; //六个关节的目标值 //滤波初始化 first_order_filter_type_t Qset_filter[6]; //关节目标值滤波 //机械臂参数初始化 robotics::Link link[6]; float debug_kp=0,debug_ki=0,debug_kd=0,debug_maxout=6000; static Publisher_t *arm_pub; // 云台应用消息发布者(云台反馈给cmd) static Subscriber_t *arm_sub; // cmd控制消息订阅者 static Arm_Upload_Data_s arm_feedback_data; // 回传给cmd的云台状态信息 static Arm_Ctrl_Cmd_s arm_cmd_recv; // 来自cmd的控制信息 void ArmInit() { Motor_Init_Config_s Joint1LeftMotor_Config = { .controller_param_init_config = { .speed_PID = { .Kp = 3.0f, .Ki = 0.0f, .Kd = 0, .MaxOut = 16384, .Improve = static_cast(PID_Integral_Limit | PID_Derivative_On_Measurement), .IntegralLimit = 10.0F, }, .angle_PID = { .Kp = 13.0f, .Ki = 0.0f, .Kd = 0.0f, .MaxOut = 10000, .Improve = static_cast(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement), .IntegralLimit = 10.0F, }, }, .controller_setting_init_config = { .outer_loop_type = ANGLE_LOOP, .close_loop_type = static_cast(ANGLE_LOOP | SPEED_LOOP), .angle_feedback_source = MOTOR_FEED, .speed_feedback_source = MOTOR_FEED, }, .motor_type = M3508, }; Motor_Init_Config_s Joint1RightMotor_Config = { .controller_param_init_config = { .speed_PID = { .Kp = 3.0f, .Ki = 0.0f, .Kd = 0, .MaxOut = 16384, .Improve = static_cast(PID_Integral_Limit | PID_Derivative_On_Measurement), .IntegralLimit = 10.0F, }, .angle_PID = { .Kp = 13.0f, .Ki = 0.0f, .Kd = 0.0f, .MaxOut = 10000, .Improve = static_cast(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement), .IntegralLimit = 10.0F, }, }, .controller_setting_init_config = { .outer_loop_type = ANGLE_LOOP, .close_loop_type = static_cast(ANGLE_LOOP | SPEED_LOOP), .angle_feedback_source = MOTOR_FEED, .speed_feedback_source = MOTOR_FEED, }, .motor_type = M3508, }; Motor_Init_Config_s Joint2Motor_Config = { .controller_param_init_config = { .speed_PID = { .Kp = 3.5f, .Ki = 0.0f, .Kd = 0, .MaxOut = 16384, .Improve = static_cast(PID_Integral_Limit | PID_Derivative_On_Measurement), .IntegralLimit = 10.0F, }, .angle_PID = { .Kp = 13.0f, .Ki = 0.0f, .Kd = 0.0f, .MaxOut = 16000, .Improve = static_cast(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement), .IntegralLimit = 10.0F, }, }, .controller_setting_init_config = { .outer_loop_type = ANGLE_LOOP, .close_loop_type = static_cast(ANGLE_LOOP | SPEED_LOOP), .angle_feedback_source = MOTOR_FEED, .speed_feedback_source = MOTOR_FEED, }, .motor_type = M3508, }; Motor_Init_Config_s Joint3Motor_Config = { .controller_param_init_config = { .speed_PID = { .Kp = 1.0f, .Ki = 0.0f, .Kd = 0.0f, .MaxOut = 10, .Improve = static_cast(PID_Integral_Limit | PID_Derivative_On_Measurement), .IntegralLimit = 10.0F, }, .angle_PID = { .Kp = 50.0f, .Ki = 0.3f, .Kd = 0.0f, .MaxOut = 20, .Improve = static_cast(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement), .IntegralLimit = 10.0F, }, }, .controller_setting_init_config = { .outer_loop_type = ANGLE_LOOP, .close_loop_type = static_cast(ANGLE_LOOP | SPEED_LOOP), .angle_feedback_source = MOTOR_FEED, .speed_feedback_source = MOTOR_FEED, }, .motor_type = DM4340, }; Motor_Init_Config_s Joint4Motor_Config = { .controller_param_init_config = { .current_feedforward_ptr = &arm_gravity_feedforward, .speed_PID = { .Kp = 1.5f, .Ki = 0.0f, .Kd = 0.0f, .MaxOut = 10, .Improve = static_cast(PID_Integral_Limit | PID_Derivative_On_Measurement), .IntegralLimit = 10.0F, }, .angle_PID = { .Kp = 5.0f, .Ki = 0.0f, .Kd = 0.0f, .MaxOut = 10, .Improve = static_cast(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement), .IntegralLimit = 10.0F, }, }, .controller_setting_init_config = { .outer_loop_type = ANGLE_LOOP, .close_loop_type = static_cast(ANGLE_LOOP | SPEED_LOOP), .angle_feedback_source = MOTOR_FEED, .speed_feedback_source = MOTOR_FEED, .feedforward_flag = CURRENT_FEEDFORWARD, }, .motor_type = DM6006, }; Motor_Init_Config_s Joint56Motor_Config = { .controller_param_init_config = { .speed_PID = { .Kp = 2.0f, .Ki = 0.0f, .Kd = 0.0f, .MaxOut = 10000, .Improve = static_cast(PID_Integral_Limit | PID_Derivative_On_Measurement), .IntegralLimit = 10.0F, }, .angle_PID = { .Kp = 8.0f, .Ki = 0.0f, .Kd = 0.1f, .MaxOut = 4000, .Improve = static_cast(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement), .IntegralLimit = 10.0F, }, }, .controller_setting_init_config = { .outer_loop_type = ANGLE_LOOP, .close_loop_type = static_cast(ANGLE_LOOP | SPEED_LOOP), .angle_feedback_source = MOTOR_FEED, .speed_feedback_source = MOTOR_FEED, }, .motor_type = M2006, }; Motor_Init_Config_s Joint65Motor_Config = { .controller_param_init_config = { .speed_PID = { .Kp = 2.0f, .Ki = 0.0f, .Kd = 0.0f, .MaxOut = 10000, .Improve = static_cast(PID_Integral_Limit | PID_Derivative_On_Measurement), .IntegralLimit = 10.0F, }, .angle_PID = { .Kp = 8.0f, .Ki = 0.0f, .Kd = 0.1f, .MaxOut = 4000, .Improve = static_cast(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement), .IntegralLimit = 10.0F, }, }, .controller_setting_init_config = { .outer_loop_type = ANGLE_LOOP, .close_loop_type = static_cast(ANGLE_LOOP | SPEED_LOOP), .angle_feedback_source = MOTOR_FEED, .speed_feedback_source = MOTOR_FEED, }, .motor_type = M2006, }; Joint1LeftMotor_Config.can_init_config.fdcan_handle = &hfdcan1; Joint1LeftMotor_Config.can_init_config.tx_id = 1; Joint1LeftMotor = DJIMotorInit(&Joint1LeftMotor_Config); Joint1RightMotor_Config.can_init_config.fdcan_handle = &hfdcan1; Joint1RightMotor_Config.can_init_config.tx_id = 2; Joint1RightMotor = DJIMotorInit(&Joint1RightMotor_Config); Joint2Motor_Config.can_init_config.fdcan_handle = &hfdcan1; Joint2Motor_Config.can_init_config.tx_id = 3; Joint2Motor = DJIMotorInit(&Joint2Motor_Config); Joint56Motor_Config.can_init_config.fdcan_handle = &hfdcan1; Joint56Motor_Config.can_init_config.tx_id = 4; Joint56Motor = DJIMotorInit(&Joint56Motor_Config); Joint65Motor_Config.can_init_config.fdcan_handle = &hfdcan1; Joint65Motor_Config.can_init_config.tx_id = 5; Joint65Motor = DJIMotorInit(&Joint65Motor_Config); Joint3Motor_Config.can_init_config.fdcan_handle = &hfdcan1; Joint3Motor_Config.can_init_config.tx_id = 6; Joint3Motor_Config.can_init_config.rx_id = 7; Joint3Motor = DMMotorInit(&Joint3Motor_Config); Joint4Motor_Config.can_init_config.fdcan_handle = &hfdcan1; Joint4Motor_Config.can_init_config.tx_id = 8; Joint4Motor_Config.can_init_config.rx_id = 9; Joint4Motor = DMMotorInit(&Joint4Motor_Config); //上传并接收控制中心的命令 arm_pub = PubRegister("arm_feed", sizeof(Arm_Upload_Data_s)); arm_sub = SubRegister("arm_cmd", sizeof(Arm_Ctrl_Cmd_s)); } void ArmTask() { // 获取云台控制数据 // 后续增加未收到数据的处理 SubGetMessage(arm_sub, &arm_cmd_recv); //debug控制参数 // Joint1LeftMotor->motor_controller.angle_PID.Kp=Joint1RightMotor->motor_controller.angle_PID.Kp = debug_kp; // Joint1LeftMotor->motor_controller.angle_PID.Ki=Joint1RightMotor->motor_controller.angle_PID.Ki = debug_ki; // Joint1LeftMotor->motor_controller.angle_PID.Kd=Joint1RightMotor->motor_controller.angle_PID.Kd = debug_kd; // Joint1LeftMotor->motor_controller.angle_PID.MaxOut=Joint1RightMotor->motor_controller.angle_PID.MaxOut = debug_maxout; /* 控制参数计算 ------------------------------------------------------------------------*/ //腕部pitch重力补偿 arm_gravity_feedforward = 1.5f * arm_sin_f32(Joint4Motor->measure.position); if(arm_cmd_recv.gimbal_mode == GIMBAL_ZERO_FORCE) { DMMotorStop(Joint3Motor);DMMotorStop(Joint4Motor); DJIMotorStop(Joint1LeftMotor);DJIMotorStop(Joint1RightMotor); DJIMotorStop(Joint56Motor);DJIMotorStop(Joint65Motor); DJIMotorStop(Joint2Motor); //不使能模式下 目标值为当前值 防止使能甩大臂 q_set[3] = Joint4Motor->measure.position; } else { DMMotorEnable(Joint3Motor);DMMotorEnable(Joint4Motor); DJIMotorEnable(Joint1LeftMotor);DJIMotorEnable(Joint1RightMotor); DJIMotorEnable(Joint56Motor);DJIMotorEnable(Joint65Motor); DJIMotorEnable(Joint2Motor); } if(arm_cmd_recv.gimbal_mode == ARM_MANUAL_MODE) //各关节分开控制 { q_set[0] += arm_cmd_recv.joint1 * DEGREE_2_RAD; q_set[1] += arm_cmd_recv.joint2 * DEGREE_2_RAD; q_set[2] += arm_cmd_recv.joint3 * DEGREE_2_RAD; q_set[3] += arm_cmd_recv.joint4 * DEGREE_2_RAD; q_set[4] += arm_cmd_recv.joint5 * DEGREE_2_RAD; q_set[5] -= arm_cmd_recv.joint6 * DEGREE_2_RAD; } //先暂时随便限位一下 q_set[2] = float_constrain(q_set[2],JOINT3_MIN* DEGREE_2_RAD,JOINT3_MAX* DEGREE_2_RAD); q_set[3] = float_constrain(q_set[3],JOINT4_MIN* DEGREE_2_RAD,JOINT4_MAX* DEGREE_2_RAD); //q_set[0]需要映射到3508的角度上 DJIMotorSetRef(Joint1LeftMotor,q_set[0]); DJIMotorSetRef(Joint1RightMotor,q_set[0]); //q_set[1]也需要映射到3508的角度上 DJIMotorSetRef(Joint2Motor,q_set[1]); DMMotorSetRef(Joint3Motor,q_set[2]); DMMotorSetRef(Joint4Motor,q_set[3]); DJIMotorSetRef(Joint56Motor,q_set[4]+q_set[5]); DJIMotorSetRef(Joint65Motor,q_set[4]-q_set[5]); // 设置反馈数据 // 推送消息 PubPushMessage(arm_pub, (void *)&arm_feedback_data); }