623 lines
22 KiB
C
623 lines
22 KiB
C
/*
|
||
* far♂蛇模块
|
||
*/
|
||
|
||
/********************************* 使用示例 **********************************/
|
||
/*1.配置config参数以及Config_ShootInit函数参数*/
|
||
/*2.
|
||
COMP_AT9S_CMD_t shoot_ctrl_cmd_rc;
|
||
Shoot_t shoot;
|
||
Shoot_CMD_t shoot_cmd;
|
||
|
||
void Task(void *argument) {
|
||
|
||
Config_ShootInit();
|
||
Shoot_Init(&shoot,&Config_GetRobotParam()->shoot_param,SHOOT_CTRL_FREQ);
|
||
Shoot_SetMode(&shoot,SHOOT_MODE_SINGLE); 关于模式选择：初始化一个模式
|
||
|
||
while (1) {
|
||
|
||
shoot_cmd.online =shoot_ctrl_cmd_rc.online;
|
||
shoot_cmd.ready =shoot_ctrl_cmd_rc.shoot.ready;
|
||
shoot_cmd.firecmd =shoot_ctrl_cmd_rc.shoot.firecmd;
|
||
|
||
shoot.mode =shoot_ctrl_cmd_rc.mode; 关于模式选择：或者用遥控器随时切换模式，二选一
|
||
|
||
Chassis_UpdateFeedback(&shoot);
|
||
Shoot_Control(&shoot,&shoot_cmd);
|
||
}
|
||
}
|
||
*******************************************************************************/
|
||
|
||
|
||
/* Includes ----------------------------------------------------------------- */
|
||
#include <string.h>
|
||
#include "module/shoot.h"
|
||
#include "bsp/mm.h"
|
||
#include "bsp/time.h"
|
||
#include "component/filter.h"
|
||
#include "component/user_math.h"
|
||
/* Private typedef ---------------------------------------------------------- */
|
||
/* Private define ----------------------------------------------------------- */
|
||
/* Private macro ------------------------------------------------------------ */
|
||
/* Private variables -------------------------------------------------------- */
|
||
static bool last_firecmd;
|
||
/* Private function -------------------------------------------------------- */
|
||
|
||
/**
|
||
* \brief 设置射击模式
|
||
*
|
||
* \param s 包含射击数据的结构体
|
||
* \param mode 要设置的模式
|
||
*
|
||
* \return 函数运行结果
|
||
*/
|
||
int8_t Shoot_SetMode(Shoot_t *s, Shoot_Mode_t mode)
|
||
{
|
||
if (s == NULL) {
|
||
return SHOOT_ERR_NULL; // 参数错误
|
||
}
|
||
s->mode=mode;
|
||
s->anglecalu.num_to_shoot=0;
|
||
return SHOOT_OK;
|
||
}
|
||
|
||
/**
|
||
* \brief 重置PID积分
|
||
*
|
||
* \param s 包含射击数据的结构体
|
||
*
|
||
* \return 函数运行结果
|
||
*/
|
||
int8_t Shoot_ResetIntegral(Shoot_t *s)
|
||
{
|
||
if (s == NULL) {
|
||
return SHOOT_ERR_NULL; // 参数错误
|
||
}
|
||
uint8_t fric_num = s->param->fric_num;
|
||
for(int i=0;i<fric_num;i++)
|
||
{
|
||
PID_ResetIntegral(&s->pid.fric_agl[i]);
|
||
PID_ResetIntegral(&s->pid.fric_omg[i]);
|
||
}
|
||
PID_ResetIntegral(&s->pid.trig);
|
||
PID_ResetIntegral(&s->pid.trig_omg);
|
||
return SHOOT_OK;
|
||
}
|
||
|
||
/**
|
||
* \brief 重置计算模块
|
||
*
|
||
* \param s 包含射击数据的结构体
|
||
*
|
||
* \return 函数运行结果
|
||
*/
|
||
int8_t Shoot_ResetCalu(Shoot_t *s)
|
||
{
|
||
if (s == NULL) {
|
||
return SHOOT_ERR_NULL; // 参数错误
|
||
}
|
||
uint8_t fric_num = s->param->fric_num;
|
||
// for(int i=0;i<fric_num;i++)
|
||
// {
|
||
// PID_Reset(&s->pid.fric_agl[i]);
|
||
// PID_Reset(&s->pid.fric_omg[i]);
|
||
// LowPassFilter2p_Reset(&s->filter.fric.in[i], 0.0f);
|
||
// LowPassFilter2p_Reset(&s->filter.fric.out[i], 0.0f);
|
||
// }
|
||
PID_Reset(&s->pid.trig);
|
||
PID_Reset(&s->pid.trig_omg);
|
||
LowPassFilter2p_Reset(&s->filter.trig.in, 0.0f);
|
||
LowPassFilter2p_Reset(&s->filter.trig.out, 0.0f);
|
||
return SHOOT_OK;
|
||
}
|
||
|
||
/**
|
||
* \brief 重置输出
|
||
*
|
||
* \param s 包含射击数据的结构体
|
||
*
|
||
* \return 函数运行结果
|
||
*/
|
||
int8_t Shoot_ResetOutput(Shoot_t *s)
|
||
{
|
||
if (s == NULL) {
|
||
return SHOOT_ERR_NULL; // 参数错误
|
||
}
|
||
// uint8_t fric_num = s->param->fric_num;
|
||
// for(int i=0;i<fric_num;i++)
|
||
// {
|
||
// s->output.outagl_fric[i]=0.0f;
|
||
// s->output.outomg_fric[i]=0.0f;
|
||
// s->output.out_fric[i]=0.0f;
|
||
// s->output.lpfout_fric[i]=0.0f;
|
||
// }
|
||
s->output.outagl_trig=0.0f;
|
||
s->output.outomg_trig=0.0f;
|
||
s->output.outlpf_trig=0.0f;
|
||
return SHOOT_OK;
|
||
}
|
||
|
||
/**
|
||
* \brief 根据目标弹丸速度计算摩擦轮目标转速
|
||
*
|
||
* \param s 包含射击数据的结构体
|
||
* \param target_speed 目标弹丸速度，单位m/s
|
||
*
|
||
* \return 函数运行结果
|
||
*/
|
||
int8_t Shoot_CaluTargetRPM(Shoot_t *s, Shoot_CMD_t *cmd)
|
||
{
|
||
if (s == NULL) {
|
||
return SHOOT_ERR_NULL; // 参数错误
|
||
}
|
||
|
||
//s->target_variable.target_pair_angle[1]=(float)(cmd->pair_position) *31.415926535*2/1500 *1130/1500;
|
||
for(int i=0;i<=1;i++)
|
||
{
|
||
s->target_variable.target_pair_angle[i] = (float)(cmd->pair_position)*7/1500*M_2PI - cmd->position*M_2PI + s->pos_comp[i];
|
||
Clip(&s->target_variable.target_pair_angle[i], 0 , 5000);
|
||
}
|
||
|
||
return SHOOT_OK;
|
||
}
|
||
|
||
/**
|
||
* \brief 根据发射弹丸数量及发射频率计算拨弹电机目标角度
|
||
*
|
||
* \param s 包含发射数据的结构体
|
||
* \param cmd 包含射击指令的结构体
|
||
*
|
||
* \return 函数运行结果
|
||
*/
|
||
int8_t Shoot_CaluTargetAngle(Shoot_t *s, Shoot_CMD_t *cmd)
|
||
{
|
||
if (s == NULL || cmd == NULL || s->anglecalu.num_to_shoot == 0) {//弹丸数为0时不再转
|
||
return SHOOT_ERR_NULL;
|
||
}
|
||
float dt = s->now - s->anglecalu.time_last_shoot;
|
||
float dpos;
|
||
dpos = CircleError(s->target_variable.target_angle, s->feedback.trig_agl, M_2PI);
|
||
if(dt >= 1.0f/s->param->shot_freq && cmd->firecmd && dpos<=1.0f && dpos>=-1.0f)
|
||
{
|
||
|
||
if (cmd->reverse == true) { // 顺时针
|
||
CircleAdd(&s->target_variable.target_angle,
|
||
M_2PI/s->param->num_trig_tooth, M_2PI);
|
||
} else if (cmd->reverse == false) { // 逆时针
|
||
CircleAdd(&s->target_variable.target_angle,
|
||
-M_2PI/s->param->num_trig_tooth, M_2PI);
|
||
}
|
||
s->anglecalu.time_last_shoot=s->now;
|
||
s->anglecalu.num_to_shoot--;
|
||
}
|
||
return SHOOT_OK;
|
||
}
|
||
|
||
/**
|
||
* \brief 更新射击模块的电机反馈信息
|
||
*
|
||
* \param s 包含射击数据的结构体
|
||
*
|
||
* \return 函数运行结果
|
||
*/
|
||
int8_t Shoot_UpdateFeedback(Shoot_t *s)
|
||
{
|
||
if (s == NULL) {
|
||
return SHOOT_ERR_NULL; // 参数错误
|
||
}
|
||
float agl_sum=0.0f;
|
||
uint8_t fric_num = s->param->fric_num;
|
||
for(int i = 0; i < fric_num; i++) {
|
||
/* 更新蓄力电机反馈 */
|
||
MOTOR_RM_Update(&s->param->fric_motor_param[i].param);
|
||
MOTOR_RM_t *motor_fed = MOTOR_RM_GetMotor(&s->param->fric_motor_param[i].param);
|
||
if(motor_fed!=NULL)
|
||
{
|
||
s->feedback.fric[i]=*motor_fed;
|
||
}
|
||
/* 滤波摩擦轮电机转速反馈 */
|
||
s->feedback.fil_fric_rpm[i] = LowPassFilter2p_Apply(&s->filter.fric.in[i], s->feedback.fric[i].feedback.rotor_speed);
|
||
/* 归一化摩擦轮电机转速反馈 */
|
||
s->feedback.fric_rpm[i] = s->feedback.fil_fric_rpm[i] / MAX_FRIC_RPM;
|
||
if(s->feedback.fric_rpm[i]>1.0f) s->feedback.fric_rpm[i] = 1.0f;
|
||
if(s->feedback.fric_rpm[i]<-1.0f) s->feedback.fric_rpm[i] = -1.0f;
|
||
/* 计算平均摩擦轮电机转速反馈 */
|
||
|
||
|
||
/* 计算蓄力电机角度反馈 */
|
||
s->feedback.fric_agl[i] = s->feedback.fric[i].gearbox_total_angle;
|
||
// while(s->feedback.fric_agl[i]<0) s->feedback.fric_agl[i] += M_2PI;
|
||
// while(s->feedback.fric_agl[i]>=M_2PI) s->feedback.fric_agl[i] -= M_2PI;
|
||
if (s->feedback.fric[i].motor.reverse) {//处理电机反装
|
||
s->feedback.fric_agl[i] = - s->feedback.fric_agl[i];
|
||
}
|
||
/* 计算蓄力电机角度和 */
|
||
agl_sum+=s->feedback.fric_rpm[i];
|
||
|
||
}
|
||
/* 计算蓄力电机平均角度 */
|
||
s->feedback.fric_avgagl=agl_sum/fric_num;
|
||
/* 更新拨弹电机反馈 */
|
||
MOTOR_RM_Update(&s->param->trig_motor_param);
|
||
s->feedback.trig = *MOTOR_RM_GetMotor(&s->param->trig_motor_param);
|
||
s->feedback.trig_agl=s->param->extra_deceleration_ratio*s->feedback.trig.gearbox_total_angle;
|
||
while(s->feedback.trig_agl<0)s->feedback.trig_agl+=M_2PI;
|
||
while(s->feedback.trig_agl>=M_2PI)s->feedback.trig_agl-=M_2PI;
|
||
if (s->feedback.trig.motor.reverse) {
|
||
s->feedback.trig_agl = M_2PI - s->feedback.trig_agl;
|
||
}
|
||
s->feedback.fil_trig_rpm = LowPassFilter2p_Apply(&s->filter.trig.in, s->feedback.trig.feedback.rotor_speed);
|
||
s->feedback.trig_rpm = s->feedback.trig.feedback.rotor_speed / MAX_TRIG_RPM;
|
||
if(s->feedback.trig_rpm>1.0f)s->feedback.trig_rpm=1.0f;
|
||
if(s->feedback.trig_rpm<-1.0f)s->feedback.trig_rpm=-1.0f;
|
||
|
||
// s->errtosee = s->feedback.fric[0].rotor_speed - s->feedback.fric[1].rotor_speed;
|
||
return SHOOT_OK;
|
||
}
|
||
|
||
/**
|
||
* \brief 射击模块运行状态机
|
||
*
|
||
* \param s 包含射击数据的结构体
|
||
* \param cmd 包含射击指令的结构体
|
||
*
|
||
* \return 函数运行结果
|
||
*/uint8_t last_flag = 0;
|
||
int8_t Shoot_RunningFSM(Shoot_t *s, Shoot_CMD_t *cmd)
|
||
{
|
||
if (s == NULL || cmd == NULL) {
|
||
return SHOOT_ERR_NULL; // 参数错误
|
||
}
|
||
uint8_t fric_num = s->param->fric_num;
|
||
uint8_t num_multilevel = s->param->num_multilevel;
|
||
if(!s->online /*|| s->mode==SHOOT_MODE_SAFE*/){
|
||
for(int i=0;i<fric_num;i++)
|
||
{
|
||
MOTOR_RM_Relax(&s->param->fric_motor_param[i].param);
|
||
}
|
||
MOTOR_RM_Relax(&s->param->trig_motor_param);
|
||
}
|
||
else{
|
||
static float pos;
|
||
switch(s->running_state)
|
||
{
|
||
case SHOOT_STATE_IDLE:/*熄火等待*/
|
||
|
||
s->output.outagl_trig = PID_Calc(&s->pid.trig,pos,s->feedback.trig_agl,0,s->dt);
|
||
s->output.outomg_trig = PID_Calc(&s->pid.trig_omg,s->output.outagl_trig,s->feedback.trig_rpm,0,s->dt);
|
||
s->output.outlpf_trig = LowPassFilter2p_Apply(&s->filter.trig.out, s->output.outomg_trig);
|
||
MOTOR_RM_SetOutput(&s->param->trig_motor_param, s->output.outlpf_trig);
|
||
|
||
/* 检查状态机 */
|
||
if(cmd->ready)
|
||
{
|
||
Shoot_ResetCalu(s);
|
||
Shoot_ResetIntegral(s);
|
||
Shoot_ResetOutput(s);
|
||
s->running_state=SHOOT_STATE_READY;
|
||
}
|
||
break;
|
||
|
||
case SHOOT_STATE_READY:/*准备射击*/
|
||
|
||
/* 设置拨弹电机输出 */
|
||
s->output.outagl_trig =PID_Calc(&s->pid.trig,
|
||
pos,
|
||
s->feedback.trig_agl,
|
||
0,
|
||
s->dt);
|
||
s->output.outomg_trig =PID_Calc(&s->pid.trig_omg,
|
||
s->output.outagl_trig,
|
||
s->feedback.trig_rpm,
|
||
0,
|
||
s->dt);
|
||
s->output.outlpf_trig = LowPassFilter2p_Apply(&s->filter.trig.out, s->output.outomg_trig);
|
||
MOTOR_RM_SetOutput(&s->param->trig_motor_param, s->output.outlpf_trig);
|
||
|
||
/* 检查状态机 */
|
||
if(!cmd->ready)
|
||
{
|
||
Shoot_ResetCalu(s);
|
||
Shoot_ResetOutput(s);
|
||
s->running_state=SHOOT_STATE_IDLE;
|
||
}
|
||
else if(last_firecmd==false&&cmd->firecmd==true)
|
||
{
|
||
s->running_state=SHOOT_STATE_FIRE;
|
||
/* 根据模式设置待发射弹数 */
|
||
switch(s->mode)
|
||
{
|
||
case SHOOT_MODE_SINGLE:
|
||
s->anglecalu.num_to_shoot=1;
|
||
break;
|
||
case SHOOT_MODE_BURST:
|
||
//s->anglecalu.num_to_shoot=s->param->shot_burst_num;
|
||
s->anglecalu.num_to_shoot = (int)(cmd->position * 100);//最多100
|
||
break;
|
||
case SHOOT_MODE_CONTINUE:
|
||
s->anglecalu.num_to_shoot=6666;
|
||
break;
|
||
default:
|
||
s->anglecalu.num_to_shoot=0;
|
||
break;
|
||
}
|
||
}
|
||
break;
|
||
|
||
case SHOOT_STATE_FIRE:/*射击*/
|
||
|
||
Shoot_CaluTargetAngle(s, cmd);
|
||
/* 设置拨弹电机输出 */
|
||
s->output.outagl_trig =PID_Calc(&s->pid.trig,
|
||
s->target_variable.target_angle,
|
||
s->feedback.trig_agl,
|
||
0,
|
||
s->dt);
|
||
s->output.outomg_trig =PID_Calc(&s->pid.trig_omg,
|
||
s->output.outagl_trig,
|
||
s->feedback.trig_rpm,
|
||
0,
|
||
s->dt);
|
||
s->output.outlpf_trig =LowPassFilter2p_Apply(&s->filter.trig.out, s->output.outomg_trig);
|
||
MOTOR_RM_SetOutput(&s->param->trig_motor_param, s->output.outlpf_trig);
|
||
|
||
/* 检查状态机 */
|
||
if(!cmd->firecmd)
|
||
{
|
||
s->running_state=SHOOT_STATE_READY;
|
||
pos=s->feedback.trig_agl;
|
||
}
|
||
break;
|
||
|
||
default:
|
||
s->running_state=SHOOT_STATE_IDLE;
|
||
break;
|
||
}
|
||
// //**************************//
|
||
// if(last_flag == 1)
|
||
// {
|
||
// last_flag = 0;
|
||
// Shoot_ResetIntegral(&s);
|
||
// }
|
||
// //**************************//
|
||
switch(cmd->pair_state)
|
||
{
|
||
|
||
case SHOOT_PAIR_STOP:/*熄火等待*/
|
||
for(int i=0;i<fric_num;i++)
|
||
{ /* 转速归零 */
|
||
PID_ResetIntegral(&s->pid.fric_agl[i]);
|
||
s->output.outagl_fric[i]=PID_Calc(&s->pid.fric_agl[i],0.0f,s->feedback.fric_rpm[i],0,s->dt);
|
||
s->output.out_fric[i]=s->output.outagl_fric[i];
|
||
s->output.lpfout_fric[i] = LowPassFilter2p_Apply(&s->filter.fric.out[i], s->output.out_fric[i]);
|
||
MOTOR_RM_SetOutput(&s->param->fric_motor_param[i].param, s->output.lpfout_fric[i]);
|
||
}
|
||
break;
|
||
|
||
case SHOOT_PAIR_CW:/*正转*/
|
||
case SHOOT_PAIR_CCW:/*反转*/
|
||
for(int i=0;i<fric_num;i++)
|
||
{
|
||
float target_agl;
|
||
if(cmd->pair_state == SHOOT_PAIR_CW)
|
||
target_agl= s->target_variable.target_pair_angle[i];
|
||
else
|
||
target_agl=-(s->target_variable.target_pair_angle[i]);
|
||
/* 计算跟随输出、计算修正输出 */
|
||
s->output.outagl_fric[i]=PID_Calc(&s->pid.fric_agl[i],
|
||
target_agl,
|
||
s->feedback.fric_agl[i],
|
||
0,
|
||
s->dt);
|
||
s->output.outomg_fric[i]=PID_Calc(&s->pid.fric_omg[i],
|
||
s->output.outagl_fric[i],
|
||
s->feedback.fric_rpm[i],
|
||
0,
|
||
s->dt);
|
||
s->output.out_fric[i] = s->output.outomg_fric[i];
|
||
/* 滤波 */
|
||
s->output.lpfout_fric[i] = LowPassFilter2p_Apply(&s->filter.fric.out[i], s->output.out_fric[i]);
|
||
/* 设置输出 */
|
||
MOTOR_RM_SetOutput(&s->param->fric_motor_param[i].param, s->output.lpfout_fric[i]);
|
||
}
|
||
break;
|
||
|
||
default:
|
||
cmd->pair_state = SHOOT_PAIR_STOP;
|
||
break;
|
||
}
|
||
}
|
||
/* 输出 */
|
||
//注意ID，目前用了can1 id是1 2 3
|
||
MOTOR_RM_Ctrl(&s->param->trig_motor_param);
|
||
last_firecmd = cmd->firecmd;
|
||
return SHOOT_OK;
|
||
}
|
||
|
||
/**
|
||
* \brief 射击模块堵塞检测状态机（可在初始化中开关）
|
||
*
|
||
* \param s 包含射击数据的结构体
|
||
* \param cmd 包含射击指令的结构体
|
||
*
|
||
* \return 函数运行结果
|
||
*/
|
||
int8_t Shoot_JamDetectionFSM(Shoot_t *s, Shoot_CMD_t *cmd)
|
||
{
|
||
if (s == NULL) {
|
||
return SHOOT_ERR_NULL; // 参数错误
|
||
}
|
||
if(s->param->jam_enable){
|
||
switch (s->jamdetection.jamfsm_state) {
|
||
case SHOOT_JAMFSM_STATE_NORMAL:/* 正常运行 */
|
||
/* 检测电流是否超过阈值 */
|
||
if (s->feedback.trig.feedback.torque_current/1000.0f > s->param->jam_threshold) {
|
||
s->jamdetection.jamfsm_state = SHOOT_JAMFSM_STATE_SUSPECTED;
|
||
s->jamdetection.jam_last_time = s->now; /* 记录怀疑开始时间 */
|
||
}
|
||
/* 正常运行射击状态机 */
|
||
Shoot_RunningFSM(s, cmd);
|
||
break;
|
||
|
||
case SHOOT_JAMFSM_STATE_SUSPECTED:/* 怀疑堵塞 */
|
||
/* 检测电流是否低于阈值 */
|
||
if (s->feedback.trig.feedback.torque_current/1000.0f < s->param->jam_threshold) {
|
||
s->jamdetection.jamfsm_state = SHOOT_JAMFSM_STATE_NORMAL;
|
||
break;
|
||
}
|
||
/* 检测高阈值状态是否超过设定怀疑时间 */
|
||
else if ((s->now - s->jamdetection.jam_last_time) >= s->param->jam_suspected_time) {
|
||
s->jamdetection.jam_detected =true;
|
||
s->jamdetection.jamfsm_state = SHOOT_JAMFSM_STATE_CONFIRMED;
|
||
break;
|
||
}
|
||
/* 正常运行射击状态机 */
|
||
Shoot_RunningFSM(s, cmd);
|
||
break;
|
||
|
||
case SHOOT_JAMFSM_STATE_CONFIRMED:/* 确认堵塞 */
|
||
/* 清空待发射弹 */
|
||
s->anglecalu.num_to_shoot=0;
|
||
/* 修改拨弹盘目标角度 */
|
||
s->target_variable.target_angle = s->feedback.trig_agl-(M_2PI/s->param->num_trig_tooth);
|
||
/* 切换状态 */
|
||
s->jamdetection.jamfsm_state = SHOOT_JAMFSM_STATE_DEAL;
|
||
/* 记录处理开始时间 */
|
||
s->jamdetection.jam_last_time = s->now;
|
||
|
||
case SHOOT_JAMFSM_STATE_DEAL:/* 堵塞处理 */
|
||
/* 正常运行射击状态机 */
|
||
Shoot_RunningFSM(s, cmd);
|
||
/* 给予0.3秒响应时间并检测电流小于20A，认为堵塞已解除 */
|
||
if ((s->now - s->jamdetection.jam_last_time)>=0.3f&&s->feedback.trig.feedback.torque_current/1000.0f < 20.0f) {
|
||
s->jamdetection.jamfsm_state = SHOOT_JAMFSM_STATE_NORMAL;
|
||
}
|
||
break;
|
||
|
||
default:
|
||
s->jamdetection.jamfsm_state = SHOOT_JAMFSM_STATE_NORMAL;
|
||
break;
|
||
}
|
||
}
|
||
else{
|
||
s->jamdetection.jamfsm_state = SHOOT_JAMFSM_STATE_NORMAL;
|
||
s->jamdetection.jam_detected = false;
|
||
Shoot_RunningFSM(s, cmd);
|
||
}
|
||
|
||
return SHOOT_OK;
|
||
}
|
||
/* Exported functions ------------------------------------------------------- */
|
||
/**
|
||
* \brief 初始化射击模块
|
||
*
|
||
* \param s 包含射击数据的结构体
|
||
* \param param 包含射击参数的结构体
|
||
* \param target_freq 控制循环频率，单位Hz
|
||
*
|
||
* \return 函数运行结果
|
||
*/
|
||
int8_t Shoot_Init(Shoot_t *s, Shoot_Params_t *param, float target_freq)
|
||
{
|
||
if (s == NULL || param == NULL || target_freq <= 0.0f) {
|
||
return SHOOT_ERR_NULL; // 参数错误
|
||
}
|
||
uint8_t fric_num = param->fric_num;
|
||
|
||
/* 分配内存 */
|
||
s->param=param;
|
||
s->output.outagl_fric = (float *) BSP_Malloc(fric_num * sizeof(float));
|
||
s->output.outomg_fric = (float *) BSP_Malloc(fric_num * sizeof(float));
|
||
s->output.out_fric = (float *) BSP_Malloc(fric_num * sizeof(float));
|
||
s->output.lpfout_fric = (float *) BSP_Malloc(fric_num * sizeof(float));
|
||
s->feedback.fric = (MOTOR_RM_t *) BSP_Malloc(fric_num * sizeof(MOTOR_RM_t));
|
||
s->feedback.fric_agl = (float *) BSP_Malloc(fric_num * sizeof(float));
|
||
s->feedback.fil_fric_rpm = (float *) BSP_Malloc(fric_num * sizeof(float));
|
||
s->feedback.fric_rpm = (float *) BSP_Malloc(fric_num * sizeof(float));
|
||
s->pid.fric_agl = (KPID_t *) BSP_Malloc(fric_num * sizeof(KPID_t));
|
||
s->pid.fric_omg = (KPID_t *) BSP_Malloc(fric_num * sizeof(KPID_t));
|
||
s->filter.fric.in = (LowPassFilter2p_t *)BSP_Malloc(fric_num * sizeof(LowPassFilter2p_t));
|
||
s->filter.fric.out = (LowPassFilter2p_t *)BSP_Malloc(fric_num * sizeof(LowPassFilter2p_t));
|
||
|
||
/* 内存分配失败 */
|
||
if (s->feedback.fric == NULL || s->feedback.fil_fric_rpm == NULL || s->feedback.fric_rpm == NULL ||
|
||
s->output.outagl_fric == NULL || s->output.outomg_fric == NULL || s->output.out_fric == NULL ||
|
||
s->output.lpfout_fric == NULL || s->param->fric_motor_param == NULL || s->pid.fric_agl == NULL ||
|
||
s->pid.fric_omg == NULL || s->filter.fric.in == NULL || s->filter.fric.out == NULL) {
|
||
BSP_Free(s->feedback.fric);
|
||
BSP_Free(s->feedback.fil_fric_rpm);
|
||
BSP_Free(s->feedback.fric_rpm);
|
||
BSP_Free(s->output.outagl_fric);
|
||
BSP_Free(s->output.outomg_fric);
|
||
BSP_Free(s->output.out_fric);
|
||
BSP_Free(s->output.lpfout_fric);
|
||
BSP_Free(s->param->fric_motor_param);
|
||
BSP_Free(s->pid.fric_agl);
|
||
BSP_Free(s->pid.fric_omg);
|
||
BSP_Free(s->filter.fric.in);
|
||
BSP_Free(s->filter.fric.out);
|
||
return SHOOT_ERR_MALLOC;}
|
||
|
||
BSP_CAN_Init();
|
||
/* 初始化摩擦轮PID和滤波器 */
|
||
for(int i=0;i<fric_num;i++){
|
||
MOTOR_RM_Register(¶m->fric_motor_param[i].param);
|
||
s->feedback.fric[i].param = param->fric_motor_param[i].param;
|
||
PID_Init(&s->pid.fric_agl[i], KPID_MODE_CALC_D, target_freq,¶m->fric_agl);
|
||
PID_Init(&s->pid.fric_omg[i], KPID_MODE_CALC_D, target_freq,¶m->fric_omg);
|
||
LowPassFilter2p_Init(&s->filter.fric.in[i], target_freq, s->param->filter.fric.in);
|
||
LowPassFilter2p_Init(&s->filter.fric.out[i], target_freq, s->param->filter.fric.out);
|
||
}
|
||
/* 初始化拨弹PID和滤波器 */
|
||
MOTOR_RM_Register(¶m->trig_motor_param);
|
||
switch(s->param->trig_motor_param.module)
|
||
{
|
||
case MOTOR_M3508:
|
||
PID_Init(&s->pid.trig, KPID_MODE_CALC_D, target_freq,¶m->trig_3508);
|
||
PID_Init(&s->pid.trig_omg, KPID_MODE_CALC_D, target_freq,¶m->trig_omg_3508);
|
||
break;
|
||
case MOTOR_M2006:
|
||
PID_Init(&s->pid.trig, KPID_MODE_CALC_D, target_freq,¶m->trig_2006);
|
||
PID_Init(&s->pid.trig_omg, KPID_MODE_CALC_D, target_freq,¶m->trig_omg_2006);
|
||
break;
|
||
default:
|
||
return SHOOT_ERR_MOTOR;
|
||
break;
|
||
}
|
||
LowPassFilter2p_Init(&s->filter.trig.in, target_freq, s->param->filter.trig.in);
|
||
LowPassFilter2p_Init(&s->filter.trig.out, target_freq, s->param->filter.trig.out);
|
||
|
||
/* 归零变量 */
|
||
memset(&s->anglecalu,0,sizeof(s->anglecalu));
|
||
return SHOOT_OK;
|
||
}
|
||
|
||
/**
|
||
* \brief 射击模块控制主函数
|
||
*
|
||
* \param s 包含射击数据的结构体
|
||
* \param cmd 包含射击指令的结构体
|
||
*
|
||
* \return 函数运行结果
|
||
*/
|
||
int8_t Shoot_Control(Shoot_t *s, Shoot_CMD_t *cmd)
|
||
{
|
||
if (s == NULL || cmd == NULL) {
|
||
return SHOOT_ERR_NULL; // 参数错误
|
||
}
|
||
s->now =BSP_TIME_Get_us() / 1000000.0f;
|
||
s->dt =(BSP_TIME_Get_us() - s->lask_wakeup) / 1000000.0f;
|
||
s->lask_wakeup =BSP_TIME_Get_us();
|
||
s->online =cmd->online;
|
||
Shoot_CaluTargetRPM(s,cmd);
|
||
Shoot_JamDetectionFSM(s, cmd);
|
||
// Shoot_CalufeedbackRPM(s);
|
||
return SHOOT_OK;
|
||
}
|
||
|
||
|
||
|
||
|
||
|