#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" #ifdef __cplusplus } #endif #include "matrix.h" #include "robotics.h" static attitude_t *gimba_IMU_data; // 云台IMU数据 static DMMotorInstance *yaw_motor, *pitch_motor, *roll_motor; static DMMotorInstance *diff_r_motor,*diff_l_motor; 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 = 5.5; 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的控制信息 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; void GimbalInit() { Motor_Init_Config_s yaw_motor_config = { .controller_param_init_config = { .other_speed_feedback_ptr = &yaw_spd, .speed_PID = { .Kp = 3,//5, .Ki = 3.0f, .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,//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, .speed_PID = { .Kp = 2.5f, .Ki = 0.8f, .Kd = 0.0f, .MaxOut = 10, .Improve = static_cast(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement), .IntegralLimit = 10.0F, }, .angle_PID = { .Kp = 8.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, }; pitch_motor_config.can_init_config.can_handle = &hcan2; pitch_motor_config.can_init_config.tx_id = 3; pitch_motor_config.can_init_config.rx_id = 4; pitch_motor = DMMotorInit(&pitch_motor_config); //@todo：roll轴机械固定不牢 待细调 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_setting_init_config = { .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 = 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.1f; 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 = 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); 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)); } /* 机器人云台控制核心任务,后续考虑只保留IMU控制,不再需要电机的反馈 */ void GimbalTask() { /* Example 1: PUMA560 ------------------------------------------------------------------------*/ float m[6] = {0.2645, 0.17, 0.1705, 0, 0, 0}; float data[18]={0, -8.5e-2, 0, 0, 0, 0, 13.225e-2, 0, 0, 0, 0, 0, 0, 3.7e-2, 8.525e-2, 0, 0, 0}; Matrixf<3, 6> rc(data); Matrixf<3, 3> I[6]; // 获取云台控制数据 // 后续增加未收到数据的处理 SubGetMessage(gimbal_sub, &gimbal_cmd_recv); //大臂重力补偿力矩 arm_gravity_feedforward = - GRAVITY_COMP * arm_cos_f32(- pitch_motor->measure.position); //电机速度滤波 first_order_filter_cali(&pitch_spd_filter,pitch_motor->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; float pitch_angle_out = PIDCalculate(&diff_pitch_loop,diff_pitch_angle,gimbal_cmd_recv.diff_pitch * DEGREE_2_RAD); float roll_angle_out = PIDCalculate(&diff_roll_loop,diff_roll_angle,gimbal_cmd_recv.diff_roll * DEGREE_2_RAD); float diff_pitch_spd = (diff_r_spd + (-diff_l_spd)) / 2; float diff_roll_spd = (diff_r_spd - (-diff_l_spd)) /2 * 18/52; float pitch_out = PIDCalculate(&diff_pitch_spd_loop,diff_pitch_spd,pitch_angle_out); float roll_out = PIDCalculate(&diff_roll_spd_loop,diff_roll_spd,roll_angle_out); float r_speed_set = pitch_out + roll_out; float l_speed_set = pitch_out - roll_out; // @todo:现在已不再需要电机反馈,实际上可以始终使用IMU的姿态数据来作为云台的反馈,yaw电机的offset只是用来跟随底盘 // 根据控制模式进行电机反馈切换和过渡,视觉模式在robot_cmd模块就已经设置好,gimbal只看yaw_ref和pitch_ref switch (gimbal_cmd_recv.gimbal_mode) { // 停止 case GIMBAL_ZERO_FORCE: DMMotorStop(yaw_motor); DMMotorStop(pitch_motor); DMMotorStop(roll_motor); DMMotorStop(diff_r_motor); DMMotorStop(diff_l_motor); break; // 使用陀螺仪的反馈,底盘根据yaw电机的offset跟随云台或视觉模式采用 case GIMBAL_GYRO_MODE: // 后续只保留此模式 DMMotorEnable(yaw_motor); DMMotorEnable(pitch_motor); DMMotorEnable(roll_motor); DMMotorSetRef(pitch_motor,gimbal_cmd_recv.pitch * DEGREE_2_RAD); DMMotorSetRef(yaw_motor,gimbal_cmd_recv.yaw * DEGREE_2_RAD); DMMotorSetRef(roll_motor,gimbal_cmd_recv.roll * DEGREE_2_RAD); DMMotorEnable(diff_r_motor); DMMotorEnable(diff_l_motor); DMMotorSetRef(diff_r_motor,r_speed_set); DMMotorSetRef(diff_l_motor,-l_speed_set); break; default: break; } // 在合适的地方添加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); }