From 697104d1ce8f71b13d94f618777e73c3b40a1352 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Mon, 13 Oct 2025 02:22:16 +0800 Subject: [PATCH] =?UTF-8?q?=E6=9A=82=E5=AD=982=E8=BD=B4=E4=BA=91=E5=8F=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- assets/User_code/module/2_axis_gimbal.c | 259 ++++++++++++++++++++++++ assets/User_code/module/2_axis_gimbal.h | 179 ++++++++++++++++ 2 files changed, 438 insertions(+) create mode 100644 assets/User_code/module/2_axis_gimbal.c create mode 100644 assets/User_code/module/2_axis_gimbal.h diff --git a/assets/User_code/module/2_axis_gimbal.c b/assets/User_code/module/2_axis_gimbal.c new file mode 100644 index 0000000..53ff302 --- /dev/null +++ b/assets/User_code/module/2_axis_gimbal.c @@ -0,0 +1,259 @@ +/* + * 云台模组 + */ + +/* Includes ----------------------------------------------------------------- */ +#include "bsp/can.h" +#include "bsp/time.h" +#include "component/filter.h" +#include "component/pid.h" +#include "device/motor_dm.h" +#include "device/motor_rm.h" +#include "gimbal.h" +#include + +/* Private typedef ---------------------------------------------------------- */ +/* Private define ----------------------------------------------------------- */ +/* Private macro ------------------------------------------------------------ */ +/* Private variables -------------------------------------------------------- */ +/* Private function -------------------------------------------------------- */ + +/** + * \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_angle); + PID_Reset(&g->pid.yaw_omega); + PID_Reset(&g->pid.pit_angle); + PID_Reset(&g->pid.pit_omega); + LowPassFilter2p_Reset(&g->filter_out.yaw, 0.0f); + LowPassFilter2p_Reset(&g->filter_out.pit, 0.0f); + + MOTOR_DM_Enable(&(g->param->yaw_motor)); + + AHRS_ResetEulr(&(g->setpoint.eulr)); /* 切换模式后重置设定值 */ + // if (g->mode == GIMBAL_MODE_RELAX) { + // if (mode == GIMBAL_MODE_ABSOLUTE) { + // g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw; + // } else if (mode == GIMBAL_MODE_RELATIVE) { + // g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw; + // } + // } + g->setpoint.eulr.pit = g->feedback.imu.eulr.rol; + g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw; + + g->mode = mode; + return 0; +} + +/* Exported functions ------------------------------------------------------- */ + +/** + * \brief 初始化云台 + * + * \param g 包含云台数据的结构体 + * \param param 包含云台参数的结构体指针 + * \param target_freq 任务预期的运行频率 + * + * \return 函数运行结果 + */ +int8_t Gimbal_Init(Gimbal_t *g, const Gimbal_Params_t *param, + float target_freq) { + if (g == NULL) + return -1; + + g->param = param; + g->mode = GIMBAL_MODE_RELAX; /* 设置默认模式 */ + + /* 初始化云台电机控制PID和LPF */ + PID_Init(&(g->pid.yaw_angle), KPID_MODE_NO_D, target_freq, + &(g->param->pid.yaw_angle)); + PID_Init(&(g->pid.yaw_omega), KPID_MODE_CALC_D, target_freq, + &(g->param->pid.yaw_omega)); + PID_Init(&(g->pid.pit_angle), KPID_MODE_NO_D, target_freq, + &(g->param->pid.pit_angle)); + PID_Init(&(g->pid.pit_omega), KPID_MODE_CALC_D, target_freq, + &(g->param->pid.pit_omega)); + + LowPassFilter2p_Init(&g->filter_out.yaw, target_freq, + g->param->low_pass_cutoff_freq.out); + LowPassFilter2p_Init(&g->filter_out.pit, target_freq, + g->param->low_pass_cutoff_freq.out); + g->limit.yaw.max = g->param->mech_zero.yaw + g->param->travel.yaw; + g->limit.yaw.min = g->param->mech_zero.yaw; + g->limit.pit.max = g->param->mech_zero.pit + g->param->travel.pit; + g->limit.pit.min = g->param->mech_zero.pit; + BSP_CAN_Init(); + + MOTOR_RM_Register(&(g->param->pit_motor)); + MOTOR_DM_Register(&(g->param->yaw_motor)); + + MOTOR_DM_Enable(&(g->param->yaw_motor)); + return 0; +} + +/** + * \brief 通过CAN设备更新云台反馈信息 + * + * \param gimbal 云台 + * \param can CAN设备 + * + * \return 函数运行结果 + */ +int8_t Gimbal_UpdateFeedback(Gimbal_t *gimbal) { + if (gimbal == NULL) + return -1; + + /* 更新RM电机反馈数据（pitch轴） */ + MOTOR_RM_Update(&(gimbal->param->pit_motor)); + MOTOR_RM_t *rm_motor = MOTOR_RM_GetMotor(&(gimbal->param->pit_motor)); + if (rm_motor != NULL) { + gimbal->feedback.motor.pit = rm_motor->feedback; + } + + /* 更新DM电机反馈数据（yaw轴） */ + MOTOR_DM_Update(&(gimbal->param->yaw_motor)); + MOTOR_DM_t *dm_motor = MOTOR_DM_GetMotor(&(gimbal->param->yaw_motor)); + if (dm_motor != NULL) { + gimbal->feedback.motor.yaw = dm_motor->motor.feedback; + } + + return 0; +} + +int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu) { + + if (gimbal == NULL) { + return -1; + } + gimbal->feedback.imu.gyro = imu->gyro; + gimbal->feedback.imu.eulr = imu->eulr; +} + +/** + * \brief 运行云台控制逻辑 + * + * \param g 包含云台数据的结构体 + * \param g_cmd 云台控制指令 + * + * \return 函数运行结果 + */ +int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) { + if (g == NULL || g_cmd == NULL) { + return -1; + } + + g->dt = (BSP_TIME_Get_us() - g->lask_wakeup) / 1000000.0f; + g->lask_wakeup = BSP_TIME_Get_us(); + + Gimbal_SetMode(g, g_cmd->mode); + + /* 处理yaw控制命令，软件限位 - 使用电机绝对角度 */ + float delta_yaw = g_cmd->delta_yaw * g->dt * 1.5f; + if (g->param->travel.yaw > 0) { + /* 计算当前电机角度与IMU角度的偏差 */ + float motor_imu_offset = + g->feedback.motor.yaw.rotor_abs_angle - g->feedback.imu.eulr.yaw; + /* 处理跨越±π的情况 */ + 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.yaw.max, (g->setpoint.eulr.yaw + motor_imu_offset + delta_yaw), + M_2PI); + const float delta_min = CircleError( + g->limit.yaw.min, (g->setpoint.eulr.yaw + motor_imu_offset + delta_yaw), + M_2PI); + + /* 限制控制命令 */ + if (delta_yaw > delta_max) + delta_yaw = delta_max; + if (delta_yaw < delta_min) + delta_yaw = delta_min; + } + CircleAdd(&(g->setpoint.eulr.yaw), delta_yaw, M_2PI); + + /* 处理pitch控制命令，软件限位 - 使用电机绝对角度 */ + float delta_pit = g_cmd->delta_pit * g->dt; + if (g->param->travel.pit > 0) { + /* 计算当前电机角度与IMU角度的偏差 */ + float motor_imu_offset = + g->feedback.motor.pit.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.pit.max, (g->setpoint.eulr.pit + motor_imu_offset + delta_pit), + M_2PI); + const float delta_min = CircleError( + g->limit.pit.min, (g->setpoint.eulr.pit + motor_imu_offset + delta_pit), + M_2PI); + + /* 限制控制命令 */ + if (delta_pit > delta_max) + delta_pit = delta_max; + if (delta_pit < delta_min) + delta_pit = delta_min; + } + CircleAdd(&(g->setpoint.eulr.pit), delta_pit, M_2PI); + + /* 控制相关逻辑 */ + float yaw_omega_set_point, pit_omega_set_point; + switch (g->mode) { + case GIMBAL_MODE_RELAX: + g->out.yaw = 0.0f; + g->out.pit = 0.0f; + break; + + case GIMBAL_MODE_ABSOLUTE: + yaw_omega_set_point = PID_Calc(&(g->pid.yaw_angle), g->setpoint.eulr.yaw, + g->feedback.imu.eulr.yaw, 0.0f, g->dt); + g->out.yaw = PID_Calc(&(g->pid.pit_omega), yaw_omega_set_point, + g->feedback.imu.gyro.z, 0.f, g->dt); + + pit_omega_set_point = PID_Calc(&(g->pid.pit_angle), g->setpoint.eulr.pit, + g->feedback.imu.eulr.rol, 0.0f, g->dt); + g->out.pit = PID_Calc(&(g->pid.pit_omega), pit_omega_set_point, + g->feedback.imu.gyro.y, 0.f, g->dt); + + break; + + /* 输出滤波 */ + g->out.yaw = LowPassFilter2p_Apply(&g->filter_out.yaw, g->out.yaw); + g->out.pit = LowPassFilter2p_Apply(&g->filter_out.pit, g->out.pit); + + return 0; + } +} + +/** + * \brief 云台输出 + * + * \param s 包含云台数据的结构体 + * \param out CAN设备云台输出结构体 + */ +void Gimbal_Output(Gimbal_t *g) { + MOTOR_RM_SetOutput(&g->param->pit_motor, g->out.pit); + MOTOR_MIT_Output_t output = {0}; + output.torque = g->out.yaw * 5.0f; // 乘以减速比 + output.kd = 0.3f; + MOTOR_RM_Ctrl(&g->param->pit_motor); + MOTOR_DM_MITCtrl(&g->param->yaw_motor, &output); +} diff --git a/assets/User_code/module/2_axis_gimbal.h b/assets/User_code/module/2_axis_gimbal.h new file mode 100644 index 0000000..e45faa7 --- /dev/null +++ b/assets/User_code/module/2_axis_gimbal.h @@ -0,0 +1,179 @@ +/* + * 云台模组 + */ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include "component/ahrs.h" +#include "component/filter.h" +#include "component/pid.h" +#include "device/motor.h" +#include "device/motor_dm.h" +#include "device/motor_rm.h" + +/* Exported constants ------------------------------------------------------- */ +#define GIMBAL_OK (0) /* 运行正常 */ +#define GIMBAL_ERR (-1) /* 运行时发现了其他错误 */ +#define GIMBAL_ERR_NULL (-2) /* 运行时发现NULL指针 */ +#define GIMBAL_ERR_MODE (-3) /* 运行时配置了错误的CMD_GimbalMode_t */ + +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ + +typedef enum { + GIMBAL_MODE_RELAX, /* 放松模式，电机不输出。一般情况云台初始化之后的模式 */ + GIMBAL_MODE_ABSOLUTE, /* 绝对坐标系控制，控制在空间内的绝对姿态 */ + GIMBAL_MODE_RELATIVE, /* 相对坐标系控制，控制相对于底盘的姿态 */ +} Gimbal_Mode_t; + +typedef struct { + Gimbal_Mode_t mode; + float delta_yaw; + float delta_pit; +} Gimbal_CMD_t; + +/* 软件限位 */ +typedef struct { + float max; + float min; +} Gimbal_Limit_t; + +/* 云台参数的结构体，包含所有初始化用的参数，通常是const，存好几组。*/ +typedef struct { + MOTOR_RM_Param_t pit_motor; /* pitch轴电机参数 */ + MOTOR_DM_Param_t yaw_motor; /* yaw轴电机参数 */ + struct { + KPID_Params_t yaw_omega; /* yaw轴角速度环PID参数 */ + KPID_Params_t yaw_angle; /* yaw轴角位置环PID参数 */ + KPID_Params_t pit_omega; /* pitch轴角速度环PID参数 */ + KPID_Params_t pit_angle; /* pitch轴角位置环PID参数 */ + } pid; + + /* 低通滤波器截止频率 */ + struct { + float out; /* 电机输出 */ + float gyro; /* 陀螺仪数据 */ + } low_pass_cutoff_freq; + + struct { + float yaw; /* yaw轴机械限位 */ + float pit; /* pitch轴机械限位 */ + } mech_zero; + + struct { + float yaw; /* yaw轴机械限位行程 -1表示无限位 */ + float pit; /* pitch轴机械限位行程 -1表示无限位*/ + } travel; + +} Gimbal_Params_t; + +typedef struct { + AHRS_Gyro_t gyro; + AHRS_Eulr_t eulr; +} Gimbal_IMU_t; +/* 云台反馈数据的结构体，包含反馈控制用的反馈数据 */ +typedef struct { + Gimbal_IMU_t imu; + struct { + MOTOR_Feedback_t yaw; /* yaw轴电机反馈 */ + MOTOR_Feedback_t pit; /* pitch轴电机反馈 */ + } motor; +} Gimbal_Feedback_t; + +/* 云台输出数据的结构体*/ +typedef struct { + float yaw; /* yaw轴电机输出 */ + float pit; /* pitch轴电机输出 */ +} Gimbal_Output_t; +/* + * 运行的主结构体，所有这个文件里的函数都在操作这个结构体。 + * 包含了初始化参数，中间变量，输出变量。 + */ +typedef struct { + uint64_t lask_wakeup; + float dt; + + Gimbal_Params_t *param; /* 云台的参数，用Gimbal_Init设定 */ + /* 模块通用 */ + Gimbal_Mode_t mode; /* 云台模式 */ + + /* PID计算的目标值 */ + struct { + AHRS_Eulr_t eulr; /* 表示云台姿态的欧拉角 */ + } setpoint; + + struct { + KPID_t yaw_angle; /* yaw轴角位置环PID */ + KPID_t yaw_omega; /* yaw轴角速度环PID */ + KPID_t pit_angle; /* pitch轴角位置环PID */ + KPID_t pit_omega; /* pitch轴角速度环PID */ + } pid; + + struct { + Gimbal_Limit_t yaw; + Gimbal_Limit_t pit; + } limit; + + struct { + LowPassFilter2p_t yaw; + LowPassFilter2p_t pit; + } filter_out; + + Gimbal_Output_t out; /* 云台输出 */ + Gimbal_Feedback_t feedback; /* 反馈 */ + +} Gimbal_t; + +/* Exported functions prototypes -------------------------------------------- */ + +/** + * \brief 初始化云台 + * + * \param g 包含云台数据的结构体 + * \param param 包含云台参数的结构体指针 + * \param target_freq 任务预期的运行频率 + * + * \return 函数运行结果 + */ +int8_t Gimbal_Init(Gimbal_t *g, const Gimbal_Params_t *param, + float target_freq); + +/** + * \brief 通过CAN设备更新云台反馈信息 + * + * \param gimbal 云台 + * \param can CAN设备 + * + * \return 函数运行结果 + */ +int8_t Gimbal_UpdateFeedback(Gimbal_t *gimbal); + +int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu); +/** + * \brief 运行云台控制逻辑 + * + * \param g 包含云台数据的结构体 + * \param fb 云台反馈信息 + * \param g_cmd 云台控制指令 + * \param dt_sec 两次调用的时间间隔 + * + * \return 函数运行结果 + */ +int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd); + +/** + * \brief 云台输出 + * + * \param s 包含云台数据的结构体 + * \param out CAN设备云台输出结构体 + */ +void Gimbal_Output(Gimbal_t *g); + +#ifdef __cplusplus +} +#endif