/*云台控制代码*/ /* 云台模组 example： Gimbal_CMD_t cmd_gimbal; Gimbal_IMU_t gimbal_imu; Gimbal_t gimbal; Gimbal_Init(&gimbal, &Config_GetRobotParam()->gimbal, GIMBAL_FREQ); while(1) { osMessageQueueGet(task_runtime.msgq.gimbal.imu, &gimbal_imu,NULL, 0); Gimbal_UpdateIMU(&gimbal, &gimbal_imu); osMessageQueueGet(task_runtime.msgq.gimbal.cmd, &cmd_gimbal, NULL, 0); Gimbal_UpdateFeedback(&gimbal); osMessageQueueReset(task_runtime.msgq.gimbal.yaw6020); osMessageQueuePut(task_runtime.msgq.gimbal.yaw6020, &gimbal.feedback.motor.yaw_4310_motor_feedback, 0, 0); Gimbal_Control(&gimbal, &cmd_gimbal); Gimbal_Output(&gimbal); } 后续可以加入NUC传来的数据命令 */ #include "gimbal.h" #include "device/motor_dm.h" #include "component/pid.h" #include "bsp/can.h" #include "bsp/time.h" #include "component/filter.h" #include "math.h" #include "module/cmd.h" #include "device/motor_rm.h" #include "device/motor.h" /* *拟合函数，用于对pitch的重力补偿 */ static double poly(double x) { return 0.420757*pow(x,3) + -2.27122*pow(x,2) + 4.13016*x + -1.98984; } /** * \brief 设置云台模式 * * \param c 包含云台数据的结构体 * \param mode 要设置的模式 * * \return 函数运行结果 */ static int8_t Gimbal_SetMode(Gimbal_t *g, Gimbal_Mode_t mode) { if (g == NULL) return -1; if (mode == g->mode) return GIMBAL_OK; PID_Reset(&g->pid.yaw_4310_angle); PID_Reset(&g->pid.yaw_4310_omega); PID_Reset(&g->pid.pitch_4310_angle); PID_Reset(&g->pid.pitch_4310_omega); PID_Reset(&g->pid.yaw_6020_angle); PID_Reset(&g->pid.yaw_6020_omega); LowPassFilter2p_Reset(&g->filter_out.yaw_4310,0.0f); LowPassFilter2p_Reset(&g->filter_out.pitch_4310,0.0f); LowPassFilter2p_Reset(&g->filter_out.yaw_6020,0.0f); MOTOR_DM_Enable(&g->param->yaw_4310_motor); MOTOR_DM_Enable(&g->param->pitch_4310_motor); AHRS_ResetEulr(&g->feedback.imu.eulr);/* 切换模式后重置设定值 */ g->setpoint.eulr.pit = g->feedback.imu.eulr.rol; g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw; g->mode = mode; return GIMBAL_OK; } /* Exported functions ------------------------------------------------------- */ /** * \brief 初始化云台 * * \param g 包含云台数据的结构体 * \param param 包含云台参数的结构体指针 * \param target_freq 任务预期的运行频率 * * \return 函数运行结果 */ /*云台初始化内容*/ int8_t Gimbal_Init(Gimbal_t *g, Gimbal_Param_t *param,float target_freq) { if(g == NULL || param == NULL) { return GIMBAL_ERR; // 参数错误 } g->param = param; g->mode = GIMBAL_MODE_RELAX; /* 设置默认模式 不输出*/ /*6020小yaw pid初始化*/ PID_Init(&g->pid.yaw_6020_angle,KPID_MODE_NO_D, target_freq,&g->param->pid.yaw_6020_motor_angle ); PID_Init(&g->pid.yaw_6020_omega,KPID_MODE_CALC_D, target_freq,&g->param->pid.yaw_6020_motor_omega ); /*4310大yaw pid初始化 大yaw单环控*/ PID_Init(&g->pid.yaw_4310_angle,KPID_MODE_SET_D, target_freq,&g->param->pid.yaw_4310_motor_angle ); /*4310 pitch pid初始化 单环*/ PID_Init(&g->pid.pitch_4310_angle,KPID_MODE_NO_D, target_freq,&g->param->pid.pitch_4310_motor_angle ); PID_Init(&g->pid.pitch_4310_omega,KPID_MODE_CALC_D, target_freq,&g->param->pid.pitch_4310_motor_omega ); /*输出滤波器初始化*/ LowPassFilter2p_Init(&g->filter_out.yaw_6020, target_freq, g->param->low_pass_cutoff_freq.out); /*机械限位*/ g->limit.yaw_6020.max = g->param->mech_zero.yaw_6020 + g->param->travel.yaw_6020; g->limit.yaw_6020.min = g->param->mech_zero.yaw_6020 ; g->limit.pitch_4310.max = g->param->mech_zero.pitch_4310 + g->param->travel.pitch_4310; g->limit.pitch_4310.min = g->param->mech_zero.pitch_4310 ; BSP_CAN_Init(); MOTOR_DM_Register(&g->param->yaw_4310_motor); MOTOR_DM_Register(&g->param->pitch_4310_motor); MOTOR_RM_Register(&g->param->yaw_6020_motor); MOTOR_DM_Enable(&g->param->yaw_4310_motor); MOTOR_DM_Enable(&g->param->pitch_4310_motor); return GIMBAL_OK; } /** * \brief 通过CAN设备更新云台电机反馈信息 * * \param gimbal 云台 * \param can CAN设备 * * \return 函数运行结果 */ int8_t Gimbal_UpdateFeedback(Gimbal_t *gimbal) { if (gimbal == NULL) return GIMBAL_ERR_NULL; MOTOR_RM_UpdateAll(); MOTOR_RM_Update(&gimbal->param->yaw_6020_motor); gimbal->feedback.motor.yaw_6020_motor_feedback.rotor_abs_angle = MOTOR_GetRotorAbsAngle(&(MOTOR_RM_GetMotor(&gimbal->param->yaw_6020_motor)->motor)); gimbal->feedback.motor.yaw_6020_motor_feedback.rotor_speed = MOTOR_GetRotorSpeed(&(MOTOR_RM_GetMotor(&gimbal->param->yaw_6020_motor)->motor)); MOTOR_DM_Update(&gimbal->param->yaw_4310_motor); MOTOR_DM_t *yaw_4310_motor = MOTOR_DM_GetMotor(&gimbal->param->yaw_4310_motor); if(yaw_4310_motor != NULL) { gimbal->feedback.motor.yaw_4310_motor_feedback.rotor_abs_angle = MOTOR_GetRotorAbsAngle(&yaw_4310_motor->motor); gimbal->feedback.motor.yaw_4310_motor_feedback.rotor_speed = MOTOR_GetRotorSpeed(&yaw_4310_motor->motor); } MOTOR_DM_Update(&gimbal->param->pitch_4310_motor); MOTOR_DM_t *pitch_4310_motor = MOTOR_DM_GetMotor(&gimbal->param->pitch_4310_motor); if(pitch_4310_motor != NULL) { gimbal->feedback.motor.pitch_4310_motor_feedback.rotor_abs_angle = MOTOR_GetRotorAbsAngle(&pitch_4310_motor->motor); gimbal->feedback.motor.pitch_4310_motor_feedback.rotor_speed = MOTOR_GetRotorSpeed(&pitch_4310_motor->motor); } return GIMBAL_OK; } /* *更新陀螺仪数据欧拉角的值 */ int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu){ if (gimbal == NULL) { return GIMBAL_ERR; // 参数错误 } gimbal->feedback.imu.gyro = imu->gyro; gimbal->feedback.imu.eulr = imu->eulr; gimbal->feedback.imu.eulr.pit = -imu->eulr.rol; // 坐标系转换 return GIMBAL_OK; } /** * \brief 运行云台控制逻辑 * * \param g 包含云台数据的结构体 * \param g_cmd 云台控制指令 * * \return 函数运行结果 */ // float motor_imu_offset; // float delta_max_pitch; // float delta_min_pitch; /*这三个参数是用来调pitch的*/ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) { if (g == NULL || g_cmd == NULL) { return GIMBAL_ERR; // 参数错误 } g->dt = (BSP_TIME_Get_us() - g->last_wakeup) / 1000000.0f; g->last_wakeup = BSP_TIME_Get_us(); Gimbal_SetMode(g, g_cmd->mode); /*处理小yaw轴控制命令*/ float delta_yaw_6020 = 3.0f*g_cmd->delta_yaw_6020 * g->dt; if(g->param->travel.yaw_6020 > 0) // 有限位才处理 { /* 计算当前电机角度与IMU角度的偏差 */ float motor_imu_offset_6020 = g->feedback.motor.yaw_6020_motor_feedback.rotor_abs_angle - g->feedback.imu.eulr.yaw; /* 处理跨越±π的情况 */ if (motor_imu_offset_6020 > M_PI) motor_imu_offset_6020 -= M_2PI; if (motor_imu_offset_6020 < -M_PI) motor_imu_offset_6020 += M_2PI; /*计算限位距离*/ const float delta_max_6020 = CircleError(g->limit.yaw_6020.max, (g->setpoint.eulr.yaw + motor_imu_offset_6020 + delta_yaw_6020), M_2PI); const float delta_min_6020 = CircleError(g->limit.yaw_6020.min, (g->setpoint.eulr.yaw + motor_imu_offset_6020 + delta_yaw_6020), M_2PI); if(delta_yaw_6020 > delta_max_6020) delta_yaw_6020 = delta_max_6020; if(delta_yaw_6020 < delta_min_6020) delta_yaw_6020 = delta_min_6020; } CircleAdd(&(g->setpoint.eulr.yaw), delta_yaw_6020, M_2PI); /*处理大yaw控制命令，软件限位 - 使用电机绝对角度*/ /*获得小YAW的中点位置，如果小YAW大于中点的一定范围，大YAW开始运动， 跟随小YAW向相同方向运动，将遥控数据进行分配*/ /*小YAW中点*/ float small_yaw_center_offset = g->param->mech_zero.yaw_6020 + g->param->travel.yaw_6020* 0.5f ; g->setpoint.yaw_4310=small_yaw_center_offset; /*处理大pitch控制命令*/ float delta_pitch_4310 = 3.0f*g_cmd->delta_pitch_4310*g->dt; if(g->param->travel.pitch_4310 > 0) // 有限位才处理 { /* 计算当前电机角度与IMU角度的偏差 */ float motor_imu_offset = g ->feedback.motor.pitch_4310_motor_feedback.rotor_abs_angle - g->feedback.imu.eulr.rol; /* 处理跨越±π的情况 */ if (motor_imu_offset > M_PI) motor_imu_offset -= M_2PI; if (motor_imu_offset < -M_PI) motor_imu_offset += M_2PI; /* 计算到限位边界的距离 */ // const float delta_max = CircleError(g->limit.pitch_4310.max, // (g->setpoint.eulr.pit + motor_imu_offset + delta_pitch_4310), M_2PI); // const float delta_min = CircleError(g->limit.pitch_4310.min, // (g->setpoint.eulr.pit + motor_imu_offset + delta_pitch_4310), M_2PI); float delta_max_pitch = CircleError(g->limit.pitch_4310.max, (g->setpoint.eulr.pit + motor_imu_offset + delta_pitch_4310), M_2PI); float delta_min_pitch = CircleError(g->limit.pitch_4310.min, (g->setpoint.eulr.pit + motor_imu_offset + delta_pitch_4310), M_2PI); /* 限制控制命令 */ if (delta_pitch_4310 > delta_max_pitch) delta_pitch_4310 = delta_max_pitch; if (delta_pitch_4310 < delta_min_pitch) delta_pitch_4310 = delta_min_pitch; } CircleAdd(&(g->setpoint.eulr.pit), delta_pitch_4310, M_2PI); /* 控制相关逻辑 */ float yaw_omega_set_point, pitch_omega_set_point,yaw_6020_omega_set_point; switch (g->mode) { case GIMBAL_MODE_RELAX: g->out.yaw_4310 = 0.0f; g->out.pitch_4310 = 0.0f; g->out.yaw_6020 = 0.0f; break; case GIMBAL_MODE_ABSOLUTE: /*绝对坐标 大地坐标系下云台控制*/ /*6020小YAW控制*/ yaw_6020_omega_set_point =PID_Calc(&g->pid.yaw_6020_angle,g->setpoint.eulr.yaw, g->feedback.imu.eulr.yaw,0.0f,g->dt); g->out.yaw_6020 = PID_Calc(&g->pid.yaw_6020_omega,yaw_6020_omega_set_point, g->feedback.imu.gyro.z,0.0f,g->dt); /*4310大YAW控制 这里是单环控制的，有需要加双环 */ yaw_omega_set_point =PID_Calc(&g->pid.yaw_4310_angle,g->setpoint.yaw_4310, g->feedback.motor.yaw_6020_motor_feedback.rotor_abs_angle,g->feedback.motor.yaw_6020_motor_feedback.rotor_speed,g->dt); // g->out.yaw_4310 = PID_Calc(&g->pid.yaw_4310_omega,yaw_omega_set_point, // g->feedback.imu.gyro.z,0.0f,g->dt); g->out.yaw_4310 = yaw_omega_set_point; // 直接输出速度环目标值作为电机输出 /*控制云台4310电机 也是单环，但是加了重力补偿函数，可以根据不一样的情况去拟合函数*/ pitch_omega_set_point =PID_Calc(&g->pid.pitch_4310_angle,g->setpoint.eulr.pit, g->feedback.imu.eulr.rol,0.0f,g->dt); g->out.pitch_4310 = pitch_omega_set_point+poly(g->feedback.motor.pitch_4310_motor_feedback.rotor_abs_angle); break; /*云台相对底盘控制，暂时没写，后面可以更进*/ case GIMBAL_MODE_RELATIVE: g->out.yaw_4310 = 0.0f; g->out.pitch_4310 = 0.0f; g->out.yaw_6020 = 0.0f; break; /* 6020输出滤波 4310个人觉得不是很需要滤波，可以自己加*/ g->out.yaw_6020 = LowPassFilter2p_Apply(&g->filter_out.yaw_6020, g->out.yaw_6020); } return 0; } /** * \brief 云台输出 * * \param g 包含云台数据的结构体 * \param out CAN设备云台输出结构体 * void类型的，可以改成int * 4310为力矩输出控制，传统的电机控制方法，两者之积不要大于12.5 */ void Gimbal_Output(Gimbal_t *g) { MOTOR_RM_SetOutput(&g->param->yaw_6020_motor, g->out.yaw_6020); MOTOR_MIT_Output_t output_yaw_4310 = {0}; output_yaw_4310.torque = g->out.yaw_4310 * 1.0f; MOTOR_MIT_Output_t output_pitch_4310 = {0}; output_pitch_4310.torque = g->out.pitch_4310 * 1.0f; output_pitch_4310.kd=0.2f; MOTOR_RM_Ctrl(&g->param->yaw_6020_motor); MOTOR_DM_MITCtrl(&g->param->yaw_4310_motor, &output_yaw_4310); MOTOR_DM_MITCtrl(&g->param->pitch_4310_motor, &output_pitch_4310); }