235 lines
7.6 KiB
C
235 lines
7.6 KiB
C
/*
|
||
* far♂蛇模块
|
||
*/
|
||
|
||
#pragma once
|
||
|
||
#ifdef __cplusplus
|
||
extern "C" {
|
||
#endif
|
||
|
||
#include "main.h"
|
||
#include <stdbool.h>
|
||
#include "component/pid.h"
|
||
#include "device/motor_rm.h"
|
||
|
||
|
||
/* Exported constants ------------------------------------------------------- */
|
||
#define SHOOT_OK (0) /* 运行正常 */
|
||
#define SHOOT_ERR_NULL (-1) /* 运行时发现NULL指针 */
|
||
#define SHOOT_ERR_ERR (-2) /* 运行时发现了其他错误 */
|
||
#define SHOOT_ERR_MODE (-3) /* 运行时配置了错误的Mode */
|
||
#define SHOOT_ERR_MOTOR (-4) /* 运行时配置了不存在的电机类型 */
|
||
#define SHOOT_ERR_MALLOC (-5) /* 内存分配失败 */
|
||
|
||
#define MAX_FRIC_RPM 7000.0f
|
||
#define MAX_TRIG_RPM 5000.0f//这里可能也会影响最高发射频率，待测试
|
||
/* Exported macro ----------------------------------------------------------- */
|
||
/* Exported types ----------------------------------------------------------- */
|
||
typedef enum {
|
||
SHOOT_JAMFSM_STATE_NORMAL = 0, /* 常规状态 */
|
||
SHOOT_JAMFSM_STATE_SUSPECTED, /* 怀疑状态 */
|
||
SHOOT_JAMFSM_STATE_CONFIRMED, /* 确认状态 */
|
||
SHOOT_JAMFSM_STATE_DEAL /* 处理状态 */
|
||
} Shoot_JamDetectionFSM_State_t;
|
||
typedef enum {
|
||
SHOOT_STATE_IDLE = 0, /* 熄火 */
|
||
SHOOT_STATE_READY, /* 准备射击 */
|
||
SHOOT_STATE_FIRE /* 射击 */
|
||
} Shoot_Running_State_t;
|
||
|
||
typedef enum {
|
||
SHOOT_MODE_SAFE = 0, /* 安全模式 */
|
||
SHOOT_MODE_SINGLE, /* 单发模式 */
|
||
SHOOT_MODE_BURST, /* 多发模式 */
|
||
SHOOT_MODE_CONTINUE /* 连发模式 */
|
||
} Shoot_Mode_t;
|
||
typedef struct {
|
||
bool online;
|
||
|
||
bool ready; /* 准备射击 */
|
||
bool firecmd; /* 射击指令 */
|
||
|
||
} Shoot_CMD_t;
|
||
|
||
typedef struct{
|
||
MOTOR_RM_Param_t param;
|
||
uint8_t level; /*电机属于几级发射；1起始；num_multilevel大于1时有效，且不可大于num_multilevel*/
|
||
}Shoot_MOTOR_RM_Param_t;
|
||
|
||
typedef struct {
|
||
MOTOR_Feedback_t *fric; /* 摩擦轮电机反馈 */
|
||
MOTOR_Feedback_t trig; /* 拨弹电机反馈 */
|
||
|
||
float *fil_fric_rpm; /* 滤波后的摩擦轮转速 */
|
||
float fil_trig_rpm; /* 滤波后的拨弹电机转速*/
|
||
|
||
float *fric_rpm; /* 归一化摩擦轮转速 */
|
||
float fric_avgrpm; /* 归一化摩擦轮平均转速*/
|
||
float trig_rpm; /* 归一化拨弹电机转速*/
|
||
|
||
}Shoot_Feedback_t;
|
||
|
||
typedef struct{
|
||
float time_last_shoot;
|
||
uint16_t num_to_shoot;
|
||
uint16_t num_shooted;
|
||
}Shoot_AngleCalu_t;
|
||
|
||
typedef struct {
|
||
bool jam_detected; /* 卡弹检测结果 */
|
||
float jam_last_time;/* 用于记录怀疑状态或处理状态的开始时间 */
|
||
Shoot_JamDetectionFSM_State_t jamfsm_state; /* 卡弹检测状态 */
|
||
}Shoot_JamDetection_t;
|
||
typedef struct {
|
||
float *out_follow;
|
||
float *out_err;
|
||
float *out_fric;
|
||
float *lpfout_fric;
|
||
|
||
|
||
float outagl_trig;
|
||
float outomg_trig;
|
||
float outlpf_trig;
|
||
}Shoot_Output_t;
|
||
|
||
|
||
/* 底盘参数的结构体，包含所有初始化用的参数，通常是const，存好几组 */
|
||
typedef struct {
|
||
uint8_t fric_num; /* 摩擦轮数量 */
|
||
|
||
float num_trig_tooth; /* 拨弹盘每圈弹丸数量 */
|
||
float shot_freq; /* 射击频率，单位Hz */
|
||
uint8_t shot_burst_num; /* 多发模式一次射击的数量 */
|
||
|
||
uint8_t num_multilevel; /* 多级发射级数 (默认每级摩擦轮的数量相等)*/
|
||
float *ratio_multilevel; /* 多级发射各级速度比例 */
|
||
|
||
bool jam_enable; /* 是否启用卡弹检测 */ //还没加到逻辑里
|
||
float jam_threshold; /* 卡弹检测阈值，单位A (dji2006建议设置为120A，dji3508建议设置为300A,根据实际测试调整)*/
|
||
float jam_suspected_time; /* 卡弹怀疑时间，单位秒 */
|
||
|
||
Shoot_MOTOR_RM_Param_t *fric_motor_param;
|
||
MOTOR_RM_Param_t trig_motor_param;
|
||
|
||
|
||
KPID_Params_t fric_follow; /* 摩擦轮电机PID控制参数，用于跟随目标速度 */
|
||
KPID_Params_t fric_err; /* 摩擦轮电机PID控制参数，用于消除转速误差 */
|
||
KPID_Params_t trig_2006; /* 拨弹电机PID控制参数 */
|
||
KPID_Params_t trig_omg_2006; /* 拨弹电机PID控制参数 */
|
||
KPID_Params_t trig_3508; /* 拨弹电机PID控制参数 */
|
||
KPID_Params_t trig_omg_3508; /* 拨弹电机PID控制参数 */
|
||
/* 低通滤波器截止频率 */
|
||
struct {
|
||
struct{
|
||
float in; /* 反馈值滤波器 */
|
||
float out; /* 输出值滤波器 */
|
||
}fric;
|
||
struct{
|
||
float in; /* 反馈值滤波器 */
|
||
float out; /* 输出值滤波器 */
|
||
}trig;
|
||
} filter;
|
||
} Shoot_Params_t;
|
||
|
||
/*
|
||
* 运行的主结构体，所有这个文件里的函数都在操作这个结构体
|
||
* 包含了初始化参数，中间变量，输出变量
|
||
*/
|
||
typedef struct {
|
||
bool online;//待完善，电机或遥控器在线检测
|
||
|
||
float now; /* 当前时间，单位秒 */
|
||
uint64_t lask_wakeup; /* 上次唤醒时间，单位微秒 */
|
||
float dt; /* 两次唤醒间隔时间，单位秒 */
|
||
Shoot_Params_t *param; /* 发射参数 */
|
||
/* 模块通用 */
|
||
Shoot_Running_State_t running_state; /* 运行状态机 */
|
||
Shoot_Mode_t mode; /* 射击模式 */
|
||
/* 反馈信息 */
|
||
Shoot_Feedback_t feedback; /* 反馈信息 */
|
||
/* 控制信息*/
|
||
Shoot_JamDetection_t jamdetection; /* 卡弹检测控制信息 */
|
||
Shoot_AngleCalu_t anglecalu; /* 角度计算控制信息 */
|
||
Shoot_Output_t output; /* 输出信息 */
|
||
/* 目标控制量 */
|
||
struct {
|
||
float target_rpm; /* 目标摩擦轮转速 */
|
||
float target_angle; /* 目标拨弹位置 */
|
||
}target_variable;
|
||
|
||
/* 反馈控制用的PID */
|
||
struct {
|
||
KPID_t *fric_follow; /* 摩擦轮PID主结构体 */
|
||
KPID_t *fric_err; /* 摩擦轮PID主结构体 */
|
||
KPID_t trig; /* 拨弹PID主结构体 */
|
||
KPID_t trig_omg; /* 拨弹PID主结构体 */
|
||
} pid;
|
||
|
||
/* 滤波器 */
|
||
struct {
|
||
struct{
|
||
LowPassFilter2p_t *in; /* 反馈值滤波器 */
|
||
LowPassFilter2p_t *out; /* 输出值滤波器 */
|
||
}fric;
|
||
struct{
|
||
LowPassFilter2p_t in; /* 反馈值滤波器 */
|
||
LowPassFilter2p_t out; /* 输出值滤波器 */
|
||
}trig;
|
||
} filter;
|
||
|
||
float errtosee; /*调试用*/
|
||
|
||
} Shoot_t;
|
||
|
||
/* Exported functions prototypes -------------------------------------------- */
|
||
|
||
/**
|
||
* \brief 初始化发射
|
||
*
|
||
* \param s 包含发射数据的结构体
|
||
* \param param 包含发射参数的结构体指针
|
||
* \param target_freq 任务预期的运行频率
|
||
*
|
||
* \return 函数运行结果
|
||
*/
|
||
int8_t Shoot_Init(Shoot_t *s, Shoot_Params_t *param, float target_freq);
|
||
|
||
/**
|
||
* \brief 设置发射模式
|
||
*
|
||
* \param s 包含发射数据的结构体
|
||
* \param mode 包含发射模式的枚举
|
||
*
|
||
* \return 函数运行结果
|
||
*/
|
||
int8_t Shoot_SetMode(Shoot_t *s, Shoot_Mode_t mode);
|
||
|
||
/**
|
||
* \brief 更新反馈
|
||
*
|
||
* \param s 包含发射数据的结构体
|
||
*
|
||
* \return 函数运行结果
|
||
*/
|
||
int8_t Chassis_UpdateFeedback(Shoot_t *s);
|
||
|
||
/**
|
||
* \brief 初始化发射
|
||
*
|
||
* \param s 包含发射数据的结构体
|
||
* \param cmd 包含发射命令的结构体
|
||
*
|
||
* \return 函数运行结果
|
||
*/
|
||
int8_t Shoot_Control(Shoot_t *s, Shoot_CMD_t *cmd);
|
||
|
||
|
||
|
||
#ifdef __cplusplus
|
||
}
|
||
#endif
|
||
|
||
|
||
|