202 lines
6.8 KiB
C
202 lines
6.8 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]);
|
||
// }
|
||
|
||
|