#ifdef __cplusplus extern "C" { #endif #include "gimbal.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 attitude_t *gimba_IMU_data; // 云台IMU数据 static DMMotorInstance *yaw_motor, *pitch_motor_l,*pitch_motor_r, *roll_motor; static DMMotorInstance *diff_r_motor,*diff_l_motor; //pitch轴双6006 双环pid算出力矩 一人一半 static PIDInstance pitch_spd_loop,pitch_angle_loop; static PIDInstance diff_pitch_loop,diff_roll_loop; static PIDInstance diff_pitch_spd_loop,diff_roll_spd_loop; float arm_gravity_feedforward = 0; float GRAVITY_COMP = 6.0; static Publisher_t *gimbal_pub; // 云台应用消息发布者(云台反馈给cmd) static Subscriber_t *gimbal_sub; // cmd控制消息订阅者 static Gimbal_Upload_Data_s gimbal_feedback_data; // 回传给cmd的云台状态信息 static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv; // 来自cmd的控制信息 static gimbal_mode_e last_gimbal_mode; //上次模式 用于模式切换数据过渡 first_order_filter_type_t pitch_spd_filter,yaw_spd_filter,roll_spd_filter; static float pitch_spd,yaw_spd,roll_spd; first_order_filter_type_t diff_r_spd_filter,diff_l_spd_filter; static float diff_r_spd,diff_l_spd; //机械臂参数初始化 //float arm_q[5] = {0}; // 机械臂各关节位置 robotics::Link link[5]; robotics::Serial_Link<5> engineer_arm(link); Matrixf<4, 4> fk_T; //正运动学 末端变换矩阵 Matrixf<3, 1> fk_p; //正运动学 末端位置向量 Matrixf<3, 1> fk_rpy; //正运动学 末端欧拉角 Matrixf<5, 1> arm_q; //正运动学 末端关节位置 Matrixf<4, 4> T_cmd; //拟运动学 期望末端变换矩阵 Matrixf<5, 2> ik_q; //逆运动学 关节位置 Matrixf<5, 1> ik_q_cmd;//逆运动学 期望关节位置 Matrixf<3, 1> cmd_xyz; //逆运动学 末端期望位置 Matrixf<3, 3> cmd_R; //逆运动学 末端期望姿态 Matrixf<4, 1> cmd_quat; //逆运动学 末端期望姿态 Matrixf<3, 2> ik_q3; //逆运动学 球形手腕关节位置 Matrixf<3, 1> ik_q3_cmd;//逆运动学 球形手腕期望关节位置 const float l1 = 0.151 ,l3 = 0.350, l4 = 0;//0.139; void Arm_Init() { link[0] = robotics::Link(0,l1,0,PI/2); link[1] = robotics::Link(0,0,l3,PI/2); link[2] = robotics::Link(0,0,0,-PI/2); link[3] = robotics::Link(0,0,0,PI/2); link[4] = robotics::Link(0,l4,0,0); engineer_arm = robotics::Serial_Link<5>(link); engineer_arm.ikine_analytic = robotics::my_analytic_ikine; engineer_arm.ikine_analytic_check = robotics::check_ikine; } void GimbalInit() { Arm_Init(); Motor_Init_Config_s yaw_motor_config = { .controller_param_init_config = { .other_speed_feedback_ptr = &yaw_spd, .speed_PID = { .Kp = 3.5f,//5, .Ki = 0.0f, .Kd = 0.02f, .MaxOut = 10, .Improve = static_cast(PID_Integral_Limit | PID_Derivative_On_Measurement), .IntegralLimit = 10.0F, }, .angle_PID = { .Kp = 10.0f,//13.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 = OTHER_FEED, .feedforward_flag = CURRENT_FEEDFORWARD, }, .motor_type = DM6006, }; yaw_motor_config.can_init_config.can_handle = &hcan2; yaw_motor_config.can_init_config.tx_id = 1; yaw_motor_config.can_init_config.rx_id = 2; yaw_motor = DMMotorInit(&yaw_motor_config); Motor_Init_Config_s pitch_motor_config = { .controller_param_init_config = { .other_speed_feedback_ptr = &pitch_spd, //.current_feedforward_ptr = &arm_gravity_feedforward, }, .controller_setting_init_config = { // .outer_loop_type = ANGLE_LOOP, // .close_loop_type = static_cast(ANGLE_LOOP | SPEED_LOOP), .outer_loop_type = OPEN_LOOP, .close_loop_type = OPEN_LOOP, .angle_feedback_source = MOTOR_FEED, .speed_feedback_source = OTHER_FEED, //.feedforward_flag = CURRENT_FEEDFORWARD, }, .motor_type = DM6006, }; pitch_motor_config.can_init_config.can_handle = &hcan2; pitch_motor_config.can_init_config.tx_id = 0x03; pitch_motor_config.can_init_config.rx_id = 0x04; pitch_motor_l = DMMotorInit(&pitch_motor_config); PID_Init_Config_s pitch_spd_config= { .Kp = 2.5f, .Ki = 0.8f, .Kd = 0.02f, .MaxOut = 10, .Improve = static_cast(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement), .IntegralLimit = 100, }; PID_Init_Config_s pitch_angle_config= { .Kp = 12.0f, .Ki = 0, .Kd = 0, .MaxOut = 10, .Improve = static_cast(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement), .IntegralLimit = 100, }; PIDInit(&pitch_spd_loop,&pitch_spd_config); PIDInit(&pitch_angle_loop,&pitch_angle_config); Motor_Init_Config_s roll_motor_config = { .controller_param_init_config = { .other_speed_feedback_ptr = &roll_spd, .speed_PID = { .Kp = 0.8f, .Ki = 0.3f, .Kd = 0.02f, .MaxOut = 10, .Improve = static_cast(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement), .IntegralLimit = 10.0F, }, .angle_PID = { .Kp = 10.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 = OTHER_FEED, //.feedforward_flag = CURRENT_FEEDFORWARD, }, .motor_type = DM4310, }; roll_motor_config.can_init_config.can_handle = &hcan2; roll_motor_config.can_init_config.tx_id = 5; roll_motor_config.can_init_config.rx_id = 6; roll_motor = DMMotorInit(&roll_motor_config); Motor_Init_Config_s diff_motor_config = { .controller_param_init_config = { .speed_PID = { .Kp = 0.2f,//0.6f, .Ki = 0,//0.1f, .Kd = 0.02,//0.02f, .MaxOut = 10, .Improve = static_cast( PID_Integral_Limit | PID_Derivative_On_Measurement), .IntegralLimit = 10.0F, .Output_LPF_RC = 0.02F, }, }, .controller_setting_init_config = { .outer_loop_type = SPEED_LOOP, .close_loop_type = SPEED_LOOP, .angle_feedback_source = MOTOR_FEED, .speed_feedback_source = OTHER_FEED, //.feedforward_flag = CURRENT_FEEDFORWARD, }, .motor_type = DM4310, }; diff_motor_config.can_init_config.can_handle = &hcan2; diff_motor_config.can_init_config.tx_id = 7; diff_motor_config.can_init_config.rx_id = 8; diff_motor_config.controller_param_init_config.other_speed_feedback_ptr = &diff_r_spd; diff_r_motor = DMMotorInit(&diff_motor_config); diff_motor_config.can_init_config.can_handle = &hcan2; diff_motor_config.can_init_config.tx_id = 9; diff_motor_config.can_init_config.rx_id = 10; diff_motor_config.controller_param_init_config.other_speed_feedback_ptr = &diff_l_spd; diff_l_motor = DMMotorInit(&diff_motor_config); const float spd_filter_num = 0.05f; first_order_filter_init(&pitch_spd_filter,5e-3,&spd_filter_num); const float spd_filter_num_yaw = 0.05f; first_order_filter_init(&yaw_spd_filter,5e-3,&spd_filter_num_yaw); const float spd_filter_num_roll = 0.05f; first_order_filter_init(&roll_spd_filter,5e-3,&spd_filter_num_roll); const float spd_filter_num_diff = 0.05f; first_order_filter_init(&diff_r_spd_filter,5e-3,&spd_filter_num_diff); first_order_filter_init(&diff_l_spd_filter,5e-3,&spd_filter_num_diff); PID_Init_Config_s diff_pitch_config= { .Kp = 20.0F,//15.0f, .Ki = 0, .Kd = 0, .MaxOut = 10, .Improve = static_cast(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement), .IntegralLimit = 100, }; PIDInit(&diff_pitch_loop,&diff_pitch_config); PID_Init_Config_s diff_pitch_spd_config= { .Kp = 0.6f, .Ki = 2.0f, .Kd = 0.01f, .MaxOut = 10, .Improve = static_cast(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement), .IntegralLimit = 100, }; PIDInit(&diff_pitch_spd_loop,&diff_pitch_spd_config); PID_Init_Config_s diff_roll_config= { .Kp = 14.0f, .Ki = 0, .Kd = 0, .MaxOut = 10, .Improve = static_cast(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement), .IntegralLimit = 100, }; PIDInit(&diff_roll_loop,&diff_roll_config); PID_Init_Config_s diff_roll_spd_config= { .Kp = 1.0f, .Ki = 1.0f, .Kd = 0.02f, .MaxOut = 10, .Improve = static_cast(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement), .IntegralLimit = 100, }; PIDInit(&diff_roll_spd_loop,&diff_roll_spd_config); pitch_motor_config.can_init_config.tx_id = 11; //按顺序初始化！！！ pitch_motor_config.can_init_config.rx_id = 12; pitch_motor_r = DMMotorInit(&pitch_motor_config); gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源 // YAW gimbal_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s)); gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s)); } //static void DMMotroEnable() //{ // if(gimbal_cmd_recv.MotorEnableFlag) // { // DMMotorSetMode(DM_CMD_MOTOR_MODE,yaw_motor); // DMMotorSetMode(DM_CMD_MOTOR_MODE,pitch_motor); // DMMotorSetMode(DM_CMD_MOTOR_MODE,roll_motor); // DMMotorSetMode(DM_CMD_MOTOR_MODE,diff_r_motor ); // DMMotorSetMode(DM_CMD_MOTOR_MODE,diff_l_motor ); // } //} /* 机器人云台控制核心任务,后续考虑只保留IMU控制,不再需要电机的反馈 */ //float debug_diff_kp = 0.1f,debug_diff_ki=0,debug_diff_kd = 0; void GimbalTask() { // diff_r_motor->motor_controller.speed_PID.Kp = debug_diff_kp; // diff_r_motor->motor_controller.speed_PID.Ki = debug_diff_ki; // diff_r_motor->motor_controller.speed_PID.Kd = debug_diff_kd; // // diff_l_motor->motor_controller.speed_PID.Kp = debug_diff_kp; // diff_l_motor->motor_controller.speed_PID.Ki = debug_diff_ki; // diff_l_motor->motor_controller.speed_PID.Kd = debug_diff_kd; // 获取云台控制数据 // 后续增加未收到数据的处理 SubGetMessage(gimbal_sub, &gimbal_cmd_recv); /* 控制参数计算 ------------------------------------------------------------------------*/ //大臂重力补偿力矩 arm_gravity_feedforward = - GRAVITY_COMP * arm_cos_f32(- pitch_motor_l->measure.position); //电机速度滤波 first_order_filter_cali(&pitch_spd_filter,pitch_motor_l->measure.velocity); pitch_spd = pitch_spd_filter.out; first_order_filter_cali(&yaw_spd_filter,yaw_motor->measure.velocity); yaw_spd = yaw_spd_filter.out; first_order_filter_cali(&roll_spd_filter,roll_motor->measure.velocity); roll_spd = roll_spd_filter.out; //手腕关节 first_order_filter_cali(&diff_r_spd_filter,diff_r_motor->measure.velocity); diff_r_spd = diff_r_spd_filter.out; first_order_filter_cali(&diff_l_spd_filter,diff_l_motor->measure.velocity); diff_l_spd = diff_l_spd_filter.out; float diff_pitch_angle = (diff_r_motor->measure.position + (-diff_l_motor->measure.position))/2; float diff_roll_angle = (diff_r_motor->measure.position - (-diff_l_motor->measure.position))/2 * 18/52; /* 控制参数计算 ------------------------------------------------------------------------*/ //正运动学 arm_q[0][0] = yaw_motor->measure.position; arm_q[1][0] = -pitch_motor_l->measure.position; arm_q[2][0] = roll_motor->measure.position; arm_q[3][0] = diff_pitch_angle; arm_q[4][0] = diff_roll_angle; fk_T = engineer_arm.fkine(arm_q); fk_p = robotics::t2p(fk_T); fk_rpy = robotics::t2rpy(fk_T); //逆运动学 // ik_q = engineer_arm.ikine_analytic(fk_T); // // ik_q_cmd = ik_q.block<5,1>(0,0); // ik_q3 = robotics::spherical_wrist_ikine(robotics::t2r(fk_T),arm_q[0][0],arm_q[1][0]); if(gimbal_cmd_recv.gimbal_mode == GIMBAL_ZERO_FORCE) { DMMotorStop(pitch_motor_l);DMMotorStop(pitch_motor_r); DMMotorStop(yaw_motor);DMMotorStop(roll_motor); DMMotorStop(diff_r_motor);DMMotorStop(diff_l_motor); } else { DMMotorEnable(pitch_motor_l);DMMotorEnable(pitch_motor_r); DMMotorEnable(yaw_motor);DMMotorEnable(roll_motor); DMMotorEnable(diff_r_motor);DMMotorEnable(diff_l_motor); } if(gimbal_cmd_recv.gimbal_mode == GIMBAL_GYRO_MODE) //各关节分开控制 { float diff_pitch_angle_out = PIDCalculate(&diff_pitch_loop, diff_pitch_angle, gimbal_cmd_recv.diff_pitch * DEGREE_2_RAD); float diff_roll_angle_out = PIDCalculate(&diff_roll_loop, diff_roll_angle, gimbal_cmd_recv.diff_roll * DEGREE_2_RAD); float r_speed_set = diff_pitch_angle_out + diff_roll_angle_out; float l_speed_set = diff_pitch_angle_out - diff_roll_angle_out; //pitch轴双环PID float pitch_angle_out = PIDCalculate(&pitch_angle_loop,pitch_motor_l->measure.position,gimbal_cmd_recv.pitch * DEGREE_2_RAD); float pitch_spd_out = PIDCalculate(&pitch_spd_loop,pitch_spd,pitch_angle_out) + arm_gravity_feedforward; DMMotorSetRef(pitch_motor_l,pitch_spd_out / 2); DMMotorSetRef(pitch_motor_r, -pitch_spd_out / 2); DMMotorSetRef(yaw_motor,gimbal_cmd_recv.yaw * DEGREE_2_RAD); DMMotorSetRef(roll_motor,gimbal_cmd_recv.roll * DEGREE_2_RAD); DMMotorSetRef(diff_r_motor,r_speed_set); DMMotorSetRef(diff_l_motor,-l_speed_set); } if(gimbal_cmd_recv.gimbal_mode == GIMBAL_IKINE_MODE) { static float q_set[5] ; //记录五个关节的目标值 if(last_gimbal_mode == GIMBAL_GYRO_MODE) { //切换至逆解模式时，目标值设置为当前值 cmd_R = robotics::t2r( fk_T); } for (int i = 0; i < 4; ++i) { cmd_quat[i][0] = gimbal_cmd_recv.quat[i]; } float trans_rpy_data[3] = {-PI,0,0}; Matrixf<3,1> trans_rpy(trans_rpy_data); cmd_R = robotics::quat2r(cmd_quat) * robotics::rpy2r(trans_rpy); //转到基座坐标系下的姿态 // T_cmd = robotics::p2t(cmd_xyz); ik_q3 = robotics::spherical_wrist_ikine(cmd_R,arm_q[0][0],arm_q[1][0]); //后三关节的误差 选用误差小的一组解 float err[2] = {0}; Matrixf<3,1> arm_q3 = arm_q.block<3,1>(2,0); for (int i = 0; i < 2; ++i) { Matrixf<3,1> ik = ik_q3.block<3,1>(0,i); err[i] = (ik - arm_q3).norm(); } // if (err[1] >= err[0]) // ik_q3_cmd = ik_q3.block<3,1>(0,0); // else // ik_q3_cmd = ik_q3.block<3,1>(0,1); // //选用 roll误差小的一组解 if (abs(ik_q3[0][0] - arm_q3[0][0]) <= abs(ik_q3[0][1]- arm_q3[0][0])) ik_q3_cmd = ik_q3.block<3,1>(0,0); else ik_q3_cmd = ik_q3.block<3,1>(0,1); ik_q3_cmd[0][0] = float_constrain(ik_q3_cmd[0][0],-ROLL* DEGREE_2_RAD,ROLL* DEGREE_2_RAD); //ik_q3_cmd[1][0] = float_constrain(ik_q3_cmd[1][0],-DIFF_PITCH* DEGREE_2_RAD,DIFF_PITCH* DEGREE_2_RAD); ik_q3_cmd[2][0] = float_constrain(ik_q3_cmd[2][0],-DIFF_ROLL* DEGREE_2_RAD,DIFF_ROLL* DEGREE_2_RAD); for (int i = 0; i < 3; ++i) { q_set[i] = ik_q3_cmd[i][0]; } float diff_pitch_angle_out = PIDCalculate(&diff_pitch_loop, diff_pitch_angle, q_set[1]); float diff_roll_angle_out = PIDCalculate(&diff_roll_loop, diff_roll_angle, q_set[2]); float r_speed_set = diff_pitch_angle_out + diff_roll_angle_out; float l_speed_set = diff_pitch_angle_out - diff_roll_angle_out; //pitch轴双环PID pitch yaw先遥控器控制 float pitch_angle_out = PIDCalculate(&pitch_angle_loop,pitch_motor_l->measure.position,gimbal_cmd_recv.pitch * DEGREE_2_RAD); float pitch_spd_out = PIDCalculate(&pitch_spd_loop,pitch_spd,pitch_angle_out) + arm_gravity_feedforward; DMMotorSetRef(pitch_motor_l,pitch_spd_out / 2); DMMotorSetRef(pitch_motor_r, -pitch_spd_out / 2); DMMotorSetRef(yaw_motor,gimbal_cmd_recv.yaw * DEGREE_2_RAD); DMMotorSetRef(roll_motor,q_set[0]); DMMotorSetRef(diff_r_motor,r_speed_set); DMMotorSetRef(diff_l_motor,-l_speed_set); } //保存上次模式 last_gimbal_mode = gimbal_cmd_recv.gimbal_mode; // 在合适的地方添加pitch重力补偿前馈力矩 // 根据IMU姿态/pitch电机角度反馈计算出当前配重下的重力矩 // ... // 设置反馈数据,主要是imu和yaw的ecd gimbal_feedback_data.gimbal_imu_data = *gimba_IMU_data; gimbal_feedback_data.yaw_motor_single_round_angle = yaw_motor->measure.angle_single_round; // 推送消息 PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data); }