#include "gimbal.h" #include "robot_def.h" #include "dji_motor.h" #include "DM4310.h" #include "ECmotor/ECA8210.h" #include "ins_task.h" #include "message_center.h" #include "general_def.h" #include "bmi088.h" #include "vofa.h" #include "power_meter.h" #include "user_lib.h" static attitude_t *gimba_IMU_data; // 云台IMU数据 static DJIMotorInstance *yaw_motor, *pitch_motor; static DMMotorInstance *big_yaw_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 current_feedforward = 0.0f; static float yaw_speed = 0.0f; //yaw轴旋转角速度需要通过pitch解算确定 static float big_yaw_angle = 0.0f; //大yaw轴绝对角度 通过小云台陀螺仪yaw-yaw轴编码器角度确定 static float big_yaw_speed = 0.0f;//大yaw轴绝对角速度 sin_input_generate_t sinInputGenerate; static Subscriber_t *chassis_sub; // 用于订阅底盘的控制命令 static Chassis_Ctrl_Cmd_s chassis_cmd_recv; // 底盘接收到的控制命令 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.5f, // 8 .Ki = 0, .Kd = 0, .DeadBand = 0.1, .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, .IntegralLimit = 100, .MaxOut = 500, }, .speed_PID = { .Kp = 6000, // 50 .Ki = 500,//5000, // 200 .Kd = 0, .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, .IntegralLimit = 5000, .MaxOut = 16000, }, .other_angle_feedback_ptr = &gimba_IMU_data->YawTotalAngle, // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明 .other_speed_feedback_ptr = &yaw_speed,//&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 = &hcan1, .tx_id = 2, }, .controller_param_init_config = { .angle_PID = { .Kp = -0.6f,//4.0f,//2.0f .Ki = -0.8f,//1,//0 .Kd = 0.0f,//0.0f .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, .IntegralLimit = 100, .MaxOut = 500, }, .speed_PID = { .Kp = -4000,//-4500,//6000,//800 .Ki = -6000, //-600,//500,//100 .Kd = 0,//0 .Improve = PID_Integral_Limit | PID_Derivative_On_Measurement, .IntegralLimit = 10000, .MaxOut = 30000, }, .other_angle_feedback_ptr = &gimba_IMU_data->Roll,//&gimba_IMU_data->Pitch, // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明 .other_speed_feedback_ptr = (&gimba_IMU_data->Gyro[1]),//(&gimba_IMU_data->Gyro[0]), .current_feedforward_ptr = ¤t_feedforward, }, .controller_setting_init_config = { .outer_loop_type = ANGLE_LOOP, .close_loop_type = ANGLE_LOOP | SPEED_LOOP | CURRENT_LOOP, .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, .feedforward_flag = CURRENT_FEEDFORWARD, }, .motor_type = GM6020, .motor_control_type = CURRENT_CONTROL,//CURRENT_CONTROL }; //大YAW Motor_Init_Config_s big_yaw_config = { .can_init_config = { .can_handle = &hcan2, .tx_id = 1, .rx_id = 1, }, .controller_param_init_config = { .angle_PID = { .Kp = 0.15f, .Ki = 0.0f, .Kd = 0.005f, .Improve = PID_Integral_Limit | PID_Derivative_On_Measurement | PID_DerivativeFilter, .IntegralLimit = 100, .MaxOut = 1000, .DeadBand = 0, }, .speed_PID = { .Kp = 3.5f,//150,//150,//这里KP尽可能小 防止震荡脱齿 .Ki = 0.0f,//200, // 350 .Kd = 0, // 0 .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, .IntegralLimit = 10000, .MaxOut = 1000, }, .other_angle_feedback_ptr = &big_yaw_angle, .other_speed_feedback_ptr = &big_yaw_speed, }, .controller_setting_init_config = { .angle_feedback_source = OTHER_FEED, .speed_feedback_source = OTHER_FEED, .outer_loop_type = ANGLE_LOOP, .close_loop_type = ANGLE_LOOP | SPEED_LOOP | CURRENT_LOOP, .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, }, .motor_type = DM4310, }; // 电机对total_angle闭环,上电时为零,会保持静止,收到遥控器数据再动 yaw_motor = DJIMotorInit(&yaw_config); pitch_motor = DJIMotorInit(&pitch_config); big_yaw_motor = DMMotorInit(&big_yaw_config); gimbal_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s)); gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s)); chassis_sub = SubRegister("chassis_cmd", sizeof(Chassis_Ctrl_Cmd_s)); //sin_input_frequency_init(&sinInputGenerate); } /* 机器人云台控制核心任务,后续考虑只保留IMU控制,不再需要电机的反馈 */ void GimbalTask() { // 获取云台控制数据 // 后续增加未收到数据的处理 SubGetMessage(gimbal_sub, &gimbal_cmd_recv); SubGetMessage(chassis_sub, &chassis_cmd_recv); // if(gimbal_cmd_recv.enable_motor > 0){ // LOGINFO("enabling DMmoter"); // DMMotorSetMode(DM_CMD_MOTOR_MODE, big_yaw_motor); // } //gimbal_cmd_recv.big_yaw = 0; // @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); DMMotorStop(big_yaw_motor); break; // 使用陀螺仪的反馈,底盘根据yaw电机的offset跟随云台或视觉模式采用 case GIMBAL_GYRO_MODE: // 后续只保留此模式 DJIMotorEnable(yaw_motor); DJIMotorEnable(pitch_motor); DMMotorEnable(big_yaw_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); DMMotorSetRef(big_yaw_motor, gimbal_cmd_recv.big_yaw); break; // 云台自由模式,使用编码器反馈,底盘和云台分离,仅云台旋转,一般用于调整云台姿态(英雄吊射等)/能量机关 case GIMBAL_FREE_MODE: // 后续删除,或加入云台追地盘的跟随模式(响应速度更快) DJIMotorEnable(yaw_motor); DJIMotorEnable(pitch_motor); DMMotorEnable(big_yaw_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); // big_yaw_angle = chassis_cmd_recv.offset_angle; // big_yaw_speed = big_yaw_motor->measure.speed_rads; // // ECMotorSetRef(big_yaw_motor,0); // big_yaw_angle = gimba_IMU_data->YawTotalAngle - (yaw_motor->measure.total_angle - 44); // big_yaw_speed = yaw_speed - yaw_motor->measure.speed_aps * DEGREE_2_RAD; DMMotorSetRef(big_yaw_motor, gimbal_cmd_recv.big_yaw); break; default: break; } // 在合适的地方添加pitch重力补偿前馈力矩 // 根据IMU姿态/pitch电机角度反馈计算出当前配重下的重力矩 // ... float input = sin_input_generate(&sinInputGenerate); //ANODT_SendF1(input*1000,pitch_motor->measure.speed_aps*1000,0,0); float theta = gimba_IMU_data->Roll / 180 * PI; //(pitch_motor->measure.angle_single_round - 5505 * ECD_ANGLE_COEF_DJI)/180*PI; yaw_speed = gimba_IMU_data->Gyro[2] * arm_cos_f32(theta) - gimba_IMU_data->Gyro[0] * arm_sin_f32(theta); //big_yaw_angle = gimba_IMU_data->YawTotalAngle - (yaw_motor->measure.total_angle - 44); //big_yaw_speed = yaw_speed - yaw_motor->measure.speed_aps * DEGREE_2_RAD; big_yaw_angle = gimba_IMU_data->YawTotalAngle - (yaw_motor->measure.total_angle - (-80)); //big_yaw_speed = yaw_speed - yaw_motor->measure.speed_aps * DEGREE_2_RAD; big_yaw_speed = (yaw_speed - yaw_motor->measure.speed_aps * DEGREE_2_RAD); // big_yaw_angle = chassis_cmd_recv.offset_angle; // //loop_float_constrain(big_yaw_motor->measure.total_angle * (4.0f/3.0f),0,2*PI) * RAD_2_DEGREE - 340.0f; // big_yaw_speed = big_yaw_motor->measure.speed_rads; //yaw轴角速度 //重力补偿力矩 current_feedforward = -10000 * arm_cos_f32(theta); //current_feedforward = 0; float vofa_send_data[6]; vofa_send_data[0] = pitch_motor->measure.speed_aps; vofa_send_data[1] = gimba_IMU_data->Pitch; vofa_send_data[2] = gimba_IMU_data->Gyro[0]; vofa_send_data[3] = pitch_motor->motor_controller.speed_PID.Ref; vofa_send_data[4] = pitch_motor->motor_controller.speed_PID.Measure; vofa_send_data[5] = pitch_motor->measure.angle_single_round; vofa_justfloat_output(vofa_send_data, 24, &huart1); // 设置反馈数据,主要是imu和yaw的ecd gimbal_feedback_data.gimbal_imu_data = *gimba_IMU_data; static float big_yaw_relative_angle; big_yaw_relative_angle = big_yaw_motor->measure.total_angle; gimbal_feedback_data.yaw_motor_single_round_angle = big_yaw_motor->measure.angle_single_round; //loop_float_constrain(big_yaw_relative_angle,0,2*PI) * RAD_2_DEGREE; gimbal_feedback_data.mini_yaw_encode_angle = yaw_motor->measure.angle_single_round; gimbal_feedback_data.big_yaw_online = DMMotorIsOnline(big_yaw_motor); gimbal_feedback_data.big_yaw_angle = big_yaw_angle; // 推送消息 PubPushMessage(gimbal_pub, (void *) &gimbal_feedback_data); } //以下代码为测试系统辨识时使用 //void sin_input_generate(float frequency, int count) //{ // static uint32_t cnt; // static float time; // while(time>=count*(1/frequency)) // { // float deltaT=DWT_GetDeltaT(&cnt); // time += deltaT; // // float input = arm_sin_f32(2*PI*frequency*time); // DJIMotorSetRef(yaw_motor,input); // } //} void sin_input_frequency_init(sin_input_generate_t *InputGenerate) { for (int i = 0; i < 43; i++) { InputGenerate->frequency[i] = 1.0 + 0.5 * i; } for (int i = 0; i < 9; i++) { InputGenerate->frequency[i + 43] = 24.0 + 2.0 * i; } for (int i = 0; i < 8; i++) { InputGenerate->frequency[i + 43 + 9] = 50 + 10 * i; } InputGenerate->frequency[60] = 200; InputGenerate->frequency[61] = 250; InputGenerate->frequency[62] = 333; InputGenerate->frequency[63] = 500; } float sin_input_generate(sin_input_generate_t *InputGenerate) { InputGenerate->DeltaT = DWT_GetDeltaT(&InputGenerate->cnt); InputGenerate->time += InputGenerate->DeltaT; if (InputGenerate->time >= 20 * (1 / InputGenerate->frequency[InputGenerate->frequency_index])) { InputGenerate->time = 0; InputGenerate->frequency_index += 1; } if (InputGenerate->frequency_index >= 64) { InputGenerate->input = 0; } else InputGenerate->input = arm_sin_f32( 2 * PI * InputGenerate->frequency[InputGenerate->frequency_index] * InputGenerate->time); //float input = arm_sin_f32(2*PI*frequency*time); InputGenerate->input *= 2000; return InputGenerate->input; } float step_input_generate(sin_input_generate_t *InputGenerate) { static int8_t forward_flag = 1; InputGenerate->DeltaT = DWT_GetDeltaT(&InputGenerate->cnt); InputGenerate->time += InputGenerate->DeltaT; if (InputGenerate->time >= 3) { if (forward_flag == 1) forward_flag = -1; else if (forward_flag == -1) forward_flag = 1; InputGenerate->time = 0; } return 60 * forward_flag; }