357 lines
12 KiB
C
357 lines
12 KiB
C
/*云台控制代码*/
|
||
|
||
/*
|
||
云台模组
|
||
|
||
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);
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|