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

infantry

Browse Source
This commit is contained in:
RB
2025-03-11 21:32:41 +08:00
parent 1480a30aa1
commit 740dc38e96
327 changed files with 252743 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

55
User/module/cap.c Normal file
View File

@@ -0,0 +1,55 @@
/*
* 电容模组
*/
/* Includes ----------------------------------------------------------------- */
#include "cap.h"
#include "component\capacity.h"
#include "component\limiter.h"
#include "device\can.h"
#include "device\referee.h"
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
#define CAP_CUTOFF_VOLT 12.0f
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* Private function -------------------------------------------------------- */
/**
* @brief 运行电容控制逻辑
*
* @param cap 电容数据结构体
* @param referee 裁判系统数据
* @param cap_out 电容输出结构体
*/
void Cap_Control(CAN_Capacitor_t *cap, const Referee_ForCap_t *referee,
CAN_CapOutput_t *cap_out) {
if (referee->ref_status != REF_STATUS_RUNNING) {
/* 当裁判系统离线时，依然使用裁判系统进程传来的数据 */
cap_out->power_limit = referee->chassis_power_limit;
} else {
/* 当裁判系统在线时，使用算法控制裁判系统输出（即电容输入） */
cap_out->power_limit =
PowerLimit_CapInput(referee->chassis_watt, referee->chassis_power_limit,
referee->chassis_pwr_buff);
}
/* 更新电容状态和百分比 */
cap->cap_status = CAN_CAP_STATUS_RUNNING;
cap->percentage = Capacity_GetCapacitorRemain(cap->cap_feedback.cap_volt,
cap->cap_feedback.input_volt,
CAP_CUTOFF_VOLT);
}
/**
* @brief 导出电容数据
*
* @param cap 电容数据
* @param ui 结构体
*/
void Cap_DumpUI(const CAN_Capacitor_t *cap, Referee_CapUI_t *ui) {
ui->percentage = cap->percentage;
ui->status = cap->cap_status;
}

40
User/module/cap.h Normal file
View File

@@ -0,0 +1,40 @@
/*
* 电容模组
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include "device\can.h"
#include "device\referee.h"
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
/* Exported functions prototypes -------------------------------------------- */
/**
* @brief 运行电容控制逻辑
*
* @param cap 电容数据结构体
* @param referee 裁判系统数据
* @param cap_out 电容输出结构体
*/
void Cap_Control(CAN_Capacitor_t *cap, const Referee_ForCap_t *referee,
CAN_CapOutput_t *cap_out);
/**
* @brief 导出电容数据
*
* @param cap 电容数据
* @param ui 结构体
*/
void Cap_DumpUI(const CAN_Capacitor_t *cap, Referee_CapUI_t *ui);
#ifdef __cplusplus
}
#endif

418
User/module/chassis.c Normal file
View File

@@ -0,0 +1,418 @@
/*
* 底盘模组
*/
/* Includes ----------------------------------------------------------------- */
#include "chassis.h"
#include <stdlib.h>
#include "bsp\mm.h"
#include "cmsis_os2.h"
#include "component\limiter.h"
#include "device\can.h"
#include "module\cap.h"
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
#define _CAP_PERCENTAGE_WORK 100 /* 底盘不再限制功率的电容电量 */
#define _CAP_PERCENTAGE_CHARGE 30 /* 电容开始工作的电容电量 */
#define CHASSIS_MAX_CAP_POWER 100 /* 电容能够提供的最大功率 */
#define CHASSIS_ROTOR_WZ_MIN 0.6f /* 小陀螺旋转位移下界 */
#define CHASSIS_ROTOR_WZ_MAX 0.8f /* 小陀螺旋转位移上界 */
#define M_7OVER72PI (M_2PI * 7.0f / 72.0f) /* 三十五度对应弧度值 */
// #define CHASSIS_ROTOR_OMEGA 0.15f /* 小陀螺转动频率 */
// 随机产生小陀螺转动频率，范围0.001-0.05
//#define CHASSIS_ROTOR_OMEGA rand() % 50 / 10000.0f + 0.001f
#define CHASSIS_ROTOR_OMEGA 0.001
/* Private macro ------------------------------------------------------------ */
/* 保证电容电量宏定义在正确范围内 */
#if ((_CAP_PERCENTAGE_WORK < 0) || (_CAP_PERCENTAGE_WORK > 100) || \
(_CAP_PERCENTAGE_CHARGE < 0) || (_CAP_PERCENTAGE_CHARGE > 100))
#error "Cap percentage should be in the range from 0 to 100."
#endif
/* 保证电容功率宏定义在正确范围内 */
#if ((CHASSIS_MAX_CAP_POWER < 60) || (CHASSIS_MAX_CAP_POWER > 200))
#error "The capacitor power should be in in the range from 60 to 200."
#endif
/* Private variables -------------------------------------------------------- */
static const float CAP_PERCENTAGE_WORK = (float)_CAP_PERCENTAGE_WORK / 100.0f;
static const float CAP_PERCENTAGE_CHARGE =
(float)_CAP_PERCENTAGE_CHARGE / 100.0f;
/* Private function -------------------------------------------------------- */
/**
* \brief 设置底盘模式
*
* \param c 包含底盘数据的结构体
* \param mode 要设置的模式
*
* \return 函数运行结果
*/
static int8_t Chassis_SetMode(Chassis_t *c, CMD_ChassisMode_t mode,
uint32_t now) {
if (c == NULL) return CHASSIS_ERR_NULL; /* 主结构体不能为空 */
if (mode == c->mode) return CHASSIS_OK; /* 模式未改变直接返回 */
if (mode == CHASSIS_MODE_ROTOR && c->mode != CHASSIS_MODE_ROTOR) {
srand(now);
c->wz_multi = (rand() % 2) ? -1 : 1;
}
/* 切换模式后重置PID和滤波器 */
for (uint8_t i = 0; i < c->num_wheel; i++) {
PID_Reset(c->pid.motor + i);
LowPassFilter2p_Reset(c->filter.in + i, 0.0f);
LowPassFilter2p_Reset(c->filter.out + i, 0.0f);
}
c->mode = mode;
return CHASSIS_OK;
}
/**
* @brief 产生小陀螺wz随机速度
*
* @param min wz产生最小速度
* @param max wz产生最大速度
* @param now ctrl_chassis的tick数
* @return float
*/
static float Chassis_CalcWz(const float min, const float max, uint32_t now) {
/* wz在min和max之间，上限0.6f */
float wz_vary = fabs(0.2f * sinf(CHASSIS_ROTOR_OMEGA * (float)now)) + min;
return wz_vary > 0.8f ? max : wz_vary;
}
/* Exported functions ------------------------------------------------------- */
/**
* \brief 初始化底盘
*
* \param c 包含底盘数据的结构体
* \param param 包含底盘参数的结构体指针
* \param target_freq 任务预期的运行频率
*
* \return 函数运行结果
*/
int8_t Chassis_Init(Chassis_t *c, const Chassis_Params_t *param,
AHRS_Eulr_t *mech_zero, float target_freq) {
if (c == NULL) return CHASSIS_ERR_NULL;
c->param = param; /* 初始化参数 */
c->mode = CHASSIS_MODE_RELAX; /* 设置上电后底盘默认模式 */
c->mech_zero = mech_zero; /* 设置底盘机械零点 */
/* 如果电机反装重新计算机械零点 */
if (param->reverse.yaw) CircleReverse(&(c->mech_zero->yaw));
/* 根据参数（param）中的底盘型号初始化Mixer */
Mixer_Mode_t mixer_mode;
switch (c->param->type) {
case CHASSIS_TYPE_MECANUM:
c->num_wheel = 4;
mixer_mode = MIXER_MECANUM;
break;
case CHASSIS_TYPE_PARLFIX4:
c->num_wheel = 4;
mixer_mode = MIXER_PARLFIX4;
break;
case CHASSIS_TYPE_PARLFIX2:
c->num_wheel = 2;
mixer_mode = MIXER_PARLFIX2;
break;
case CHASSIS_TYPE_OMNI_CROSS:
c->num_wheel = 4;
mixer_mode = MIXER_OMNICROSS;
break;
case CHASSIS_TYPE_OMNI_PLUS:
c->num_wheel = 4;
mixer_mode = MIXER_OMNIPLUS;
break;
case CHASSIS_TYPE_SINGLE:
c->num_wheel = 1;
mixer_mode = MIXER_SINGLE;
break;
case CHASSIS_TYPE_DRONE:
/* onboard sdk. */
return CHASSIS_ERR_TYPE;
}
/* 根据底盘型号动态分配控制时使用的变量 */
c->feedback.motor_rpm =
BSP_Malloc((size_t)c->num_wheel * sizeof(*c->feedback.motor_rpm));
if (c->feedback.motor_rpm == NULL) goto error; /* 变量未分配，返回错误 */
c->feedback.motor_current =
BSP_Malloc((size_t)c->num_wheel * sizeof(*c->feedback.motor_current));
if (c->feedback.motor_current == NULL) goto error;
c->setpoint.motor_rpm =
BSP_Malloc((size_t)c->num_wheel * sizeof(*c->setpoint.motor_rpm));
if (c->setpoint.motor_rpm == NULL) goto error;
c->pid.motor = BSP_Malloc((size_t)c->num_wheel * sizeof(*c->pid.motor));
if (c->pid.motor == NULL) goto error;
c->out = BSP_Malloc((size_t)c->num_wheel * sizeof(*c->out));
if (c->out == NULL) goto error;
c->filter.in = BSP_Malloc((size_t)c->num_wheel * sizeof(*c->filter.in));
if (c->filter.in == NULL) goto error;
c->filter.out = BSP_Malloc((size_t)c->num_wheel * sizeof(*c->filter.out));
if (c->filter.out == NULL) goto error;
/* 初始化轮子电机控制PID和LPF */
for (uint8_t i = 0; i < c->num_wheel; i++) {
PID_Init(c->pid.motor + i, KPID_MODE_NO_D, target_freq,
&(c->param->motor_pid_param));
LowPassFilter2p_Init(c->filter.in + i, target_freq,
c->param->low_pass_cutoff_freq.in);
LowPassFilter2p_Init(c->filter.out + i, target_freq,
c->param->low_pass_cutoff_freq.out);
}
/* 初始化跟随云台的控制PID */
PID_Init(&(c->pid.follow), KPID_MODE_NO_D, target_freq,
&(c->param->follow_pid_param));
Mixer_Init(&(c->mixer), mixer_mode); /* 初始化混合器 */
return CHASSIS_OK;
error:
/* 动态内存分配错误时，释放已经分配的内存，返回错误值 */
BSP_Free(c->feedback.motor_rpm);
BSP_Free(c->setpoint.motor_rpm);
BSP_Free(c->pid.motor);
BSP_Free(c->out);
BSP_Free(c->filter.in);
BSP_Free(c->filter.out);
return CHASSIS_ERR_NULL;
}
/**
* \brief 更新底盘的反馈信息
*
* \param c 包含底盘数据的结构体
* \param can CAN设备结构体
*
* \return 函数运行结果
*/
int8_t Chassis_UpdateFeedback(Chassis_t *c, const CAN_t *can) {
/* 底盘数据和CAN结构体不能为空 */
if (c == NULL) return CHASSIS_ERR_NULL;
if (can == NULL) return CHASSIS_ERR_NULL;
/* 如果电机反装重新计算正确的反馈值 */
if (c->param->reverse.yaw) {
c->feedback.gimbal_yaw_encoder =
-can->motor.gimbal.named.yaw.rotor_angle + M_2PI;
} else {
c->feedback.gimbal_yaw_encoder = can->motor.gimbal.named.yaw.rotor_angle;
}
/* 将CAN中的反馈数据写入到feedback中 */
for (uint8_t i = 0; i < c->num_wheel; i++) {
c->feedback.motor_rpm[i] = can->motor.chassis.as_array[i].rotor_speed;
c->feedback.motor_current[i] =
can->motor.chassis.as_array[i].torque_current;
}
return CHASSIS_OK;
}
/**
* \brief 运行底盘控制逻辑
*
* \param c 包含底盘数据的结构体
* \param c_cmd 底盘控制指令
* \param dt_sec 两次调用的时间间隔
*
* \return 函数运行结果
*/
int8_t Chassis_Control(Chassis_t *c, const CMD_ChassisCmd_t *c_cmd,
uint32_t now) {
/* 底盘数据和控制指令结构体不能为空 */
if (c == NULL) return CHASSIS_ERR_NULL;
if (c_cmd == NULL) return CHASSIS_ERR_NULL;
c->dt = (float)(now - c->lask_wakeup) / 1000.0f;
c->lask_wakeup = now;
/* 根据遥控器命令更改底盘模式 */
Chassis_SetMode(c, c_cmd->mode, now);
/* ctrl_vec -> move_vec 控制向量和真实的移动向量之间有一个换算关系 */
/* 计算vx、vy */
switch (c->mode) {
case CHASSIS_MODE_BREAK: /* 刹车模式电机停止 */
c->move_vec.vx = 0.0f;
c->move_vec.vy = 0.0f;
break;
case CHASSIS_MODE_INDENPENDENT: /* 独立模式控制向量与运动向量相等 */
c->move_vec.vx = c_cmd->ctrl_vec.vx;
c->move_vec.vy = c_cmd->ctrl_vec.vx;
break;
case CHASSIS_MODE_OPEN:
case CHASSIS_MODE_RELAX:
case CHASSIS_MODE_FOLLOW_GIMBAL: /* 按照云台方向换算运动向量 */
case CHASSIS_MODE_FOLLOW_GIMBAL_35:
case CHASSIS_MODE_ROTOR: {
float beta = c->feedback.gimbal_yaw_encoder - c->mech_zero->yaw;
float cos_beta = cosf(beta);
float sin_beta = sinf(beta);
c->move_vec.vx =
cos_beta * c_cmd->ctrl_vec.vx - sin_beta * c_cmd->ctrl_vec.vy;
c->move_vec.vy =
sin_beta * c_cmd->ctrl_vec.vx + cos_beta * c_cmd->ctrl_vec.vy;
}
}
/* 计算wz */
switch (c->mode) {
case CHASSIS_MODE_RELAX:
case CHASSIS_MODE_BREAK:
case CHASSIS_MODE_INDENPENDENT: /* 独立模式wz为0 */
c->move_vec.wz = 0.0f;
break;
case CHASSIS_MODE_OPEN:
case CHASSIS_MODE_FOLLOW_GIMBAL: /* 跟随模式通过PID控制使车头跟随云台 */
c->move_vec.wz = PID_Calc(&(c->pid.follow), c->mech_zero->yaw,
c->feedback.gimbal_yaw_encoder, 0.0f, c->dt);
break;
case CHASSIS_MODE_FOLLOW_GIMBAL_35:
c->move_vec.wz =
PID_Calc(&(c->pid.follow), c->mech_zero->yaw + M_7OVER72PI,
c->feedback.gimbal_yaw_encoder, 0.0f, c->dt);
break;
case CHASSIS_MODE_ROTOR: { /* 小陀螺模式使底盘以一定速度旋转 */
c->move_vec.wz = c->wz_multi * Chassis_CalcWz(CHASSIS_ROTOR_WZ_MIN,
CHASSIS_ROTOR_WZ_MAX, now);
}
}
/* move_vec -> motor_rpm_set. 通过运动向量计算轮子转速目标值 */
Mixer_Apply(&(c->mixer), &(c->move_vec), c->setpoint.motor_rpm, c->num_wheel,
7000.0f);
/* 根据轮子转速目标值，利用PID计算电机输出值 */
for (uint8_t i = 0; i < c->num_wheel; i++) {
/* 输入滤波. */
c->feedback.motor_rpm[i] =
LowPassFilter2p_Apply(c->filter.in + i, c->feedback.motor_rpm[i]);
/* 根据底盘模式计算输出值 */
switch (c->mode) {
case CHASSIS_MODE_BREAK:
case CHASSIS_MODE_FOLLOW_GIMBAL:
case CHASSIS_MODE_FOLLOW_GIMBAL_35:
case CHASSIS_MODE_ROTOR:
case CHASSIS_MODE_INDENPENDENT: /* 独立模式,受PID控制 */
c->out[i] = PID_Calc(c->pid.motor + i, c->setpoint.motor_rpm[i],
c->feedback.motor_rpm[i], 0.0f, c->dt);
break;
case CHASSIS_MODE_OPEN: /* 开环模式,不受PID控制 */
c->out[i] = c->setpoint.motor_rpm[i] / 9000.0f;
break;
case CHASSIS_MODE_RELAX: /* 放松模式,不输出 */
c->out[i] = 0;
break;
}
/* 输出滤波. */
c->out[i] = LowPassFilter2p_Apply(c->filter.out + i, c->out[i]);
}
return CHASSIS_OK;
}
/**
* @brief 底盘功率限制
*
* @param c 底盘数据
* @param cap 电容数据
* @param ref 裁判系统数据
* @return 函数运行结果
*/
int8_t Chassis_PowerLimit(Chassis_t *c, const CAN_Capacitor_t *cap,
const Referee_ForChassis_t *ref) {
float power_limit = 0.0f;
if (ref->ref_status != REF_STATUS_RUNNING) {
/* 裁判系统离线，将功率限制为固定值 */
power_limit = CHASSIS_POWER_MAX_WITHOUT_REF;
} else {
if (cap->cap_status == CAN_CAP_STATUS_RUNNING &&
cap->percentage > CAP_PERCENTAGE_CHARGE) {
/* 电容在线且电量足够，使用电容 */
if (cap->percentage > CAP_PERCENTAGE_WORK) {
/* 电容接近充满时不再限制功率 */
power_limit = -1.0f;
} else {
/* 按照电容能量百分比计算输出功率 */
power_limit = ref->chassis_power_limit +
(cap->percentage - CAP_PERCENTAGE_CHARGE) /
(CAP_PERCENTAGE_WORK - CAP_PERCENTAGE_CHARGE) *
(float)CHASSIS_MAX_CAP_POWER;
}
} else {
/* 电容不在工作，根据缓冲能量计算输出功率限制 */
power_limit = PowerLimit_TargetPower(ref->chassis_power_limit,
ref->chassis_pwr_buff);
}
}
/* 应用功率限制 */
PowerLimit_ChassicOutput(power_limit, c->out, c->feedback.motor_rpm,
c->num_wheel);
return CHASSIS_OK;
}
/**
* \brief 复制底盘输出值
*
* \param s 包含底盘数据的结构体
* \param out CAN设备底盘输出结构体
*/
void Chassis_DumpOutput(Chassis_t *c, CAN_ChassisOutput_t *out) {
for (uint8_t i = 0; i < c->num_wheel; i++) {
out->as_array[i] = c->out[i];
}
}
/**
* \brief 清空Chassis输出数据
*
* \param out CAN设备底盘输出结构体
*/
void Chassis_ResetOutput(CAN_ChassisOutput_t *out) {
for (uint8_t i = 0; i < 4; i++) {
out->as_array[i] = 0.0f;
}
}
/**
* @brief 导出底盘数据
*
* @param chassis 底盘数据结构体
* @param ui UI数据结构体
*/
void Chassis_DumpUI(const Chassis_t *c, Referee_ChassisUI_t *ui) {
ui->mode = c->mode;
ui->angle = c->feedback.gimbal_yaw_encoder - c->mech_zero->yaw;
}

180
User/module/chassis.h Normal file
View File

@@ -0,0 +1,180 @@
/*
* 底盘模组
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include "component\cmd.h"
#include "component\filter.h"
#include "component\mixer.h"
#include "component\pid.h"
#include "device\can.h"
#include "device\referee.h"
/* Exported constants ------------------------------------------------------- */
#define CHASSIS_OK (0) /* 运行正常 */
#define CHASSIS_ERR (-1) /* 运行时发现了其他错误 */
#define CHASSIS_ERR_NULL (-2) /* 运行时发现NULL指针 */
#define CHASSIS_ERR_MODE (-3) /* 运行时配置了错误的CMD_ChassisMode_t */
#define CHASSIS_ERR_TYPE (-4) /* 运行时配置了错误的Chassis_Type_t */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
/* 底盘类型（底盘的物理设计） */
typedef enum {
CHASSIS_TYPE_MECANUM, /* 麦克纳姆轮 */
CHASSIS_TYPE_PARLFIX4, /* 平行摆设的四个驱动轮 */
CHASSIS_TYPE_PARLFIX2, /* 平行摆设的两个驱动轮 */
CHASSIS_TYPE_OMNI_CROSS, /* 叉型摆设的四个全向轮 */
CHASSIS_TYPE_OMNI_PLUS, /* 十字型摆设的四个全向轮 */
CHASSIS_TYPE_DRONE, /* 底盘为无人机 */
CHASSIS_TYPE_SINGLE, /* 单个摩擦轮 */
} Chassis_Type_t;
/* 底盘参数的结构体，包含所有初始化用的参数，通常是const，存好几组 */
typedef struct {
Chassis_Type_t type; /* 底盘类型，底盘的机械设计和轮子选型 */
KPID_Params_t motor_pid_param; /* 轮子控制PID的参数 */
KPID_Params_t follow_pid_param; /* 跟随云台PID的参数 */
/* 低通滤波器截止频率 */
struct {
float in; /* 输入 */
float out; /* 输出 */
} low_pass_cutoff_freq;
/* 电机反装 应该和云台设置相同*/
struct {
bool yaw;
} reverse;
} Chassis_Params_t;
/*
* 运行的主结构体，所有这个文件里的函数都在操作这个结构体
* 包含了初始化参数，中间变量，输出变量
*/
typedef struct {
uint32_t lask_wakeup;
float dt;
const Chassis_Params_t *param; /* 底盘的参数，用Chassis_Init设定 */
AHRS_Eulr_t *mech_zero;
/* 模块通用 */
CMD_ChassisMode_t mode; /* 底盘模式 */
/* 底盘设计 */
int8_t num_wheel; /* 底盘轮子数量 */
Mixer_t mixer; /* 混合器，移动向量->电机目标值 */
MoveVector_t move_vec; /* 底盘实际的运动向量 */
/* 反馈信息 */
struct {
float gimbal_yaw_encoder; /* 云台Yaw轴编码器角度 */
float *motor_rpm; /* 电机转速的动态数组，单位：RPM */
float *motor_current; /* 转矩电流 单位：A */
} feedback;
float wz_multi; /* 小陀螺模式旋转方向 */
/* PID计算的目标值 */
struct {
float *motor_rpm; /* 电机转速的动态数组，单位：RPM */
} setpoint;
/* 反馈控制用的PID */
struct {
KPID_t *motor; /* 控制轮子电机用的PID的动态数组 */
KPID_t follow; /* 跟随云台用的PID */
} pid;
/* 滤波器 */
struct {
LowPassFilter2p_t *in; /* 反馈值滤波器 */
LowPassFilter2p_t *out; /* 输出值滤波器 */
} filter;
float *out; /* 电机最终的输出值的动态数组 */
} Chassis_t;
/* Exported functions prototypes -------------------------------------------- */
/**
* \brief 初始化底盘
*
* \param c 包含底盘数据的结构体
* \param param 包含底盘参数的结构体指针
* \param target_freq 任务预期的运行频率
*
* \return 函数运行结果
*/
int8_t Chassis_Init(Chassis_t *c, const Chassis_Params_t *param,
AHRS_Eulr_t *mech_zero, float target_freq);
/**
* \brief 更新底盘的反馈信息
*
* \param c 包含底盘数据的结构体
* \param can CAN设备结构体
*
* \return 函数运行结果
*/
int8_t Chassis_UpdateFeedback(Chassis_t *c, const CAN_t *can);
/**
* \brief 运行底盘控制逻辑
*
* \param c 包含底盘数据的结构体
* \param c_cmd 底盘控制指令
* \param dt_sec 两次调用的时间间隔
*
* \return 函数运行结果
*/
int8_t Chassis_Control(Chassis_t *c, const CMD_ChassisCmd_t *c_cmd,
uint32_t now);
/**
* @brief 底盘功率限制
*
* @param c 底盘数据
* @param cap 电容数据
* @param ref 裁判系统数据
* @return 函数运行结果
*/
int8_t Chassis_PowerLimit(Chassis_t *c, const CAN_Capacitor_t *cap,
const Referee_ForChassis_t *ref);
/**
* \brief 复制底盘输出值
*
* \param s 包含底盘数据的结构体
* \param out CAN设备底盘输出结构体
*/
void Chassis_DumpOutput(Chassis_t *c, CAN_ChassisOutput_t *out);
/**
* \brief 清空Chassis输出数据
*
* \param out CAN设备底盘输出结构体
*/
void Chassis_ResetOutput(CAN_ChassisOutput_t *out);
/**
* @brief 导出底盘数据
*
* @param chassis 底盘数据结构体
* @param ui UI数据结构体
*/
void Chassis_DumpUI(const Chassis_t *c, Referee_ChassisUI_t *ui);
#ifdef __cplusplus
}
#endif

496
User/module/config.c Normal file
View File

@@ -0,0 +1,496 @@
/*
* 配置相关
*/
#include "config.h"
#include <stdint.h>
#include <string.h>
#include "bsp/flash.h"
#define CONFIG_BASE_ADDRESS (ADDR_FLASH_SECTOR_11)
/* clang-format off */
#ifdef DEBUG
Config_RobotParam_t param_default = {
#else
static const Config_RobotParam_t param_default = {
#endif
.model = ROBOT_MODEL_INFANTRY,
.chassis = { /* 底盘模块参数 */
.type = CHASSIS_TYPE_MECANUM,
.motor_pid_param = {
.k = 0.001f,
.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,
},
.follow_pid_param = {
.k = 0.5f,
.p = 1.0f,
.i = 0.0f,
.d = 0.0f,
.i_limit = 1.0f,
.out_limit = 1.0f,
.d_cutoff_freq = -1.0f,
.range = M_2PI,
},
.low_pass_cutoff_freq = {
.in = -1.0f,
.out = -1.0f,
},
.reverse = {
.yaw = false,
},
}, /* chassis */
.gimbal = { /* 云台模块参数 */
.pid = {
{
// /* GIMBAL_PID_YAW_OMEGA_IDX */
// .k = 0.25f,
// .p = 1.0f,
// .i = 1.0f,
// .d = 0.0f,
// .i_limit = 1.0f,
// .out_limit = 1.0f,
// .d_cutoff_freq = -1.0f,
// .range = -1.0f,
// }, {
// /* GIMBAL_PID_YAW_ANGLE_IDX */
// .k = 12.0f,
// .p = 1.0f,
// .i = 0.0f,
// .d = 0.05f,
// .i_limit = 0.0f,
// .out_limit = 10.0f,
// .d_cutoff_freq = -1.0f,
// .range = M_2PI,
/* GIMBAL_PID_YAW_OMEGA_IDX */
.k = 0.24f,
.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,
}, {
/* GIMBAL_PID_YAW_ANGLE_IDX */
.k = 10.0f,
.p = 1.0f,
.i = 0.0f,
.d = 0.05f,
.i_limit = 0.0f,
.out_limit = 10.0f,
.d_cutoff_freq = -1.0f,
.range = M_2PI,
}, {
// /* GIMBAL_PID_PIT_OMEGA_IDX */
// .k = 0.35f,
// .p = 1.0f,
// .i = 0.f,
// .d = 0.003f,
// .i_limit = 1.0f,
// .out_limit = 1.0f,
// .d_cutoff_freq = -1.0f,
// .range = -1.0f,
// }, {
// /* GIMBAL_PID_PIT_ANGLE_IDX */
// .k = 15.0f,
// .p = 1.0f,
// .i = 0.0f,
// .d = 0.0f,
// .i_limit = 0.0f,
// .out_limit = 10.0f,
// .d_cutoff_freq = -1.0f,
// .range = M_2PI,
/* GIMBAL_PID_PIT_OMEGA_IDX */
.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,
}, {
/* GIMBAL_PID_PIT_ANGLE_IDX */
.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,
},
}, /* pid */
.pitch_travel_rad = 1.05f,
.low_pass_cutoff_freq = {
.out = -1.0f,
.gyro = 1000.0f,
},
.reverse = {
.yaw = false,
.pit = true,
},
}, /* gimbal */
.shoot = { /* 射击模块参数 */
.fric_pid_param = {
.k = 0.001f,
.p = 1.0f,
.i = 0.2f,
.d = 0.01f,
.i_limit = 0.5f,
.out_limit = 0.5f,
.d_cutoff_freq = -1.0f,
},
.trig_pid_param = {
.k = 12.0f,
.p = 1.0f,
.i = 0.0f,
.d = 0.0450000018f,
.i_limit = 1.0f,
.out_limit = 1.0f,
.d_cutoff_freq = -1.0f,
.range = M_2PI,
},
.low_pass_cutoff_freq = {
.in = {
.fric = -1.0f,
.trig = -1.0f,
},
.out = {
.fric = -1.0f,
.trig = -1.0f,
},
},
.num_trig_tooth = 10.0f,
.trig_gear_ratio = 36.0f,
.fric_radius = 0.03f,
.cover_open_duty = 0.10f,
.cover_close_duty = 0.050f,
.model = SHOOT_MODEL_17MM,
.bullet_speed = 30.f,
.min_shoot_delay = (uint32_t)(1000.0f / 10.0f),
}, /* shoot */
.can = {
.chassis = BSP_CAN_1,
.gimbal = BSP_CAN_2,
.shoot = BSP_CAN_2,
.cap = BSP_CAN_1,
}, /* can */
}; /* param_default */
static const Config_RobotParam_t param_hero = {
.model = ROBOT_MODEL_HERO,
.chassis = { /* 底盘模块参数 */
.type = CHASSIS_TYPE_MECANUM,
.motor_pid_param = {
.k = 0.0011f,
.p = 1.0f,
.i = 0.001f,
.d = 0.0f,
.i_limit = 1.0f,
.out_limit = 1.0f,
.d_cutoff_freq = -1.0f,
.range = -1.0f,
},
.follow_pid_param = {
.k = 0.5f,
.p = 1.0f,
.i = 0.0f,
.d = 0.0f,
.i_limit = 1.0f,
.out_limit = 1.0f,
.d_cutoff_freq = -1.0f,
.range = M_2PI,
},
.low_pass_cutoff_freq = {
.in = -1.0f,
.out = -1.0f,
},
.reverse = {
.yaw = true,
},
}, /* chassis */
.gimbal = { /* 云台模块参数 */
.pid = {
{
/* GIMBAL_PID_YAW_OMEGA_IDX */
.k = 0.45f,
.p = 1.0f,
.i = 6.0f,
.d = 0.0008f,
.i_limit = 1.0f,
.out_limit = 1.0f,
.d_cutoff_freq = -1.0f,
.range = -1.0f,
}, {
/* GIMBAL_PID_YAW_ANGLE_IDX */
.k = 20.0f,
.p = 1.0f,
.i = 0.0f,
.d = 0.0f,
.i_limit = 0.0f,
.out_limit = 10.0f,
.d_cutoff_freq = -1.0f,
.range = M_2PI,
}, {
/* GIMBAL_PID_PIT_OMEGA_IDX */
.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,
}, {
/* GIMBAL_PID_PIT_ANGLE_IDX */
.k = 12.0f,
.p = 1.0f,
.i = 0.0f,
.d = 0.0f,
.i_limit = 0.0f,
.out_limit = 10.0f,
.d_cutoff_freq = -1.0f,
.range = M_2PI,
},
}, /* pid */
.pitch_travel_rad = 1.07685447f,
.low_pass_cutoff_freq = {
.out = -1.0f,
.gyro = 1000.0f,
},
.reverse = {
.yaw = true,
.pit = true,
},
}, /* gimbal */
.shoot = { /* 射击模块参数 */
.fric_pid_param = {
.k = 0.001f,
.p = 1.0f,
.i = 0.2f,
.d = 0.01f,
.i_limit = 0.5f,
.out_limit = 0.5f,
.d_cutoff_freq = -1.0f,
},
.trig_pid_param = {
.k = 10.0f,
.p = 1.0f,
.i = 0.0f,
.d = 0.032f,
.i_limit = 1.0f,
.out_limit = 1.0f,
.d_cutoff_freq = -1.0f,
.range = M_2PI,
},
.low_pass_cutoff_freq = {
.in = {
.fric = -1.0f,
.trig = -1.0f,
},
.out = {
.fric = -1.0f,
.trig = -1.0f,
},
},
.num_trig_tooth = 6.0f,
.trig_gear_ratio = 3591.0f / 187.0f,
.fric_radius = 0.03f,
.cover_open_duty = 0.125f,
.cover_close_duty = 0.075f,
.model = SHOOT_MODEL_42MM,
.bullet_speed = 16.0f,
.min_shoot_delay = (uint32_t)(1000.0f / 10.0f),
}, /* shoot */
.can = {
.chassis = BSP_CAN_1,
.gimbal = BSP_CAN_2,
.shoot = BSP_CAN_2,
.cap = BSP_CAN_1,
}, /* can */
}; /* param_hero */
/* static const Config_RobotParam_t param_xxx; */
static const Config_PilotCfg_t cfg_qs = {
.param = {
.sens_mouse = 0.06f,
.sens_rc = 6.0f,
.map = {
.key_map[CMD_BEHAVIOR_FORE] = {CMD_ACTIVE_PRESSED, CMD_KEY_W},
.key_map[CMD_BEHAVIOR_BACK] = {CMD_ACTIVE_PRESSED, CMD_KEY_S},
.key_map[CMD_BEHAVIOR_LEFT] = {CMD_ACTIVE_PRESSED, CMD_KEY_A},
.key_map[CMD_BEHAVIOR_RIGHT] = {CMD_ACTIVE_PRESSED, CMD_KEY_D},
.key_map[CMD_BEHAVIOR_ACCELERATE] = {CMD_ACTIVE_PRESSED, CMD_KEY_SHIFT},
.key_map[CMD_BEHAVIOR_DECELEBRATE] = {CMD_ACTIVE_PRESSED, CMD_KEY_CTRL},
.key_map[CMD_BEHAVIOR_FIRE] = {CMD_ACTIVE_PRESSED, CMD_L_CLICK},
.key_map[CMD_BEHAVIOR_FIRE_MODE] = {CMD_ACTIVE_PRESSING, CMD_R_CLICK},
.key_map[CMD_BEHAVIOR_FOLLOWGIMBAL35] = {CMD_ACTIVE_PRESSING, CMD_KEY_E},
.key_map[CMD_BEHAVIOR_OPENCOVER] = {CMD_ACTIVE_PRESSING, CMD_KEY_F},
.key_map[CMD_BEHAVIOR_REVTRIG] = {CMD_ACTIVE_PRESSING, CMD_KEY_R},
.key_map[CMD_BEHAVIOR_ROTOR] = {CMD_ACTIVE_PRESSING, CMD_KEY_G},
},
.move = {
.move_sense = 1.6f,
.move_fast_sense = 2.4f,
.move_slow_sense = 0.8f,
},
.screen = {
.height = 1080,
.width = 1920,
},
},
};
static const Config_PilotCfg_t cfg_zyma = {
.param = {
.sens_mouse = 0.06f,
.sens_rc = 6.0f,
.map = {
.key_map[CMD_BEHAVIOR_FORE] = {CMD_ACTIVE_PRESSED, CMD_KEY_W},
.key_map[CMD_BEHAVIOR_BACK] = {CMD_ACTIVE_PRESSED, CMD_KEY_S},
.key_map[CMD_BEHAVIOR_LEFT] = {CMD_ACTIVE_PRESSED, CMD_KEY_A},
.key_map[CMD_BEHAVIOR_RIGHT] = {CMD_ACTIVE_PRESSED, CMD_KEY_D},
.key_map[CMD_BEHAVIOR_ACCELERATE] = {CMD_ACTIVE_PRESSED, CMD_KEY_SHIFT},
.key_map[CMD_BEHAVIOR_DECELEBRATE] = {CMD_ACTIVE_PRESSED, CMD_KEY_CTRL},
.key_map[CMD_BEHAVIOR_FIRE] = {CMD_ACTIVE_PRESSED, CMD_L_CLICK},
.key_map[CMD_BEHAVIOR_FIRE_MODE] = {CMD_ACTIVE_PRESSING, CMD_R_CLICK},
.key_map[CMD_BEHAVIOR_FOLLOWGIMBAL35] = {CMD_ACTIVE_PRESSING, CMD_KEY_E},
.key_map[CMD_BEHAVIOR_OPENCOVER] = {CMD_ACTIVE_PRESSING, CMD_KEY_F},
.key_map[CMD_BEHAVIOR_REVTRIG] = {CMD_ACTIVE_PRESSING, CMD_KEY_R},
.key_map[CMD_BEHAVIOR_ROTOR] = {CMD_ACTIVE_PRESSING, CMD_KEY_G},
},
.move = {
.move_sense = 1.6f,
.move_fast_sense = 2.4f,
.move_slow_sense = 1.6f,
},
},
};
/* static const Config_PilotCfg_t cfg_xx; */
/* clang-format on */
static const Config_RobotParamMap_t robot_param_map[] = {
{"default", &param_default},
{"infantry", &param_default},
{"hero", &param_hero},
// {"engineer", &param_engineer},
// {"drone", &param_drone},
// {"sentry", &param_sentry},
/* {"xxx", &param_xxx}, */
{NULL, NULL},
};
static const Config_PilotCfgMap_t pilot_cfg_map[] = {
{"qs", &cfg_qs},
{"zyma", &cfg_zyma},
/* {"xx", &cfg_xx}, */
{NULL, NULL},
};
/**
* \brief 从Flash读取配置信息
*
* \param cfg 配置信息
*/
void Config_Get(Config_t *cfg) {
BSP_Flash_ReadBytes(CONFIG_BASE_ADDRESS, (uint8_t *)cfg, sizeof(*cfg));
cfg->pilot_cfg = Config_GetPilotCfg(cfg->pilot_cfg_name);
cfg->robot_param = Config_GetRobotParam(cfg->robot_param_name);
/* 防止第一次烧写后访问NULL指针 */
if (cfg->robot_param == NULL) cfg->robot_param = &param_default;
if (cfg->pilot_cfg == NULL) cfg->pilot_cfg = &cfg_qs;
/* 防止擦除后全为1 */
if ((uint32_t)(cfg->robot_param) == UINT32_MAX)
cfg->robot_param = &param_default;
if ((uint32_t)(cfg->pilot_cfg) == UINT32_MAX) cfg->pilot_cfg = &cfg_qs;
}
/**
* \brief 将配置信息写入Flash
*
* \param cfg 配置信息
*/
void Config_Set(Config_t *cfg) {
osKernelLock();
BSP_Flash_EraseSector(11);
BSP_Flash_WriteBytes(CONFIG_BASE_ADDRESS, (uint8_t *)cfg, sizeof(*cfg));
osKernelUnlock();
}
/**
* @brief 通过机器人参数名称获取机器人参数的指针
*
* @param robot_param_name 机器人参数名称
* @return const Config_RobotParam_t* 机器人参数的指针
*/
const Config_RobotParam_t *Config_GetRobotParam(const char *robot_param_name) {
if (robot_param_name == NULL) return NULL;
for (size_t j = 0; robot_param_map[j].name != NULL; j++) {
if (strcmp(robot_param_map[j].name, robot_param_name) == 0) {
return robot_param_map[j].param;
}
}
return NULL; /* No match. */
}
/**
* @brief 通过操作手配置名称获取操作手配置的指针
*
* @param pilot_cfg_name 操作手配置名称
* @return const Config_PilotCfg_t* 操作手配置的指针
*/
const Config_PilotCfg_t *Config_GetPilotCfg(const char *pilot_cfg_name) {
if (pilot_cfg_name == NULL) return NULL;
for (size_t j = 0; pilot_cfg_map[j].name != NULL; j++) {
if (strcmp(pilot_cfg_map[j].name, pilot_cfg_name) == 0) {
return pilot_cfg_map[j].param;
}
}
return NULL; /* No match. */
}
const Config_PilotCfgMap_t *Config_GetPilotNameMap(void) {
return pilot_cfg_map;
}
const Config_RobotParamMap_t *Config_GetRobotNameMap(void) {
return robot_param_map;
}

99
User/module/config.h Normal file
View File

@@ -0,0 +1,99 @@
/*
* 配置相关
* 读写在FALSH上的信息
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#include "component\cmd.h"
#include "device\bmi088.h"
#include "device\can.h"
#include "device\ist8310.h"
#include "module\chassis.h"
#include "module\gimbal.h"
#include "module\shoot.h"
/* 机器人参数，保存后不会变化 */
typedef struct {
CMD_RobotModel_t model; /* 型号 */
Chassis_Params_t chassis; /* 底盘 */
Gimbal_Params_t gimbal; /* 云台 */
Shoot_Params_t shoot; /* 射击 */
CAN_Params_t can; /* 电机CAN配置 */
} Config_RobotParam_t;
/* 操作员配置 */
typedef struct {
CMD_Params_t param; /* 参数 */
} Config_PilotCfg_t;
/* 机器人配置，保存在Flash上的信息，根据机器人变化 */
typedef struct {
char robot_param_name[20];
char pilot_cfg_name[20];
const Config_RobotParam_t *robot_param;
const Config_PilotCfg_t *pilot_cfg;
struct {
IST8310_Cali_t ist8310;
BMI088_Cali_t bmi088;
} cali; /* 校准 */
AHRS_Eulr_t mech_zero; /* 机械零点 */
float gimbal_limit; /* 云台pitch轴软件限位最高点 */
} Config_t;
/* 机器人参数和对应字符串的映射 */
typedef struct {
const char *const name;
const Config_RobotParam_t *param;
} Config_RobotParamMap_t;
/* 操作手配置和对应字符串的映射 */
typedef struct {
const char *const name;
const Config_PilotCfg_t *param;
} Config_PilotCfgMap_t;
/**
* \brief 从Flash读取配置信息
*
* \param cfg 配置信息
*/
void Config_Get(Config_t *cfg);
/**
* \brief 将配置信息写入Flash
*
* \param cfg 配置信息
*/
void Config_Set(Config_t *cfg);
/**
* @brief 通过机器人参数名称获取机器人参数的指针
*
* @param robot_param_name 机器人参数名称
* @return const Config_RobotParam_t* 机器人参数的指针
*/
const Config_RobotParam_t *Config_GetRobotParam(const char *robot_param_name);
/**
* @brief 通过操作手配置名称获取操作手配置的指针
*
* @param pilot_cfg_name 操作手配置名称
* @return const Config_PilotCfg_t* 操作手配置的指针
*/
const Config_PilotCfg_t *Config_GetPilotCfg(const char *pilot_cfg_name);
const Config_PilotCfgMap_t *Config_GetPilotNameMap(void);
const Config_RobotParamMap_t *Config_GetRobotNameMap(void);
#ifdef __cplusplus
}
#endif

235
User/module/gimbal.c Normal file
View File

@@ -0,0 +1,235 @@
/*
* 云台模组
*/
/* Includes ----------------------------------------------------------------- */
#include "gimbal.h"
#include "bsp/mm.h"
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* Private function -------------------------------------------------------- */
/**
* \brief 设置云台模式
*
* \param c 包含云台数据的结构体
* \param mode 要设置的模式
*
* \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;
/* 切换模式后重置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);
}
AHRS_ResetEulr(&(g->setpoint.eulr)); /* 切换模式后重置设定值 */
if (g->mode == GIMBAL_MODE_RELAX) {
if (mode == GIMBAL_MODE_ABSOLUTE) {
g->setpoint.eulr.yaw = g->feedback.eulr.imu.yaw;
} else if (mode == GIMBAL_MODE_RELATIVE) {
g->setpoint.eulr.yaw = g->feedback.eulr.encoder.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 limit_max,
float target_freq) {
if (g == NULL) return -1;
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[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]));
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);
}
return 0;
}
/**
* \brief 通过CAN设备更新云台反馈信息
*
* \param gimbal 云台
* \param can CAN设备
*
* \return 函数运行结果
*/
int8_t Gimbal_UpdateFeedback(Gimbal_t *gimbal, const CAN_t *can) {
if (gimbal == NULL) return -1;
if (can == 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;
if (gimbal->param->reverse.yaw)
CircleReverse(&(gimbal->feedback.eulr.encoder.yaw));
if (gimbal->param->reverse.pit)
CircleReverse(&(gimbal->feedback.eulr.encoder.pit));
return 0;
}
/**
* \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;
g->dt = (float)(now - g->lask_wakeup) / 1000.0f;
g->lask_wakeup = now;
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.imu.pit),
M_2PI);
const float delta_min =
CircleError(g->limit.min,
(g->feedback.eulr.encoder.pit + g->setpoint.eulr.pit -
g->feedback.eulr.imu.pit),
M_2PI);
Clip(&(g_cmd->delta_eulr.pit), delta_min, delta_max);
g->setpoint.eulr.pit += g_cmd->delta_eulr.pit;
/* 重置输入指令，防止重复处理 */
AHRS_ResetEulr(&(g_cmd->delta_eulr));
/* 控制相关逻辑 */
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_ABSOLUTE:
yaw_omega_set_point =
PID_Calc(&(g->pid[GIMBAL_PID_YAW_ANGLE_IDX]), g->setpoint.eulr.yaw,
g->feedback.eulr.imu.yaw, 0.0f, g->dt);
g->out[GIMBAL_ACTR_YAW_IDX] =
PID_Calc(&(g->pid[GIMBAL_PID_YAW_OMEGA_IDX]), yaw_omega_set_point,
g->feedback.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.imu.pit, 0.0f, g->dt);
g->out[GIMBAL_ACTR_PIT_IDX] =
PID_Calc(&(g->pid[GIMBAL_PID_PIT_OMEGA_IDX]), pit_omega_set_point,
g->feedback.gyro.x, 0.f, g->dt);
break;
case GIMBAL_MODE_RELATIVE:
for (uint8_t i = 0; i < GIMBAL_ACTR_NUM; i++) g->out[i] = 0.0f;
break;
}
/* 输出滤波 */
for (uint8_t i = 0; i < GIMBAL_ACTR_NUM; i++)
g->out[i] = LowPassFilter2p_Apply(g->filter_out + i, g->out[i]);
/* 处理电机反装 */
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 复制云台输出值
*
* \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;
}
}
/**
* @brief 导出云台UI数据
*
* @param g 云台结构体
* @param ui UI结构体
*/
void Gimbal_DumpUI(const Gimbal_t *g, Referee_GimbalUI_t *ui) {
ui->mode = g->mode;
}

172
User/module/gimbal.h Normal file
View File

@@ -0,0 +1,172 @@
/*
* 云台模组
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include "component\ahrs.h"
#include "component\cmd.h"
#include "component\filter.h"
#include "component\pid.h"
#include "device\bmi088.h"
#include "device\can.h"
#include "device\referee.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 ----------------------------------------------------------- */
/* 用enum组合所有PID，GIMBAL_PID_NUM长度的数组都可以用这个枚举访问 */
enum Gimbal_PID_e {
GIMBAL_PID_YAW_OMEGA_IDX = 0, /* Yaw轴控制的角速度环PID的索引值 */
GIMBAL_PID_YAW_ANGLE_IDX, /* Yaw轴控制的角度环PID的索引值 */
GIMBAL_PID_PIT_OMEGA_IDX, /* Pitch轴控制的角速度环PID的索引值 */
GIMBAL_PID_PIT_ANGLE_IDX, /* Pitch轴控制的角度环PID的索引值 */
GIMBAL_PID_NUM, /* 总共的PID数量 */
};
/* 用enum组合所有输出，GIMBAL_ACTR_NUM长度的数组都可以用这个枚举访问 */
enum Gimbal_Acuator_e {
GIMBAL_ACTR_YAW_IDX = 0, /* Yaw轴控制相关的索引值 */
GIMBAL_ACTR_PIT_IDX, /* Pitch轴控制相关的索引值 */
GIMBAL_ACTR_NUM, /* 总共的动作器数量 */
};
/* 云台参数的结构体，包含所有初始化用的参数，通常是const，存好几组。*/
typedef struct {
const KPID_Params_t pid[GIMBAL_PID_NUM]; /* 云台电机控制PID的参数 */
/* 低通滤波器截止频率 */
struct {
float out; /* 电机输出 */
float gyro; /* 陀螺仪数据 */
} low_pass_cutoff_freq;
float pitch_travel_rad; /* 云台pitch轴行程弧度 */
/* 设置默认运动方向 */
struct {
bool yaw;
bool pit;
} reverse;
} Gimbal_Params_t;
/* 软件限位 */
typedef struct {
float max;
float min;
} Gimbal_Limit_t;
/* 云台反馈数据的结构体，包含反馈控制用的反馈数据 */
typedef struct {
AHRS_Gyro_t gyro; /* IMU的陀螺仪数据 */
/* 欧拉角 */
struct {
AHRS_Eulr_t imu; /* 由IMU计算的欧拉角 */
AHRS_Eulr_t encoder; /* 由编码器计算的欧拉角 */
} eulr;
} Gimbal_Feedback_t;
/*
* 运行的主结构体，所有这个文件里的函数都在操作这个结构体。
* 包含了初始化参数，中间变量，输出变量。
*/
typedef struct {
uint32_t lask_wakeup;
float dt;
const Gimbal_Params_t *param; /* 云台的参数，用Gimbal_Init设定 */
/* 模块通用 */
CMD_GimbalMode_t mode; /* 云台模式 */
/* PID计算的目标值 */
struct {
AHRS_Eulr_t eulr; /* 表示云台姿态的欧拉角 */
} setpoint;
KPID_t pid[GIMBAL_PID_NUM]; /* PID数组 */
Gimbal_Limit_t limit;
LowPassFilter2p_t filter_out[GIMBAL_ACTR_NUM]; /* 输出滤波器滤波器数组 */
float out[GIMBAL_ACTR_NUM]; /* 输出数组 */
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 limit,
float target_freq);
/**
* \brief 通过CAN设备更新云台反馈信息
*
* \param gimbal 云台
* \param can CAN设备
*
* \return 函数运行结果
*/
int8_t Gimbal_UpdateFeedback(Gimbal_t *gimbal, const CAN_t *can);
/**
* \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);
/**
* \brief 复制云台输出值
*
* \param s 包含云台数据的结构体
* \param out CAN设备云台输出结构体
*/
void Gimbal_DumpOutput(Gimbal_t *g, CAN_GimbalOutput_t *out);
/**
* \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

347
User/module/shoot.c Normal file
View File

@@ -0,0 +1,347 @@
/*
* 射击模组
*/
/* Includes ----------------------------------------------------------------- */
#include "shoot.h"
#include "bsp/pwm.h"
#include "component\limiter.h"
#include "component\user_math.h"
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
#define HEAT_INCREASE_42MM (100.f) /* 每发射一颗42mm弹丸增加100热量 */
#define HEAT_INCREASE_17MM (10.f) /* 每发射一颗17mm弹丸增加10热量 */
#define BULLET_SPEED_LIMIT_42MM (16.0)
#define BULLET_SPEED_LIMIT_17MM (30.0)
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* Private function -------------------------------------------------------- */
/**
* \brief 设置射击模式
*
* \param c 包含射击数据的结构体
* \param mode 要设置的模式
*
* \return 函数运行结果
*/
static int8_t Shoot_SetMode(Shoot_t *s, CMD_ShootMode_t mode) {
if (s == NULL) return -1;
if (mode == s->mode) return SHOOT_OK;
/* 切换模式后重置PID和滤波器 */
for (uint8_t i = 0; i < 2; i++) {
PID_Reset(s->pid.fric + i);
LowPassFilter2p_Reset(s->filter.in.fric + i, 0.0f);
LowPassFilter2p_Reset(s->filter.out.fric + i, 0.0f);
}
PID_Reset(&(s->pid.trig));
LowPassFilter2p_Reset(&(s->filter.in.trig), 0.0f);
LowPassFilter2p_Reset(&(s->filter.out.trig), 0.0f);
while (fabsf(CircleError(s->setpoint.trig_angle, s->feedback.trig_angle,
M_2PI)) >= M_2PI / s->param->num_trig_tooth / 2.0f) {
CircleAdd(&(s->setpoint.trig_angle), M_2PI / s->param->num_trig_tooth,
M_2PI);
}
if (mode == SHOOT_MODE_LOADED) s->fire_ctrl.to_shoot = 0;
s->mode = mode;
return 0;
}
/**
* @brief
*
* @param s
* @param s_ref
* @return int8_t
*/
static int8_t Shoot_HeatLimit(Shoot_t *s, Referee_ForShoot_t *s_ref) {
Shoot_HeatCtrl_t *hc = &(s->heat_ctrl);
/* 当裁判系统在线时启用热量控制与射速控制 */
if (s_ref->ref_status == REF_STATUS_RUNNING) {
/* 根据机器人型号获得对应数据 */
if (s->param->model == SHOOT_MODEL_42MM) {
hc->heat = s_ref->power_heat.shoot_42_heat;
hc->heat_limit = s_ref->robot_status.shooter_heat_limit;
hc->speed_limit = BULLET_SPEED_LIMIT_42MM;
hc->cooling_rate = s_ref->robot_status.shooter_cooling_value;
hc->heat_increase = HEAT_INCREASE_42MM;
} else if (s->param->model == SHOOT_MODEL_17MM) {
hc->heat = s_ref->power_heat.shoot_id1_17_heat;
hc->heat_limit = s_ref->robot_status.shooter_heat_limit;
hc->speed_limit = BULLET_SPEED_LIMIT_17MM;
hc->cooling_rate = s_ref->robot_status.shooter_cooling_value;
hc->heat_increase = HEAT_INCREASE_17MM;
}
/* 检测热量更新后,计算可发射弹丸 */
if ((hc->heat != hc->last_heat) || (hc->heat == 0)) {
hc->available_shot =
(uint32_t)floorf((hc->heat_limit - hc->heat) / hc->heat_increase);
hc->last_heat = hc->heat;
}
/* 计算已发射弹丸 */
if (s_ref->shoot_data.bullet_speed != hc->last_bullet_speed) {
hc->last_bullet_speed = s_ref->shoot_data.bullet_speed;
}
s->fire_ctrl.bullet_speed = hc->speed_limit;
} else {
/* 裁判系统离线，不启用热量控制 */
hc->available_shot = 10;
s->fire_ctrl.bullet_speed = s->param->bullet_speed;
}
return 0;
}
/* Exported functions ------------------------------------------------------- */
/**
* \brief 初始化射击
*
* \param s 包含射击数据的结构体
* \param param 包含射击参数的结构体指针
* \param target_freq 任务预期的运行频率
*
* \return 函数运行结果
*/
int8_t Shoot_Init(Shoot_t *s, const Shoot_Params_t *param, float target_freq) {
if (s == NULL) return -1;
s->param = param; /* 初始化参数 */
s->mode = SHOOT_MODE_RELAX; /* 设置默认模式 */
for (uint8_t i = 0; i < 2; i++) {
/* PI控制器初始化PID */
PID_Init(s->pid.fric + i, KPID_MODE_NO_D, target_freq,
&(param->fric_pid_param));
LowPassFilter2p_Init(s->filter.in.fric + i, target_freq,
param->low_pass_cutoff_freq.in.fric);
LowPassFilter2p_Init(s->filter.out.fric + i, target_freq,
param->low_pass_cutoff_freq.out.fric);
}
PID_Init(&(s->pid.trig), KPID_MODE_CALC_D, target_freq,
&(param->trig_pid_param));
LowPassFilter2p_Init(&(s->filter.in.trig), target_freq,
param->low_pass_cutoff_freq.in.trig);
LowPassFilter2p_Init(&(s->filter.out.trig), target_freq,
param->low_pass_cutoff_freq.out.trig);
BSP_PWM_Start(BSP_PWM_SHOOT_SERVO);
BSP_PWM_Set(BSP_PWM_SHOOT_SERVO, param->cover_close_duty);
return 0;
}
/**
* \brief 更新射击的反馈信息
*
* \param s 包含射击数据的结构体
* \param can CAN设备结构体
*
* \return 函数运行结果
*/
int8_t Shoot_UpdateFeedback(Shoot_t *s, const CAN_t *can) {
if (s == NULL) return -1;
if (can == NULL) return -1;
for (uint8_t i = 0; i < 2; i++) {
s->feedback.fric_rpm[i] = can->motor.shoot.as_array[i].rotor_speed;
}
/* 更新拨弹电机 */
float last_trig_motor_angle = s->feedback.trig_motor_angle;
s->feedback.trig_motor_angle = can->motor.shoot.named.trig.rotor_angle;
float motor_angle_delta =
CircleError(s->feedback.trig_motor_angle, last_trig_motor_angle, M_2PI);
CircleAdd(&(s->feedback.trig_angle),
motor_angle_delta / s->param->trig_gear_ratio, M_2PI);
return 0;
}
/**
* @brief 运行射击控制逻辑
*
* @param s 包含射击数据的结构体
* @param s_cmd 射击控制指令
* @param s_ref 射击使用的裁判系统数据
* @param now 现在时刻
* @return int8_t
*/
int8_t Shoot_Control(Shoot_t *s, CMD_ShootCmd_t *s_cmd,
Referee_ForShoot_t *s_ref, uint32_t now) {
if (s == NULL) return -1;
s->dt = (float)(now - s->lask_wakeup) / 1000.0f;
s->lask_wakeup = now;
Shoot_SetMode(s, s_cmd->mode); /* 设置射击模式 */
Shoot_HeatLimit(s, s_ref); /* 热量控制 */
/* 根据开火模式计算发射行为 */
s->fire_ctrl.fire_mode = s_cmd->fire_mode;
int32_t max_burst;
switch (s_cmd->fire_mode) {
case FIRE_MODE_SINGLE: /* 点射开火模式 */
max_burst = 1;
break;
case FIRE_MODE_BURST: /* 连发开火模式 */
max_burst = 5;
break;
default:
break;
}
switch (s_cmd->fire_mode) {
case FIRE_MODE_SINGLE: /* 点射开火模式 */
case FIRE_MODE_BURST: { /* 连发开火模式 */
s->fire_ctrl.first_fire = s_cmd->fire && !s->fire_ctrl.last_fire;
s->fire_ctrl.last_fire = s_cmd->fire;
s_cmd->fire = s->fire_ctrl.first_fire;
int32_t max_shot = s->heat_ctrl.available_shot - s->fire_ctrl.shooted;
if (s_cmd->fire && !s->fire_ctrl.to_shoot) {
s->fire_ctrl.to_shoot = min(max_burst, max_shot);
}
if (s->fire_ctrl.shooted >= s->fire_ctrl.to_shoot) {
s_cmd->fire = false;
s->fire_ctrl.period_ms = UINT32_MAX;
s->fire_ctrl.shooted = 0;
s->fire_ctrl.to_shoot = 0;
} else {
s_cmd->fire = true;
s->fire_ctrl.period_ms = s->param->min_shoot_delay;
}
break;
}
case FIRE_MODE_CONT: { /* 持续开火模式 */
float shoot_freq = HeatLimit_ShootFreq(
s->heat_ctrl.heat, s->heat_ctrl.heat_limit, s->heat_ctrl.cooling_rate,
s->heat_ctrl.heat_increase, s->param->model == SHOOT_MODEL_17MM);
s->fire_ctrl.period_ms =
(shoot_freq == 0.0f) ? UINT32_MAX : (uint32_t)(1000.f / shoot_freq);
break;
}
default:
break;
}
/* 根据模式选择是否使用计算出来的值 */
switch (s->mode) {
case SHOOT_MODE_RELAX:
case SHOOT_MODE_SAFE:
s->fire_ctrl.bullet_speed = 0.0f;
s->fire_ctrl.period_ms = UINT32_MAX;
case SHOOT_MODE_LOADED:
break;
}
/* 计算摩擦轮转速的目标值 */
s->setpoint.fric_rpm[1] =
CalculateRpm(s->fire_ctrl.bullet_speed, s->param->fric_radius,
(s->param->model == SHOOT_MODEL_17MM));
s->setpoint.fric_rpm[0] = -s->setpoint.fric_rpm[1];
/* 计算拨弹电机位置的目标值 */
if (((now - s->fire_ctrl.last_shoot) >= s->fire_ctrl.period_ms) &&
(s_cmd->fire)) {
/* 将拨弹电机角度进行循环加法，每次加(减)射出一颗弹丸的弧度变化 */
if (s_cmd->reverse_trig) { /* 反转拨弹 */
CircleAdd(&(s->setpoint.trig_angle), M_2PI / s->param->num_trig_tooth,
M_2PI);
} else {
CircleAdd(&(s->setpoint.trig_angle), -M_2PI / s->param->num_trig_tooth,
M_2PI);
s->fire_ctrl.shooted++;
s->fire_ctrl.last_shoot = now;
}
}
switch (s->mode) {
case SHOOT_MODE_RELAX:
for (uint8_t i = 0; i < SHOOT_ACTR_NUM; i++) {
s->out[i] = 0.0f;
}
BSP_PWM_Stop(BSP_PWM_SHOOT_SERVO);
break;
case SHOOT_MODE_SAFE:
case SHOOT_MODE_LOADED:
/* 控制拨弹电机 */
s->feedback.trig_angle =
LowPassFilter2p_Apply(&(s->filter.in.trig), s->feedback.trig_angle);
s->out[SHOOT_ACTR_TRIG_IDX] =
PID_Calc(&(s->pid.trig), s->setpoint.trig_angle,
s->feedback.trig_angle, 0.0f, s->dt);
s->out[SHOOT_ACTR_TRIG_IDX] = LowPassFilter2p_Apply(
&(s->filter.out.trig), s->out[SHOOT_ACTR_TRIG_IDX]);
for (uint8_t i = 0; i < 2; i++) {
/* 控制摩擦轮 */
s->feedback.fric_rpm[i] = LowPassFilter2p_Apply(
&(s->filter.in.fric[i]), s->feedback.fric_rpm[i]);
s->out[SHOOT_ACTR_FRIC1_IDX + i] =
PID_Calc(&(s->pid.fric[i]), s->setpoint.fric_rpm[i],
s->feedback.fric_rpm[i], 0.0f, s->dt);
s->out[SHOOT_ACTR_FRIC1_IDX + i] = LowPassFilter2p_Apply(
&(s->filter.out.fric[i]), s->out[SHOOT_ACTR_FRIC1_IDX + i]);
}
/* 根据弹仓盖开关状态更新弹舱盖打开时舵机PWM占空比 */
if (s_cmd->cover_open) {
BSP_PWM_Start(BSP_PWM_SHOOT_SERVO);
BSP_PWM_Set(BSP_PWM_SHOOT_SERVO, s->param->cover_open_duty);
} else {
BSP_PWM_Start(BSP_PWM_SHOOT_SERVO);
BSP_PWM_Set(BSP_PWM_SHOOT_SERVO, s->param->cover_close_duty);
}
break;
}
return 0;
}
/**
* \brief 复制射击输出值
*
* \param s 包含射击数据的结构体
* \param out CAN设备射击输出结构体
*/
void Shoot_DumpOutput(Shoot_t *s, CAN_ShootOutput_t *out) {
for (uint8_t i = 0; i < SHOOT_ACTR_NUM; i++) {
out->as_array[i] = s->out[i];
}
}
/**
* \brief 清空输出值
*
* \param output 要清空的结构体
*/
void Shoot_ResetOutput(CAN_ShootOutput_t *output) {
int i = 0;
for (i = 0; i < 3; i++) {
output->as_array[i] = 0.0f;
}
}
/**
* @brief 导出射击UI数据
*
* @param s 射击结构体
* @param ui UI结构体
*/
void Shoot_DumpUI(Shoot_t *s, Referee_ShootUI_t *ui) {
ui->mode = s->mode;
ui->fire = s->fire_ctrl.fire_mode;
}

209
User/module/shoot.h Normal file
View File

@@ -0,0 +1,209 @@
/*
* 射击模组
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <cmsis_os2.h>
#include "component\cmd.h"
#include "component\filter.h"
#include "component\pid.h"
#include "device\can.h"
#include "device\referee.h"
/* Exported constants ------------------------------------------------------- */
#define SHOOT_OK (0) /* 运行正常 */
#define SHOOT_ERR (-1) /* 运行时发现了其他错误 */
#define SHOOT_ERR_NULL (-2) /* 运行时发现NULL指针 */
#define SHOOT_ERR_MODE (-3) /* 运行时配置了错误的CMD_ShootMode_t */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
/* 用enum组合所有PID，方便访问，配合数组使用 */
enum Shoot_Acuator_e {
SHOOT_ACTR_FRIC1_IDX = 0, /* 1号摩擦轮相关的索引值 */
SHOOT_ACTR_FRIC2_IDX, /* 2号摩擦轮相关的索引值 */
SHOOT_ACTR_TRIG_IDX, /* 扳机电机相关的索引值 */
SHOOT_ACTR_NUM, /* 总共的动作器数量 */
};
/* 发射机构型号 */
typedef enum {
SHOOT_MODEL_17MM = 0, /* 17mm发射机构 */
SHOOT_MODEL_42MM, /* 42mm发射机构 */
} Shoot_Model_t;
/* 射击参数的结构体，包含所有初始化用的参数，通常是const，存好几组。*/
typedef struct {
KPID_Params_t fric_pid_param; /* 摩擦轮电机控制PID的参数 */
KPID_Params_t trig_pid_param; /* 扳机电机控制PID的参数 */
/* 低通滤波器截止频率 */
struct {
/* 输入 */
struct {
float fric; /* 摩擦轮电机 */
float trig; /* 扳机电机 */
} in;
/* 输出 */
struct {
float fric; /* 摩擦轮电机 */
float trig; /* 扳机电机 */
} out;
} low_pass_cutoff_freq;
float num_trig_tooth; /* 拨弹盘中一圈能存储几颗弹丸 */
float trig_gear_ratio; /* 拨弹电机减速比 3508:19, 2006:36 */
float fric_radius; /* 摩擦轮半径，单位：米 */
float cover_open_duty; /* 弹舱盖打开时舵机PWM占空比 */
float cover_close_duty; /* 弹舱盖关闭时舵机PWM占空比 */
Shoot_Model_t model; /* 发射机构型号 */
float bullet_speed; /* 弹丸初速度 */
uint32_t min_shoot_delay; /* 通过设置最小射击间隔来设置最大射频 */
} Shoot_Params_t;
typedef struct {
float heat; /* 现在热量水平 */
float last_heat; /* 之前的热量水平 */
float heat_limit; /* 热量上限 */
float speed_limit; /* 弹丸初速是上限 */
float cooling_rate; /* 冷却速率 */
float heat_increase; /* 每发热量增加值 */
float last_bullet_speed; /* 之前的弹丸速度 */
uint32_t available_shot; /* 热量范围内还可以发射的数量 */
} Shoot_HeatCtrl_t;
typedef struct {
uint32_t last_shoot; /* 上次射击时间 单位：ms */
bool last_fire; /* 上次开火状态 */
bool first_fire; /* 第一次收到开火指令 */
uint32_t shooted; /* 已经发射的弹丸 */
uint32_t to_shoot; /* 计划发射的弹丸 */
float bullet_speed; /* 弹丸初速度 */
uint32_t period_ms; /* 弹丸击发延迟 */
CMD_FireMode_t fire_mode;
} Shoot_FireCtrl_t;
/*
* 运行的主结构体，所有这个文件里的函数都在操作这个结构体。
* 包含了初始化参数，中间变量，输出变量。
*/
typedef struct {
uint32_t lask_wakeup;
float dt;
const Shoot_Params_t *param; /* 射击的参数，用Shoot_Init设定 */
/* 模块通用 */
CMD_ShootMode_t mode; /* 射击模式 */
/* 反馈信息 */
struct {
float fric_rpm[2]; /* 摩擦轮电机转速，单位：RPM */
float trig_motor_angle; /* 拨弹电机角度，单位：弧度 */
float trig_angle; /* 拨弹转盘角度，单位：弧度 */
} feedback;
/* PID计算的目标值 */
struct {
float fric_rpm[2]; /* 摩擦轮电机转速，单位：RPM */
float trig_angle; /* 拨弹电机角度，单位：弧度 */
} setpoint;
/* 反馈控制用的PID */
struct {
KPID_t fric[2]; /* 控制摩擦轮 */
KPID_t trig; /* 控制拨弹电机 */
} pid;
/* 过滤器 */
struct {
/* 反馈值滤波器 */
struct {
LowPassFilter2p_t fric[2]; /* 过滤摩擦轮 */
LowPassFilter2p_t trig; /* 过滤拨弹电机 */
} in;
/* 输出值滤波器 */
struct {
LowPassFilter2p_t fric[2]; /* 过滤摩擦轮 */
LowPassFilter2p_t trig; /* 过滤拨弹电机 */
} out;
} filter;
Shoot_HeatCtrl_t heat_ctrl;
Shoot_FireCtrl_t fire_ctrl;
float out[SHOOT_ACTR_NUM]; /* 输出数组，通过Shoot_Acuator_e里的值访问 */
} Shoot_t;
/* Exported functions prototypes -------------------------------------------- */
/**
* \brief 初始化射击
*
* \param s 包含射击数据的结构体
* \param param 包含射击参数的结构体指针
* \param target_freq 任务预期的运行频率
*
* \return 函数运行结果
*/
int8_t Shoot_Init(Shoot_t *s, const Shoot_Params_t *param, float target_freq);
/**
* \brief 更新射击的反馈信息
*
* \param s 包含射击数据的结构体
* \param can CAN设备结构体
*
* \return 函数运行结果
*/
int8_t Shoot_UpdateFeedback(Shoot_t *s, const CAN_t *can);
/**
* \brief 运行射击控制逻辑
*
* \param s 包含射击数据的结构体
* \param s_cmd 射击控制指令
* \param s_ref 裁判系统数据
* \param dt_sec 两次调用的时间间隔
*
* \return 函数运行结果
*/
int8_t Shoot_Control(Shoot_t *s, CMD_ShootCmd_t *s_cmd,
Referee_ForShoot_t *s_ref, uint32_t now);
/**
* \brief 复制射击输出值
*
* \param s 包含射击数据的结构体
* \param out CAN设备射击输出结构体
*/
void Shoot_DumpOutput(Shoot_t *s, CAN_ShootOutput_t *out);
/**
* \brief 清空输出值
*
* \param output 要清空的结构体
*/
void Shoot_ResetOutput(CAN_ShootOutput_t *output);
/**
* @brief 导出射击UI数据
*
* @param s 射击结构体
* @param ui UI结构体
*/
void Shoot_DumpUI(Shoot_t *s, Referee_ShootUI_t *ui);
#ifdef __cplusplus
}
#endif