202 lines
6.3 KiB
C
202 lines
6.3 KiB
C
/*
|
||
* 射击模组
|
||
*/
|
||
|
||
/* Includes ----------------------------------------------------------------- */
|
||
#include "shoot.h"
|
||
|
||
#include "bsp/pwm.h"
|
||
#include "bsp/time.h"
|
||
#include "bsp/can.h"
|
||
#include "component/pid.h"
|
||
#include "component/limiter.h"
|
||
#include "component/user_math.h"
|
||
#include "device/motor_rm.h"
|
||
#include <stdint.h>
|
||
/* Private typedef ---------------------------------------------------------- */
|
||
/* Private define ----------------------------------------------------------- */
|
||
|
||
#define BULLET_SPEED_LIMIT_17MM (25.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);
|
||
PID_Reset(&(s->pid.fric[0]));
|
||
PID_Reset(&(s->pid.fric[1]));
|
||
LowPassFilter2p_Reset(&(s->filter.in.fric[0]), 0.0f);
|
||
LowPassFilter2p_Reset(&(s->filter.in.fric[1]), 0.0f);
|
||
LowPassFilter2p_Reset(&(s->filter.out.fric[0]), 0.0f);
|
||
LowPassFilter2p_Reset(&(s->filter.out.fric[1]), 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;
|
||
}
|
||
|
||
// static float Shoot_CalcRPMFromSpeed(float speed, float radius, float *rpm) {
|
||
// if (rpm == NULL) return -1;
|
||
// if (radius <= 0.0f) return -2;
|
||
|
||
// /* 限制弹丸速度 */
|
||
// speed = Limit_Abs(speed, BULLET_SPEED_LIMIT_17MM);
|
||
|
||
// /* 计算转速 */
|
||
// *rpm = speed / (2.0f * M_PI * radius) * 60.0f;
|
||
|
||
// 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; /* 设置默认模式 */
|
||
|
||
PID_Init(&(s->pid.fric[0]), KPID_MODE_NO_D, target_freq,
|
||
&(param->fric_pid_param));
|
||
PID_Init(&(s->pid.fric[1]), KPID_MODE_NO_D, target_freq,
|
||
&(param->fric_pid_param));
|
||
PID_Init(&(s->pid.trig), KPID_MODE_CALC_D, target_freq,
|
||
&(param->trig_pid_param));
|
||
|
||
LowPassFilter2p_Init(&(s->filter.in.fric[0]), target_freq,
|
||
param->low_pass_cutoff_freq.in.fric);
|
||
LowPassFilter2p_Init(&(s->filter.in.fric[1]), target_freq,
|
||
param->low_pass_cutoff_freq.in.fric);
|
||
LowPassFilter2p_Init(&(s->filter.out.fric[0]), target_freq,
|
||
param->low_pass_cutoff_freq.out.fric);
|
||
LowPassFilter2p_Init(&(s->filter.out.fric[1]), target_freq,
|
||
param->low_pass_cutoff_freq.out.fric);
|
||
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_CAN_Init();
|
||
MOTOR_RM_Register(s->param->fric_motor_param[0]);
|
||
MOTOR_RM_Register(s->param->fric_motor_param[1]);
|
||
MOTOR_RM_Register(s->param->trig_motor_param);
|
||
|
||
return SHOOT_OK;
|
||
}
|
||
|
||
/**
|
||
* \brief 更新射击的反馈信息
|
||
*
|
||
* \param s 包含射击数据的结构体
|
||
*
|
||
* \return 函数运行结果
|
||
*/
|
||
int8_t Shoot_UpdateFeedback(Shoot_t *s) {
|
||
if (s == NULL) return -1;
|
||
MOTOR_RM_Update(s->param->fric_motor_param[0]);
|
||
MOTOR_RM_Update(s->param->fric_motor_param[1]);
|
||
MOTOR_RM_Update(s->param->trig_motor_param);
|
||
|
||
MOTOR_RM_t *motor;
|
||
motor = MOTOR_RM_GetMotor(s->param->fric_motor_param[0]);
|
||
s->feedback.fric[0] = motor->feedback;
|
||
motor = MOTOR_RM_GetMotor(s->param->fric_motor_param[1]);
|
||
s->feedback.fric[1] = motor->feedback;
|
||
motor = MOTOR_RM_GetMotor(s->param->trig_motor_param);
|
||
s->feedback.trig = motor->feedback;
|
||
|
||
return SHOOT_OK;
|
||
}
|
||
|
||
/**
|
||
* \brief 运行射击控制逻辑
|
||
*
|
||
* \param s 包含射击数据的结构体
|
||
* \param s_cmd 射击控制指令
|
||
*
|
||
* \return 函数运行结果
|
||
*/
|
||
int8_t Shoot_Control(Shoot_t *s, Shoot_CMD_t *s_cmd) {
|
||
if (s == NULL) return -1;
|
||
if (s_cmd == NULL) return -1;
|
||
|
||
Shoot_SetMode(s, s_cmd->mode); /* 设置射击模式 */
|
||
|
||
/* 计算摩擦轮转速的目标值 */
|
||
// 根据弹丸射速计算转速，这里需要实现具体的计算逻辑
|
||
// s->setpoint.fric_rpm[0] = calculate_rpm_from_speed(s_cmd->bullet_speed);
|
||
// s->setpoint.fric_rpm[1] = -s->setpoint.fric_rpm[0];
|
||
|
||
switch (s->mode) {
|
||
case SHOOT_MODE_RELAX:
|
||
MOTOR_RM_SetOutput(s->param->fric_motor_param[0], 0.0f);
|
||
MOTOR_RM_SetOutput(s->param->fric_motor_param[1], 0.0f);
|
||
MOTOR_RM_SetOutput(s->param->trig_motor_param, 0.0f);
|
||
break;
|
||
case SHOOT_MODE_SAFE:
|
||
/*摩擦轮速度环到0，拨弹位置环到设定位置*/
|
||
s->setpoint.fric_rpm[0] = 0.0f;
|
||
s->setpoint.fric_rpm[1] = 0.0f;
|
||
|
||
|
||
|
||
break;
|
||
|
||
case SHOOT_MODE_LOADED:
|
||
// TODO: 实现上膛模式的控制逻辑
|
||
break;
|
||
}
|
||
return SHOOT_OK;
|
||
}
|
||
|
||
/**
|
||
* \brief 射击输出值
|
||
*
|
||
* \param s 包含射击数据的结构体
|
||
*/
|
||
void Shoot_Output(Shoot_t *s) {
|
||
if (s == NULL) return;
|
||
MOTOR_RM_Ctrl(s->param->fric_motor_param[0]);
|
||
}
|
||
|
||
|