robofish/rm_balance
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

云台差不多了

Browse Source
This commit is contained in:
Robofish 2025-10-01 02:29:24 +08:00
parent 3892733f4d
commit 5d310fb707
9 changed files with 367 additions and 232 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

72
User/module/config.c
View File

@ -5,10 +5,12 @@
/* Includes ----------------------------------------------------------------- */
#include "module/config.h"
#include "bsp/can.h"
#include "device/motor_dm.h"
#include "device/motor_rm.h"
#include "component/pid.h"
#include "component/user_math.h"
#include "device/rc_can.h"
#include <stdbool.h>
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
@ -83,6 +85,76 @@ Config_RobotParam_t robot_config = {
},
.gimbal_param = {
.pid = {
.yaw_omega = {
.k = 0.5f,
.p = 1.0f,
.i = 0.5f,
.d = 0.0f,
.i_limit = 1.0f,
.out_limit = 1.0f,
.d_cutoff_freq = -1.0f,
.range = -1.0f,
},
.yaw_angle = {
.k = 10.0f,
.p = 3.0f,
.i = 0.0f,
.d = 0.1f,
.i_limit = 0.0f,
.out_limit = 10.0f,
.d_cutoff_freq = -1.0f,
.range = M_2PI,
},
.pit_omega = {
.k = 0.25f,
.p = 1.0f,
.i = 0.0f,
.d = 0.0f,
.i_limit = 1.0f,
.out_limit = 1.0f,
.d_cutoff_freq = -1.0f,
.range = -1.0f,
},
.pit_angle = {
.k = 2.0f,
.p = 5.0f,
.i = 2.5f,
.d = 0.0f,
.i_limit = 0.0f,
.out_limit = 10.0f,
.d_cutoff_freq = -1.0f,
.range = M_2PI,
},
},
.mech_zero = {
.yaw = 0.0f,
.pit = 3.8f,
},
.travel = {
.yaw = -1.0f,
.pit = 0.8f,
},
.low_pass_cutoff_freq = {
.out = -1.0f,
.gyro = 1000.0f,
},
.pit_motor ={
.can = BSP_CAN_2,
.id = 0x209,
.gear = false,
.module = MOTOR_GM6020,
.reverse = true,
},
.yaw_motor = {
.can = BSP_CAN_1,
.can_id = 0x1,
.master_id = 0x11,
.module = MOTOR_DM_J4310,
.reverse = false,
}
},
.rc_can_param = {
.can = BSP_CAN_1,
.mode = RC_CAN_MODE_MASTER,

2
User/module/config.h
View File

@ -15,7 +15,9 @@ extern "C" {
typedef struct {
Shoot_Params_t shoot_param;
Gimbal_Params_t gimbal_param;
RC_CAN_Param_t rc_can_param;
// Shoot_Params_t shoot_param; /* 射击参数 */
} Config_RobotParam_t;

330
User/module/gimbal.c
View File

@ -4,8 +4,12 @@
/* Includes ----------------------------------------------------------------- */
#include "gimbal.h"
#include "bsp/mm.h"
#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"
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
@ -21,28 +25,31 @@
*
* \return 函数运行结果
*/
static int8_t Gimbal_SetMode(Gimbal_t *g, CMD_GimbalMode_t mode) {
if (g == NULL) return -1;
if (mode == g->mode) return GIMBAL_OK;
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和滤波器 */
for (uint8_t i = 0; i < GIMBAL_PID_NUM; i++) {
PID_Reset(g->pid + i);
}
for (uint8_t i = 0; i < GIMBAL_ACTR_NUM; i++) {
LowPassFilter2p_Reset(g->filter_out + i, 0.0f);
}
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.eulr.encoder.yaw;
g->setpoint.eulr.yaw = 3.7f;
g->setpoint.eulr.pit = 2.2f;
} else if (mode == GIMBAL_MODE_RELATIVE) {
g->setpoint.eulr.yaw = g->feedback.eulr.encoder.yaw;
}
}
// 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;
@ -59,33 +66,38 @@ static int8_t Gimbal_SetMode(Gimbal_t *g, CMD_GimbalMode_t mode) {
*
* \return 函数运行结果
*/
int8_t Gimbal_Init(Gimbal_t *g, const Gimbal_Params_t *param, float limit_max,
int8_t Gimbal_Init(Gimbal_t *g, const Gimbal_Params_t *param,
float target_freq) {
if (g == NULL) return -1;
if (g == NULL)
return -1;
g->param = param; /* 初始化参数 */
g->param = param;
g->mode = GIMBAL_MODE_RELAX; /* 设置默认模式 */
/* 设置软件限位 */
if (g->param->reverse.pit) CircleReverse(&limit_max);
g->limit.min = g->limit.max = limit_max;
CircleAdd(&(g->limit.min), -g->param->pitch_travel_rad, M_2PI);
/* 初始化云台电机控制PID和LPF */
PID_Init(&(g->pid[GIMBAL_PID_YAW_ANGLE_IDX]), KPID_MODE_NO_D, target_freq,
&(g->param->pid[GIMBAL_PID_YAW_ANGLE_IDX]));
PID_Init(&(g->pid[GIMBAL_PID_YAW_OMEGA_IDX]), KPID_MODE_CALC_D, target_freq,
&(g->param->pid[GIMBAL_PID_YAW_OMEGA_IDX]));
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));
PID_Init(&(g->pid[GIMBAL_PID_PIT_ANGLE_IDX]), KPID_MODE_NO_D, target_freq,
&(g->param->pid[GIMBAL_PID_PIT_ANGLE_IDX]));
PID_Init(&(g->pid[GIMBAL_PID_PIT_OMEGA_IDX]), KPID_MODE_CALC_D, target_freq,
&(g->param->pid[GIMBAL_PID_PIT_OMEGA_IDX]));
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));
for (uint8_t i = 0; i < GIMBAL_ACTR_NUM; i++) {
LowPassFilter2p_Init(g->filter_out + i, target_freq,
g->param->low_pass_cutoff_freq.out);
}
MOTOR_DM_Enable(&(g->param->yaw_motor));
return 0;
}
@ -97,163 +109,143 @@ int8_t Gimbal_Init(Gimbal_t *g, const Gimbal_Params_t *param, float limit_max,
*
* \return 函数运行结果
*/
int8_t Gimbal_UpdateFeedback(Gimbal_t *gimbal, const CAN_t *can) {
if (gimbal == NULL) return -1;
if (can == NULL) return -1;
int8_t Gimbal_UpdateFeedback(Gimbal_t *gimbal) {
if (gimbal == NULL)
return -1;
gimbal->feedback.eulr.encoder.yaw = can->motor.gimbal.named.yaw.rotor_angle;
gimbal->feedback.eulr.encoder.pit = can->motor.gimbal.named.pit.rotor_angle;
/* 更新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;
}
if (gimbal->param->reverse.yaw)
CircleReverse(&(gimbal->feedback.eulr.encoder.yaw));
if (gimbal->param->reverse.pit)
CircleReverse(&(gimbal->feedback.eulr.encoder.pit));
/* 更新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 fb 云台反馈信息
* \param g_cmd 云台控制指令
* \param dt_sec 两次调用的时间间隔
*
* \return 函数运行结果
*/
int8_t Gimbal_Control(Gimbal_t *g, CMD_GimbalCmd_t *g_cmd, uint32_t now) {
if (g == NULL) return -1;
if (g_cmd == NULL) return -1;
int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
if (g == NULL || g_cmd == NULL) {
return -1;
}
g->dt = (float)(now - g->lask_wakeup) / 1000.0f;
g->lask_wakeup = now;
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坐标正方向与遥控器操作逻辑相反 */
g_cmd->delta_eulr.pit = -g_cmd->delta_eulr.pit;
g_cmd->delta_eulr.yaw = -g_cmd->delta_eulr.yaw;
/* 处理yaw控制命令 */
CircleAdd(&(g->setpoint.eulr.yaw), g_cmd->delta_eulr.yaw, M_2PI);
/* 处理pitch控制命令，软件限位 */
const float delta_max =
CircleError(g->limit.max,
(g->feedback.eulr.encoder.pit + g->setpoint.eulr.pit -
g->feedback.eulr.encoder.pit),
M_2PI);
const float delta_min =
CircleError(g->limit.min,
(g->feedback.eulr.encoder.pit + g->setpoint.eulr.pit -
g->feedback.eulr.encoder.pit),
M_2PI);
Clip(&(g_cmd->delta_eulr.pit), delta_min, delta_max);
g->setpoint.eulr.pit += g_cmd->delta_eulr.pit;
/* 限制yaw转动范围（待优化） */
if (g->setpoint.eulr.yaw > 5.9f && g->setpoint.eulr.yaw < 6.01f) {
g->setpoint.eulr.yaw = 5.9f;
} else if (g->setpoint.eulr.yaw > 1.4f && g->setpoint.eulr.yaw < 1.71f) {
g->setpoint.eulr.yaw = 1.71f;
/* 处理yaw控制命令，软件限位 - 使用电机绝对角度 */
float delta_yaw = g_cmd->delta_yaw * g->dt;
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);
/* 重置输入指令，防止重复处理 */
AHRS_ResetEulr(&(g_cmd->delta_eulr));
/* 处理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:
for (uint8_t i = 0; i < GIMBAL_ACTR_NUM; i++) g->out[i] = 0.0f;
break;
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[GIMBAL_PID_YAW_ANGLE_IDX]), g->setpoint.eulr.yaw,
g->feedback.eulr.encoder.yaw, 0.0f, g->dt);
g->out[GIMBAL_ACTR_YAW_IDX] =
PID_Calc(&(g->pid[GIMBAL_PID_YAW_OMEGA_IDX]), yaw_omega_set_point,
0.0, 0.f, g->dt);
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[GIMBAL_PID_PIT_ANGLE_IDX]), g->setpoint.eulr.pit,
g->feedback.eulr.encoder.pit, 0.0f, g->dt);
g->out[GIMBAL_ACTR_PIT_IDX] =
PID_Calc(&(g->pid[GIMBAL_PID_PIT_OMEGA_IDX]), pit_omega_set_point,
0.0, 0.f, g->dt);
break;
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);
// yaw_omega_set_point = PID_Calc(&(g->pid.yaw_angle), g->setpoint.eulr.yaw,
// g->feedback.motor.yaw.rotor_abs_angle, 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);
case GIMBAL_MODE_RELATIVE:
for (uint8_t i = 0; i < GIMBAL_ACTR_NUM; i++) g->out[i] = 0.0f;
break;
}
// pit_omega_set_point = PID_Calc(&(g->pid.pit_angle), g->setpoint.eulr.pit,
// g->feedback.motor.pit.rotor_abs_angle, 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;
/* 输出滤波 */
for (uint8_t i = 0; i < GIMBAL_ACTR_NUM; i++)
g->out[i] = LowPassFilter2p_Apply(g->filter_out + i, g->out[i]);
/* 输出滤波 */
g->out.yaw = LowPassFilter2p_Apply(&g->filter_out.yaw, g->out.yaw);
g->out.pit = LowPassFilter2p_Apply(&g->filter_out.pit, g->out.pit);
/* 处理电机反装 */
if (g->param->reverse.yaw)
g->out[GIMBAL_ACTR_YAW_IDX] = -g->out[GIMBAL_ACTR_YAW_IDX];
if (g->param->reverse.pit)
g->out[GIMBAL_ACTR_PIT_IDX] = -g->out[GIMBAL_ACTR_PIT_IDX];
if (g->out[GIMBAL_ACTR_YAW_IDX] < 0.f) {
__NOP();
}
if (g->out[GIMBAL_ACTR_PIT_IDX] > 0.5f) {
__NOP();
}
return 0;
}
/**
* \brief 打包云台数据给AI
*
* \param g 包含云台数据的结构体
* \param gimbal_for_ai 云台AI数据
*
* \return 函数运行结果
*/
uint8_t Gimbal_PackAI(Gimbal_t *g, const AHRS_Eulr_t *ai){
memcpy((void *)ai, (const void *)&(g->feedback.eulr.encoder),
sizeof(g->feedback.eulr.encoder));
return 0;
}
/**
* \brief 复制云台输出值
*
* \param s 包含云台数据的结构体
* \param out CAN设备云台输出结构体
*/
void Gimbal_DumpOutput(Gimbal_t *g, CAN_GimbalOutput_t *out) {
out->named.yaw = g->out[GIMBAL_ACTR_YAW_IDX];
out->named.pit = g->out[GIMBAL_ACTR_PIT_IDX];
}
/**
* \brief 清空输出值
*
* \param output 要清空的结构体
*/
void Gimbal_ResetOutput(CAN_GimbalOutput_t *output) {
int i = 0;
for (i = 0; i < 3; i++) {
output->as_array[i] = 0.0f;
return 0;
}
}
/**
* @brief 导出云台UI数据
*
* @param g 云台结构体
* @param ui UI结构体
*/
void Gimbal_DumpUI(const Gimbal_t *g, Referee_GimbalUI_t *ui) {
ui->mode = g->mode;
}
/**
* \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 * 2.5;
MOTOR_RM_Ctrl(&g->param->pit_motor);
MOTOR_DM_MITCtrl(&g->param->yaw_motor, &output);
}

124
User/module/gimbal.h
View File

@ -12,8 +12,9 @@ extern "C" {
#include "component/ahrs.h"
#include "component/filter.h"
#include "component/pid.h"
#include "device/motor_rm.h"
#include "device/motor.h"
#include "device/motor_dm.h"
#include "device/motor_rm.h"
/* Exported constants ------------------------------------------------------- */
#define GIMBAL_OK (0) /* 运行正常 */
@ -25,17 +26,33 @@ extern "C" {
/* Exported types ----------------------------------------------------------- */
typedef enum {
GIMBAL_MODE_RELAX, /* 放松模式，电机不输出。一般情况云台初始化之后的模式 */
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 {
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参数 */
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 {
@ -44,31 +61,35 @@ typedef struct {
} low_pass_cutoff_freq;
struct {
float zero_yaw; /* yaw轴零点位置，单位弧度 */
float zero_pitch; /* pitch轴零点位置，单位弧度 */
float yaw_travel_rad; /* 云台pitch轴行程弧度，-1表示无限位 */
float pitch_travel_rad; /* 云台pitch轴行程弧度，-1表示无限位 */
} mech_limit;
float yaw; /* yaw轴机械限位 */
float pit; /* pitch轴机械限位 */
} mech_zero;
struct {
float yaw; /* yaw轴机械限位行程 -1表示无限位 */
float pit; /* pitch轴机械限位行程 -1表示无限位*/
} travel;
} Gimbal_Params_t;
/* 软件限位 */
typedef struct {
float max;
float min;
} Gimbal_Limit_t;
AHRS_Gyro_t gyro;
AHRS_Eulr_t eulr;
} Gimbal_IMU_t;
/* 云台反馈数据的结构体，包含反馈控制用的反馈数据 */
typedef struct {
AHRS_Gyro_t gyro; /* IMU的陀螺仪数据 */
/* 欧拉角 */
Gimbal_IMU_t imu;
struct {
AHRS_Eulr_t imu; /* 由IMU计算的欧拉角 */
AHRS_Eulr_t encoder; /* 由编码器计算的欧拉角 */
} eulr;
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;
/*
* 运行的主结构体，所有这个文件里的函数都在操作这个结构体。
* 包含了初始化参数，中间变量，输出变量。
@ -80,21 +101,31 @@ typedef struct {
Gimbal_Params_t *param; /* 云台的参数，用Gimbal_Init设定 */
/* 模块通用 */
CMD_GimbalMode_t mode; /* 云台模式 */
Gimbal_Mode_t mode; /* 云台模式 */
/* PID计算的目标值 */
struct {
AHRS_Eulr_t eulr; /* 表示云台姿态的欧拉角 */
} setpoint;
KPID_t pid[GIMBAL_PID_NUM]; /* PID数组 */
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;
Gimbal_Limit_t limit;
struct {
Gimbal_Limit_t yaw;
Gimbal_Limit_t pit;
} limit;
LowPassFilter2p_t filter_out[GIMBAL_ACTR_NUM]; /* 输出滤波器滤波器数组 */
float out[GIMBAL_ACTR_NUM]; /* 输出数组 */
struct {
LowPassFilter2p_t yaw;
LowPassFilter2p_t pit;
} filter_out;
Gimbal_Output_t out; /* 云台输出 */
Gimbal_Feedback_t feedback; /* 反馈 */
} Gimbal_t;
@ -110,7 +141,7 @@ typedef struct {
*
* \return 函数运行结果
*/
int8_t Gimbal_Init(Gimbal_t *g, const Gimbal_Params_t *param, float limit,
int8_t Gimbal_Init(Gimbal_t *g, const Gimbal_Params_t *param,
float target_freq);
/**
@ -121,8 +152,9 @@ int8_t Gimbal_Init(Gimbal_t *g, const Gimbal_Params_t *param, float limit,
*
* \return 函数运行结果
*/
int8_t Gimbal_UpdateFeedback(Gimbal_t *gimbal, const CAN_t *can);
int8_t Gimbal_UpdateFeedback(Gimbal_t *gimbal);
int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu);
/**
* \brief 运行云台控制逻辑
*
@ -133,40 +165,16 @@ int8_t Gimbal_UpdateFeedback(Gimbal_t *gimbal, const CAN_t *can);
*
* \return 函数运行结果
*/
int8_t Gimbal_Control(Gimbal_t *g, CMD_GimbalCmd_t *g_cmd, uint32_t now);
int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd);
/**
* \brief 打包云台数据给AI
*
* \param g 包含云台数据的结构体
* \param gimbal_for_ai 云台AI数据
*
* \return 函数运行结果
*/
uint8_t Gimbal_PackAI(Gimbal_t *g, const AHRS_Eulr_t *ai);
/**
* \brief 复制云台输出值
* \brief 云台输出
*
* \param s 包含云台数据的结构体
* \param out CAN设备云台输出结构体
*/
void Gimbal_DumpOutput(Gimbal_t *g, CAN_GimbalOutput_t *out);
void Gimbal_Output(Gimbal_t *g);
/**
* \brief 清空输出值
*
* \param output 要清空的结构体
*/
void Gimbal_ResetOutput(CAN_GimbalOutput_t *output);
/**
* @brief 导出云台UI数据
*
* @param g 云台结构体
* @param ui UI结构体
*/
void Gimbal_DumpUI(const Gimbal_t *g, Referee_GimbalUI_t *ui);
#ifdef __cplusplus
}
#endif

13
User/task/atti_esti.c
View File

@ -4,13 +4,14 @@
*/
/* Includes ----------------------------------------------------------------- */
#include "cmsis_os2.h"
#include "task/user_task.h"
/* USER INCLUDE BEGIN */
#include "bsp/pwm.h"
#include "component/ahrs.h"
#include "component/pid.h"
#include "device/bmi088.h"
#include "module/gimbal.h"
/* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */
@ -26,9 +27,13 @@ AHRS_Eulr_t eulr_to_send;
KPID_t imu_temp_ctrl_pid;
Gimbal_IMU_t gimbal_to_send;
BMI088_Cali_t cali_bmi088= {
.gyro_offset = {0.0f,0.0f,0.0f},
};
/* USER STRUCT END */
/* Private function --------------------------------------------------------- */
@ -68,6 +73,12 @@ void Task_atti_esti(void *argument) {
AHRS_GetEulr(&eulr_to_send, &gimbal_ahrs);
osKernelUnlock();
gimbal_to_send.eulr = eulr_to_send;
gimbal_to_send.gyro = bmi088.gyro;
osMessageQueueReset(task_runtime.msgq.gimbal.imu);
osMessageQueuePut(task_runtime.msgq.gimbal.imu, &gimbal_to_send, 0, 0);
/* USER CODE END */
}

20
User/task/ctrl_gimbal.c
View File

@ -4,9 +4,12 @@
*/
/* Includes ----------------------------------------------------------------- */
#include "cmsis_os2.h"
#include "component/ahrs.h"
#include "task/user_task.h"
/* USER INCLUDE BEGIN */
#include "module/gimbal.h"
#include "module/config.h"
/* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */
@ -14,7 +17,9 @@
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN */
Gimbal_t gimbal;
Gimbal_IMU_t gimbal_imu;
Gimbal_CMD_t gimbal_cmd;
/* USER STRUCT END */
/* Private function --------------------------------------------------------- */
@ -30,13 +35,22 @@ void Task_ctrl_gimbal(void *argument) {
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
/* USER CODE INIT BEGIN */
Gimbal_Init(&gimbal, &Config_GetRobotParam()->gimbal_param, CTRL_GIMBAL_FREQ);
/* USER CODE INIT END */
while (1) {
tick += delay_tick; /* 计算下一个唤醒时刻 */
/* USER CODE BEGIN */
if (osMessageQueueGet(task_runtime.msgq.gimbal.imu, &gimbal_imu, NULL, 0) == osOK) {
Gimbal_UpdateIMU(&gimbal, &gimbal_imu);
}
osMessageQueueGet(task_runtime.msgq.gimbal.cmd, &gimbal_cmd, NULL, 0);
Gimbal_UpdateFeedback(&gimbal);
Gimbal_Control(&gimbal, &gimbal_cmd);
Gimbal_Output(&gimbal);
/* USER CODE END */
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
}

6
User/task/init.c
View File

@ -8,6 +8,8 @@
/* USER INCLUDE BEGIN */
#include "module/shoot.h"
#include "module/gimbal.h"
#include "component/ahrs.h"
/* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */
@ -40,7 +42,9 @@ void Task_Init(void *argument) {
// 创建消息队列
/* USER MESSAGE BEGIN */
task_runtime.msgq.user_msg= osMessageQueueNew(2u, 10, NULL);
task_runtime.msgq.shoot.shoot_cmd=osMessageQueueNew(2u, sizeof(Shoot_CMD_t), NULL);
task_runtime.msgq.shoot.shoot_cmd = osMessageQueueNew(2u, sizeof(Shoot_CMD_t), NULL);
task_runtime.msgq.gimbal.imu= osMessageQueueNew(2u, sizeof(Gimbal_IMU_t), NULL);
task_runtime.msgq.gimbal.cmd= osMessageQueueNew(2u, sizeof(Gimbal_CMD_t), NULL);
/* USER MESSAGE END */
osKernelUnlock(); // 解锁内核

28
User/task/rc.c
View File

@ -11,6 +11,7 @@
#include "device/rc_can.h"
#include "module/config.h"
#include "module/shoot.h"
#include "module/gimbal.h"
#include <stdbool.h>
// #include
/* USER INCLUDE END */
@ -23,6 +24,7 @@
DR16_t dr16;
RC_CAN_t rc_can;
Shoot_CMD_t for_shoot;
Gimbal_CMD_t cmd_for_gimbal;
/* USER STRUCT END */
/* Private function --------------------------------------------------------- */
@ -75,6 +77,32 @@ void Task_rc(void *argument) {
RC_CAN_SendData(&rc_can, RC_CAN_DATA_JOYSTICK);
RC_CAN_SendData(&rc_can, RC_CAN_DATA_SWITCH);
switch (dr16.data.sw_l) {
case DR16_SW_UP:
cmd_for_gimbal.mode = GIMBAL_MODE_RELAX;
cmd_for_gimbal.delta_yaw = 0.0f;
cmd_for_gimbal.delta_pit = 0.0f;
break;
case DR16_SW_MID:
cmd_for_gimbal.mode = GIMBAL_MODE_ABSOLUTE;
cmd_for_gimbal.delta_yaw = -dr16.data.ch_r_x*5.0f;
cmd_for_gimbal.delta_pit = dr16.data.ch_r_y*5.0f;
break;
case DR16_SW_DOWN:
cmd_for_gimbal.mode = GIMBAL_MODE_RELATIVE;
cmd_for_gimbal.delta_yaw = -dr16.data.ch_r_x* 5.0f;
cmd_for_gimbal.delta_pit = dr16.data.ch_r_y*5.0f;
break;
default:
cmd_for_gimbal.mode = GIMBAL_MODE_RELAX;
cmd_for_gimbal.delta_yaw = 0.0f;
cmd_for_gimbal.delta_pit = 0.0f;
break;
}
osMessageQueueReset(task_runtime.msgq.gimbal.cmd);
osMessageQueuePut(task_runtime.msgq.gimbal.cmd, &cmd_for_gimbal, 0, 0);
osMessageQueueReset(task_runtime.msgq.shoot.shoot_cmd);
osMessageQueuePut(task_runtime.msgq.shoot.shoot_cmd, &for_shoot, 0, 0);
/* USER CODE END */

4
User/task/user_task.h
View File

@ -49,6 +49,10 @@ typedef struct {
struct {
osMessageQueueId_t shoot_cmd; /* 发射命令队列 */
}shoot;
struct {
osMessageQueueId_t imu;
osMessageQueueId_t cmd;
}gimbal;
} msgq;
/* USER MESSAGE END */