606 lines
22 KiB
C
606 lines
22 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;
|
||
}
|
||
|
||
static uint8_t g_scan_active = 0;
|
||
static float g_scan_pitch_center = 0.0f;
|
||
static float g_scan_pitch_dir = 1.0f;
|
||
static float g_scan_yaw_dir = 1.0f;
|
||
static int8_t g_scan_yaw_edge_latch = 0; /* -1: min边界锁存, 1: max边界锁存 */
|
||
|
||
#define GIMBAL_SCAN_SMALL_YAW_SPEED (0.40f)
|
||
#define GIMBAL_SCAN_BIG_YAW_PERIOD_SEC (5.0f)
|
||
#define GIMBAL_SCAN_PITCH_SPEED (0.16f)
|
||
#define GIMBAL_SCAN_PITCH_AMPLITUDE (0.18f)
|
||
#define GIMBAL_SCAN_YAW_EDGE_MARGIN (0.16f)
|
||
#define GIMBAL_SCAN_YAW_EDGE_HYST (0.22f)
|
||
#define GIMBAL_SCAN_YAW_EDGE_RELEASE (0.28f)
|
||
#define GIMBAL_SCAN_PITCH_EDGE_MARGIN (0.12f)
|
||
#define GIMBAL_SCAN_EDGE_SLOW_BAND (0.26f)
|
||
#define GIMBAL_REMOTE_BIG_YAW_SPEED_GAIN (1.6f)
|
||
#define GIMBAL_REMOTE_SMALL_YAW_GAIN (16.0f)
|
||
#define GIMBAL_REMOTE_EDGE_ASSIST_BAND (0.20f)
|
||
#define GIMBAL_YAW6020_INPUT_DEADBAND (0.0005f)
|
||
#define GIMBAL_CONTROL_DT_MAX (0.02f)
|
||
|
||
|
||
|
||
/**
|
||
* \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);
|
||
PID_Reset(&g->pid.daohang_6020_angle);
|
||
PID_Reset(&g->pid.daohang_4310_angle);
|
||
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->setpoint.yaw_4310 = g->feedback.motor.yaw_4310_motor_feedback.rotor_abs_angle;
|
||
|
||
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; /* 设置默认模式 不输出*/
|
||
g->last_wakeup = BSP_TIME_Get_us();
|
||
g->dt = 0.0f;
|
||
/*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 );
|
||
PID_Init(&g->pid.yaw_4310_omega,KPID_MODE_CALC_D, target_freq,&g->param->pid.yaw_4310_motor_omega );
|
||
/*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 );
|
||
/*导航PID*/
|
||
PID_Init(&g->pid.daohang_6020_angle,KPID_MODE_CALC_D, target_freq,&g->param->pid.daohang_6020_motor_angle );
|
||
PID_Init(&g->pid.daohang_4310_angle,KPID_MODE_CALC_D, target_freq,&g->param->pid.daohang_4310_motor_angle );
|
||
/*输出滤波器初始化*/
|
||
LowPassFilter2p_Init(&g->filter_out.yaw_6020, target_freq, g->param->low_pass_cutoff_freq.out);
|
||
LowPassFilter2p_Init(&g->filter_out.gyro_yaw, target_freq, g->param->low_pass_cutoff_freq.gyro);
|
||
LowPassFilter2p_Init(&g->filter_out.gyro_pitch, target_freq,g->param->low_pass_cutoff_freq.gyro);
|
||
LowPassFilter2p_Init(&g->filter_out.gyro_rol, target_freq, g->param->low_pass_cutoff_freq.gyro);
|
||
/*机械限位*/
|
||
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.yaw_4310.max = g->param->mech_zero.yaw_4310 + g->param->travel.yaw_4310;
|
||
g->limit.yaw_4310.min = g->param->mech_zero.yaw_4310 ;
|
||
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))/10;
|
||
|
||
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;
|
||
|
||
/* 先同步完整欧拉角，保证rol轴可用于90度装配下的pitch控制 */
|
||
gimbal->feedback.imu.eulr = imu->eulr;
|
||
/* 保留pit映射，兼容现有使用pit坐标的逻辑 */
|
||
gimbal->feedback.imu.eulr.pit = -imu->eulr.rol; // 坐标系转换
|
||
|
||
gimbal->feedback.imu.quat = imu->quat;
|
||
|
||
|
||
|
||
|
||
return GIMBAL_OK;
|
||
}
|
||
|
||
// int8_t Gimbal_Control_mode(Gimbal_t *g,Gimbal_CMD_t *g_cmd){
|
||
// if (g == NULL || g_cmd == NULL) {
|
||
// return -1;
|
||
// }
|
||
// switch(g_cmd->ctrl_mode){
|
||
// case GIMBAL_MODE_REMOTE:
|
||
// g->setpoint.eulr.yaw=g_cmd->delta_yaw_6020;
|
||
// g->setpoint.eulr.pit=g_cmd->delta_pitch_4310;
|
||
// break;
|
||
// case GIMBAL_MODE_AI:
|
||
// g->setpoint.eulr.yaw=g_cmd->set_yaw;
|
||
// g->setpoint.eulr.pit=g_cmd->set_pitch;
|
||
// break;
|
||
// default:
|
||
// break;
|
||
// }
|
||
// return GIMBAL_OK;
|
||
// };
|
||
|
||
/**
|
||
* \brief 运行云台控制逻辑
|
||
*
|
||
* \param g 包含云台数据的结构体
|
||
* \param g_cmd 云台控制指令
|
||
*
|
||
* \return 函数运行结果
|
||
*/
|
||
// float motor_imu_offset;
|
||
// float delta_max_pitch;
|
||
// float delta_min_pitch;
|
||
/*这三个参数是用来调pitch的*/
|
||
float yaw_omega_set_point, pitch_omega_set_point,yaw_6020_omega_set_point;
|
||
float motor_imu_offset_6020;
|
||
int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd)
|
||
{
|
||
if (g == NULL || g_cmd == NULL)
|
||
{
|
||
return GIMBAL_ERR; // 参数错误
|
||
}
|
||
|
||
{
|
||
uint64_t now_us = BSP_TIME_Get_us();
|
||
g->dt = (now_us - g->last_wakeup) / 1000000.0f;
|
||
g->last_wakeup = now_us;
|
||
if (g->dt < 0.0f || g->dt > GIMBAL_CONTROL_DT_MAX) {
|
||
g->dt = 0.002f;
|
||
}
|
||
}
|
||
|
||
Gimbal_SetMode(g, g_cmd->mode);
|
||
|
||
if (g_cmd->ctrl_mode == GIMBAL_MODE_SCAN) {
|
||
float yaw_speed_scale = 1.0f;
|
||
float big_yaw_speed = 0.0f;
|
||
float pitch_speed_scale = 1.0f;
|
||
float yaw_abs = g->feedback.motor.yaw_6020_motor_feedback.rotor_abs_angle;
|
||
float pitch_abs = g->feedback.motor.pitch_4310_motor_feedback.rotor_abs_angle;
|
||
|
||
if (!g_scan_active) {
|
||
float yaw_mid = 0.5f * (g->limit.yaw_6020.max + g->limit.yaw_6020.min);
|
||
g_scan_active = 1;
|
||
g_scan_pitch_center = g->feedback.imu.eulr.rol;
|
||
g_scan_pitch_dir = 1.0f;
|
||
g_scan_yaw_dir = (yaw_abs >= yaw_mid) ? -1.0f : 1.0f;
|
||
g_scan_yaw_edge_latch = 0;
|
||
g->setpoint.yaw_4310 = g->feedback.motor.yaw_4310_motor_feedback.rotor_abs_angle;
|
||
}
|
||
|
||
if (GIMBAL_SCAN_BIG_YAW_PERIOD_SEC > 0.0f) {
|
||
/* 连续旋转：按周期计算角速度，5秒转一圈即 2PI/5 */
|
||
big_yaw_speed = M_2PI / GIMBAL_SCAN_BIG_YAW_PERIOD_SEC;
|
||
}
|
||
|
||
if (g_scan_yaw_edge_latch == 1) {
|
||
g_scan_yaw_dir = -1.0f;
|
||
if (yaw_abs < g->limit.yaw_6020.max - GIMBAL_SCAN_YAW_EDGE_RELEASE) {
|
||
g_scan_yaw_edge_latch = 0;
|
||
}
|
||
} else if (g_scan_yaw_edge_latch == -1) {
|
||
g_scan_yaw_dir = 1.0f;
|
||
if (yaw_abs > g->limit.yaw_6020.min + GIMBAL_SCAN_YAW_EDGE_RELEASE) {
|
||
g_scan_yaw_edge_latch = 0;
|
||
}
|
||
} else if (yaw_abs >= g->limit.yaw_6020.max - GIMBAL_SCAN_YAW_EDGE_MARGIN) {
|
||
g_scan_yaw_dir = -1.0f;
|
||
g_scan_yaw_edge_latch = 1;
|
||
} else if (yaw_abs <= g->limit.yaw_6020.min + GIMBAL_SCAN_YAW_EDGE_MARGIN) {
|
||
g_scan_yaw_dir = 1.0f;
|
||
g_scan_yaw_edge_latch = -1;
|
||
} else if (g_scan_yaw_dir > 0.0f && yaw_abs > g->limit.yaw_6020.max - GIMBAL_SCAN_YAW_EDGE_HYST) {
|
||
g_scan_yaw_dir = -1.0f;
|
||
} else if (g_scan_yaw_dir < 0.0f && yaw_abs < g->limit.yaw_6020.min + GIMBAL_SCAN_YAW_EDGE_HYST) {
|
||
g_scan_yaw_dir = 1.0f;
|
||
}
|
||
|
||
if (pitch_abs >= g->limit.pitch_4310.max - GIMBAL_SCAN_PITCH_EDGE_MARGIN) {
|
||
g_scan_pitch_dir = -1.0f;
|
||
} else if (pitch_abs <= g->limit.pitch_4310.min + GIMBAL_SCAN_PITCH_EDGE_MARGIN) {
|
||
g_scan_pitch_dir = 1.0f;
|
||
}
|
||
|
||
{
|
||
float yaw_dist_to_edge_max = g->limit.yaw_6020.max - yaw_abs;
|
||
float yaw_dist_to_edge_min = yaw_abs - g->limit.yaw_6020.min;
|
||
float yaw_min_dist = (yaw_dist_to_edge_max < yaw_dist_to_edge_min) ? yaw_dist_to_edge_max : yaw_dist_to_edge_min;
|
||
if (yaw_min_dist < GIMBAL_SCAN_EDGE_SLOW_BAND) {
|
||
if (yaw_min_dist < 0.0f) yaw_min_dist = 0.0f;
|
||
yaw_speed_scale = (yaw_min_dist / GIMBAL_SCAN_EDGE_SLOW_BAND);
|
||
}
|
||
}
|
||
|
||
{
|
||
float pitch_dist_to_edge_max = g->limit.pitch_4310.max - pitch_abs;
|
||
float pitch_dist_to_edge_min = pitch_abs - g->limit.pitch_4310.min;
|
||
float pitch_min_dist = (pitch_dist_to_edge_max < pitch_dist_to_edge_min) ? pitch_dist_to_edge_max : pitch_dist_to_edge_min;
|
||
if (pitch_min_dist < GIMBAL_SCAN_EDGE_SLOW_BAND) {
|
||
if (pitch_min_dist < 0.0f) pitch_min_dist = 0.0f;
|
||
pitch_speed_scale = 0.25f + 0.75f * (pitch_min_dist / GIMBAL_SCAN_EDGE_SLOW_BAND);
|
||
}
|
||
}
|
||
|
||
if (g->feedback.imu.eulr.rol >= g_scan_pitch_center + GIMBAL_SCAN_PITCH_AMPLITUDE) {
|
||
g_scan_pitch_dir = -1.0f;
|
||
} else if (g->feedback.imu.eulr.rol <= g_scan_pitch_center - GIMBAL_SCAN_PITCH_AMPLITUDE) {
|
||
g_scan_pitch_dir = 1.0f;
|
||
}
|
||
|
||
g_cmd->delta_yaw_6020 = g_scan_yaw_dir * GIMBAL_SCAN_SMALL_YAW_SPEED * yaw_speed_scale;
|
||
g_cmd->delta_pitch_4310 = g_scan_pitch_dir * GIMBAL_SCAN_PITCH_SPEED * pitch_speed_scale;
|
||
if (g->param->travel.yaw_4310 > 0.0f) {
|
||
g->setpoint.yaw_4310 += big_yaw_speed * g->dt;
|
||
if (g->setpoint.yaw_4310 > g->limit.yaw_4310.max) g->setpoint.yaw_4310 = g->limit.yaw_4310.max;
|
||
if (g->setpoint.yaw_4310 < g->limit.yaw_4310.min) g->setpoint.yaw_4310 = g->limit.yaw_4310.min;
|
||
} else {
|
||
CircleAdd(&(g->setpoint.yaw_4310), big_yaw_speed * g->dt, M_2PI);
|
||
}
|
||
g_cmd->yaw_vel = 0.0f;
|
||
g_cmd->yaw_accl = 0.0f;
|
||
g_cmd->pit_vel = 0.0f;
|
||
g_cmd->pit_accl = 0.0f;
|
||
} else {
|
||
g_scan_active = 0;
|
||
g_scan_yaw_dir = 1.0f;
|
||
g_scan_yaw_edge_latch = 0;
|
||
}
|
||
|
||
/*处理小yaw轴控制命令*/
|
||
float yaw_6020_gain = 10.0f;
|
||
if (g_cmd->ctrl_mode == GIMBAL_MODE_REMOTE) {
|
||
yaw_6020_gain = GIMBAL_REMOTE_SMALL_YAW_GAIN;
|
||
}
|
||
float delta_yaw_6020 = yaw_6020_gain * g_cmd->delta_yaw_6020 * g->dt;
|
||
float yaw_abs_now = g->feedback.motor.yaw_6020_motor_feedback.rotor_abs_angle;
|
||
float yaw_abs_target = yaw_abs_now;
|
||
|
||
/* 摇杆无输入时抑制微小噪声，避免setpoint慢漂 */
|
||
if (fabsf(delta_yaw_6020) < GIMBAL_YAW6020_INPUT_DEADBAND) {
|
||
delta_yaw_6020 = 0.0f;
|
||
}
|
||
|
||
if (g->param->travel.yaw_6020 > 0.0f) {
|
||
/* 边界方向判定：到边界时只允许朝边界内侧运动 */
|
||
if (yaw_abs_now >= g->limit.yaw_6020.max - GIMBAL_SCAN_YAW_EDGE_MARGIN && delta_yaw_6020 > 0.0f) {
|
||
delta_yaw_6020 = 0.0f;
|
||
}
|
||
if (yaw_abs_now <= g->limit.yaw_6020.min + GIMBAL_SCAN_YAW_EDGE_MARGIN && delta_yaw_6020 < 0.0f) {
|
||
delta_yaw_6020 = 0.0f;
|
||
}
|
||
|
||
yaw_abs_target = yaw_abs_now + delta_yaw_6020;
|
||
if (yaw_abs_target > g->limit.yaw_6020.max) yaw_abs_target = g->limit.yaw_6020.max;
|
||
if (yaw_abs_target < g->limit.yaw_6020.min) yaw_abs_target = g->limit.yaw_6020.min;
|
||
|
||
/* 直接用允许的电机角增量映射到欧拉角目标，避免CircleAdd累积导致回弹 */
|
||
g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw + (yaw_abs_target - yaw_abs_now);
|
||
} else {
|
||
CircleAdd(&(g->setpoint.eulr.yaw), delta_yaw_6020, M_2PI);
|
||
}
|
||
|
||
// g->setpoint.eulr.yaw = g->setpoint.NUC_Yaw;
|
||
|
||
/*处理大yaw控制命令，软件限位 - 使用电机绝对角度*/
|
||
/*获得小YAW的中点位置，如果小YAW大于中点的一定范围，大YAW开始运动，
|
||
跟随小YAW向相同方向运动，将遥控数据进行分配*/
|
||
|
||
if (g_cmd->ctrl_mode == GIMBAL_MODE_REMOTE) {
|
||
float delta_yaw_4310 = -GIMBAL_REMOTE_BIG_YAW_SPEED_GAIN * g_cmd->delta_yaw_4310 * g->dt;
|
||
if (g->param->travel.yaw_6020 > 0.0f) {
|
||
float yaw_abs_now = g->feedback.motor.yaw_6020_motor_feedback.rotor_abs_angle;
|
||
float dist_to_max = g->limit.yaw_6020.max - yaw_abs_now;
|
||
float dist_to_min = yaw_abs_now - g->limit.yaw_6020.min;
|
||
float edge_assist = 1.0f;
|
||
if (g_cmd->delta_yaw_6020 > 0.0f && dist_to_max < GIMBAL_REMOTE_EDGE_ASSIST_BAND) {
|
||
edge_assist += (GIMBAL_REMOTE_EDGE_ASSIST_BAND - dist_to_max) / GIMBAL_REMOTE_EDGE_ASSIST_BAND;
|
||
} else if (g_cmd->delta_yaw_6020 < 0.0f && dist_to_min < GIMBAL_REMOTE_EDGE_ASSIST_BAND) {
|
||
edge_assist += (GIMBAL_REMOTE_EDGE_ASSIST_BAND - dist_to_min) / GIMBAL_REMOTE_EDGE_ASSIST_BAND;
|
||
}
|
||
if (edge_assist < 1.0f) edge_assist = 1.0f;
|
||
if (edge_assist > 2.0f) edge_assist = 2.0f;
|
||
delta_yaw_4310 *= edge_assist;
|
||
}
|
||
if (g->param->travel.yaw_4310 > 0.0f) {
|
||
g->setpoint.yaw_4310 += delta_yaw_4310;
|
||
if (g->setpoint.yaw_4310 > g->limit.yaw_4310.max) g->setpoint.yaw_4310 = g->limit.yaw_4310.max;
|
||
if (g->setpoint.yaw_4310 < g->limit.yaw_4310.min) g->setpoint.yaw_4310 = g->limit.yaw_4310.min;
|
||
} else {
|
||
CircleAdd(&(g->setpoint.yaw_4310), delta_yaw_4310, M_2PI);
|
||
}
|
||
}
|
||
|
||
/*处理大pitch控制命令*/
|
||
float delta_pitch_4310 = 8.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);
|
||
// g->setpoint.eulr.pit = g->setpoint.NUC_Pitch;
|
||
|
||
/* 控制相关逻辑 */
|
||
|
||
switch (g_cmd->ctrl_mode) {
|
||
|
||
case GIMBAL_MODE_REMOTE:
|
||
g->setpoint.eulr.yaw = g->setpoint.eulr.yaw;
|
||
g->setpoint.eulr.pit = g->setpoint.eulr.pit;
|
||
break;
|
||
|
||
case GIMBAL_MODE_SCAN:
|
||
g->setpoint.eulr.yaw = g->setpoint.eulr.yaw;
|
||
g->setpoint.eulr.pit = g->setpoint.eulr.pit;
|
||
break;
|
||
|
||
case GIMBAL_MODE_AI:
|
||
g->setpoint.eulr.yaw = g_cmd->set_yaw;
|
||
g->setpoint.eulr.pit = g_cmd->set_pitch;
|
||
|
||
g->setpoint.yaw_vel = g_cmd->yaw_vel;
|
||
g->setpoint.yaw_accl = g_cmd->yaw_accl;
|
||
g->setpoint.pit_vel = g_cmd->pit_vel;
|
||
g->setpoint.pit_accl = g_cmd->pit_accl;
|
||
break;
|
||
}
|
||
|
||
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控制*/
|
||
// Gimbal_Control_mode(g, g_cmd);
|
||
|
||
|
||
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 = g->pid.yaw_6020_omega,yaw_6020_omega_set_point,
|
||
// g-PID_Calc(&>feedback.imu.gyro.z,0.0f,g->dt);
|
||
g->out.yaw_6020 = yaw_6020_omega_set_point+g->param->K_forward_pid.K_vel*g->setpoint.yaw_vel+g->param->K_forward_pid.K_accl*g->setpoint.yaw_accl;
|
||
/*4310大YAW控制
|
||
这里是单环控制的，有需要加双环
|
||
*/
|
||
|
||
|
||
yaw_omega_set_point =PID_Calc(&g->pid.yaw_4310_angle,g->setpoint.yaw_4310,
|
||
g->feedback.motor.yaw_4310_motor_feedback.rotor_abs_angle,g->feedback.motor.yaw_4310_motor_feedback.rotor_speed,g->dt);
|
||
g->out.yaw_4310 = PID_Calc(&g->pid.yaw_4310_omega,yaw_omega_set_point,
|
||
g->feedback.motor.yaw_4310_motor_feedback.rotor_speed,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,-g->feedback.imu.gyro.y,g->dt);
|
||
g->out.pitch_4310 = pitch_omega_set_point+g->param->K_forward_pid.K_vel*g->setpoint.pit_vel+g->param->K_forward_pid.K_accl*g->setpoint.pit_accl+poly(g->feedback.motor.pitch_4310_motor_feedback.rotor_abs_angle);
|
||
// g->out.pitch_4310 = PID_Calc(&g->pid.pitch_4310_omega,8*pitch_omega_set_point,
|
||
// -g->feedback.imu.gyro.y,0.0f,g->dt)+poly(g->feedback.motor.pitch_4310_motor_feedback.rotor_abs_angle);
|
||
// 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;
|
||
|
||
case GIMBAL_MODE_DAOHANG:
|
||
/*导航模式，NUC直接给出目标角度，云台控制环跟随即可*/
|
||
g->setpoint.eulr.yaw = g->param->mech_zero.yaw_6020+g->param->travel.yaw_6020*0.5f; // 这里可以直接设置为NUC传来的目标角度
|
||
g->setpoint.yaw_4310 = 4.51735544f;
|
||
|
||
g->out.yaw_6020 = PID_Calc(&g->pid.daohang_6020_angle,g->setpoint.eulr.yaw,
|
||
g->feedback.motor.yaw_6020_motor_feedback.rotor_abs_angle,0.0f,g->dt);
|
||
g->out.yaw_4310 = PID_Calc(&g->pid.daohang_4310_angle,g->setpoint.yaw_4310,
|
||
g->feedback.motor.yaw_4310_motor_feedback.rotor_abs_angle,0.0f,g->dt);
|
||
|
||
break;
|
||
}
|
||
/* 6020输出滤波 4310个人觉得不是很需要滤波，可以自己加*/
|
||
// g->out.yaw_6020 = LowPassFilter2p_Apply(&g->filter_out.yaw_6020, g->out.yaw_6020);
|
||
// g->out.yaw_6020 = LowPassFilter2p_Apply(&g->filter_out.yaw_6020, g->out.yaw_6020);
|
||
// g->out.pitch_4310 = LowPassFilter2p_Apply(&g->filter_out.pitch_4310, g->out.pitch_4310);
|
||
// g->out.yaw_4310 = LowPassFilter2p_Apply(&g->filter_out.yaw_4310, g->out.yaw_4310);
|
||
|
||
|
||
|
||
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 * 3.0f;
|
||
// output_yaw_4310.kp = 0.2f;
|
||
// output_yaw_4310.kd = 0.1f;
|
||
// output_yaw_4310.kd=0.1f;
|
||
|
||
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);
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|