#include "gimbal.h" #include "robot_def.h" #include "dji_motor.h" #include "ins_task.h" #include "message_center.h" #include "general_def.h" #include "bmi088.h" #include "vofa.h" static attitude_t *gimba_IMU_data; // 云台IMU数据 static DJIMotorInstance *yaw_motor, *pitch_motor; 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 float gravity_current = 0; static void LimitPitchAngleAuto() { /** 注意电机角度与陀螺仪角度方向是否相同 * 目前 add > 0, 向下转动 * 电机角度值减小 * 陀螺仪角度值增大 **/ float add; float angle_set; add = gimbal_cmd_recv.pitch - gimba_IMU_data->Pitch; if(pitch_motor->measure.angle_single_round - add > PITCH_MAX_RELATIVE_ANGLE){ if(add < 0.0f ){ add = PITCH_MAX_ANGLE - pitch_motor->measure.angle_single_round; } }else if(pitch_motor->measure.angle_single_round - add < PITCH_MIN_RELATIVE_ANGLE){ if(add > 0.0f){ add = PITCH_MIN_RELATIVE_ANGLE - pitch_motor->measure.angle_single_round; } } angle_set = gimba_IMU_data->Pitch; DJIMotorSetRef(pitch_motor, angle_set+add); } void GimbalInit() { gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源 // YAW Motor_Init_Config_s yaw_config = { .can_init_config = { .can_handle = &hcan1, .tx_id = 1, }, .controller_param_init_config = { .angle_PID = { .Kp = 0.8f,//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 = 500, }, .speed_PID = { .Kp = 5000, //4000 .Ki = 100.0f, //100 .Kd = 0.03f, //0 .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, .IntegralLimit = 10000, .MaxOut = 15000, }, .other_angle_feedback_ptr = &gimba_IMU_data->YawTotalAngle, // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明 .other_speed_feedback_ptr = &gimba_IMU_data->Gyro[2], }, .controller_setting_init_config = { .angle_feedback_source = OTHER_FEED, .speed_feedback_source = OTHER_FEED, .outer_loop_type = ANGLE_LOOP, .close_loop_type = SPEED_LOOP | ANGLE_LOOP, .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, }, .motor_type = GM6020, .motor_control_type = CURRENT_CONTROL }; // PITCH Motor_Init_Config_s pitch_config = { .can_init_config = { .can_handle = &hcan2, .tx_id = 1, }, .controller_param_init_config = { .angle_PID = { .Kp = 1.0f, .Ki = 0.0f, .Kd = 0.02f, .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, .IntegralLimit = 100, .MaxOut = 500, }, .speed_PID = { .Kp = 6000.0f, .Ki = 900.0f, .Kd = 0.0f, // 0 .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, .IntegralLimit = 10000, .MaxOut = 15000, }, .other_angle_feedback_ptr = &gimba_IMU_data->Roll, // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明 .other_speed_feedback_ptr = &gimba_IMU_data->Gyro[1], .current_feedforward_ptr = &gravity_current, }, .controller_setting_init_config = { .angle_feedback_source = OTHER_FEED, .speed_feedback_source = OTHER_FEED, .outer_loop_type = ANGLE_LOOP, .close_loop_type = SPEED_LOOP | ANGLE_LOOP, .motor_reverse_flag = MOTOR_DIRECTION_REVERSE, .feedforward_flag = CURRENT_FEEDFORWARD, .feedback_reverse_flag = FEEDBACK_DIRECTION_REVERSE }, .motor_type = GM6020, .motor_control_type = CURRENT_CONTROL }; // 电机对total_angle闭环,上电时为零,会保持静止,收到遥控器数据再动 yaw_motor = DJIMotorInit(&yaw_config); pitch_motor = DJIMotorInit(&pitch_config); gimbal_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s)); gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s)); } /* 机器人云台控制核心任务,后续考虑只保留IMU控制,不再需要电机的反馈 */ void GimbalTask() { //红点激光 //HAL_GPIO_WritePin(GPIOC,GPIO_PIN_8,GPIO_PIN_SET); // 获取云台控制数据 // 后续增加未收到数据的处理 SubGetMessage(gimbal_sub, &gimbal_cmd_recv); // @todo:现在已不再需要电机反馈,实际上可以始终使用IMU的姿态数据来作为云台的反馈,yaw电机的offset只是用来跟随底盘 // 根据控制模式进行电机反馈切换和过渡,视觉模式在robot_cmd模块就已经设置好,gimbal只看yaw_ref和pitch_ref switch (gimbal_cmd_recv.gimbal_mode) { // 停止 case GIMBAL_ZERO_FORCE: DJIMotorStop(yaw_motor); DJIMotorStop(pitch_motor); break; // 使用陀螺仪的反馈,底盘根据yaw电机的offset跟随云台或视觉模式采用 case GIMBAL_GYRO_MODE: // 后续只保留此模式 DJIMotorEnable(yaw_motor); DJIMotorEnable(pitch_motor); DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, OTHER_FEED); DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, OTHER_FEED); DJIMotorChangeFeed(pitch_motor, ANGLE_LOOP, OTHER_FEED); DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, OTHER_FEED); DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈 DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch); //LimitPitchAngleAuto(); break; // 云台自由模式,使用编码器反馈,底盘和云台分离,仅云台旋转,一般用于调整云台姿态(英雄吊射等)/能量机关 case GIMBAL_FREE_MODE: // 后续删除,或加入云台追地盘的跟随模式(响应速度更快) DJIMotorEnable(yaw_motor); DJIMotorEnable(pitch_motor); DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, OTHER_FEED); DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, OTHER_FEED); DJIMotorChangeFeed(pitch_motor, ANGLE_LOOP, OTHER_FEED); DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, OTHER_FEED); DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈 DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch); break; default: break; } // 在合适的地方添加pitch重力补偿前馈力矩 // 根据IMU姿态/pitch电机角度反馈计算出当前配重下的重力矩 // ... float theta = - gimba_IMU_data->Roll/180*PI; gravity_current = (5000)*arm_cos_f32(theta); // float vofa_send_data[4]; // vofa_send_data[0] = pitch_motor->motor_controller.speed_PID.Ref; // vofa_send_data[1] = pitch_motor->motor_controller.speed_PID.Measure; // vofa_send_data[2] = pitch_motor->motor_controller.angle_PID.Ref; // vofa_send_data[3] = pitch_motor->motor_controller.angle_PID.Measure; // vofa_justfloat_output(vofa_send_data, 16, &huart1); // float vofa_send_data[4]; // vofa_send_data[0] = yaw_motor->motor_controller.speed_PID.Ref; // vofa_send_data[1] = yaw_motor->motor_controller.speed_PID.Measure; // vofa_send_data[2] = yaw_motor->motor_controller.angle_PID.Ref; // vofa_send_data[3] = yaw_motor->motor_controller.angle_PID.Measure; // vofa_justfloat_output(vofa_send_data, 16, &huart1); // 设置反馈数据,主要是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); }