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

添加发射

Browse Source
This commit is contained in:
Robofish 2026-01-10 20:48:14 +08:00
parent 5ffc6504b8
commit 20b386a53b
9 changed files with 772 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
CMakeLists.txt
View File

@ -75,6 +75,10 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
User/device/motor_lz.c User/device/motor_lz.c
User/device/motor_rm.c User/device/motor_rm.c
# User/module sources
User/module/config.c
User/module/shoot.c
# User/task sources # User/task sources
User/task/atti_esit.c User/task/atti_esit.c
User/task/blink.c User/task/blink.c

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

@ -0,0 +1,112 @@
/*
* 配置相关
*/
/* Includes ----------------------------------------------------------------- */
#include "module/config.h"
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* Exported variables ------------------------------------------------------- */
/**
* @brief 机器人参数配置
* @note 在此配置机器人参数
*/
Config_RobotParam_t robot_config = {
/* USER CODE BEGIN robot_config */
.shoot_param = {
.trig_step_angle=M_2PI/8,
.shot_delay_time=0.05f,
.shot_burst_num=1,
.fric_motor_param[0] = {
.can = BSP_CAN_2,
.id = 0x201,
.module = MOTOR_M3508,
.reverse = true,
.gear=false,
},
.fric_motor_param[1] = {
.can = BSP_CAN_2,
.id = 0x202,
.module = MOTOR_M3508,
.reverse = false,
.gear=false,
},
.trig_motor_param = {
.can = BSP_CAN_1,
.id = 0x201,
.module = MOTOR_M2006,
.reverse = true,
.gear=true,
},
.fric_follow = {
.k=1.0f,
.p=1.8f,
.i=0.0f,
.d=0.0f,
.i_limit=0.0f,
.out_limit=0.9f,
.d_cutoff_freq=30.0f,
.range=-1.0f,
},
.fric_err = {
.k=1.0f,
.p=4.0f,
.i=0.4f,
.d=0.04f,
.i_limit=0.25f,
.out_limit=0.25f,
.d_cutoff_freq=40.0f,
.range=-1.0f,
},
.trig_omg = {
.k=1.0f,
.p=1.5f,
.i=0.3f,
.d=0.5f,
.i_limit=0.2f,
.out_limit=0.9f,
.d_cutoff_freq=-1.0f,
.range=-1.0f,
},
.trig = {
.k=1.0f,
.p=1.0f,
.i=0.1f,
.d=0.05f,
.i_limit=0.8f,
.out_limit=0.5f,
.d_cutoff_freq=-1.0f,
.range=M_2PI,
},
.filter.fric = {
.in = 30.0f,
.out = 30.0f,
},
.filter.trig = {
.in = 30.0f,
.out = 30.0f,
},
},
/* USER CODE END robot_config */
};
/* Private function prototypes ---------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
/**
* @brief 获取机器人配置参数
* @return 机器人配置参数指针
*/
Config_RobotParam_t* Config_GetRobotParam(void) {
return &robot_config;
}

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

@ -0,0 +1,35 @@
/*
* 配置相关
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#include <stdbool.h>
#include "module/shoot.h"
/**
* @brief 机器人参数配置结构体
* @note 在此添加您的配置参数
*/
typedef struct {
/* USER CODE BEGIN Config_RobotParam */
Shoot_Params_t shoot_param;
/* USER CODE END Config_RobotParam */
} Config_RobotParam_t;
/* Exported functions prototypes -------------------------------------------- */
/**
* @brief 获取机器人配置参数
* @return 机器人配置参数指针
*/
Config_RobotParam_t* Config_GetRobotParam(void);
#ifdef __cplusplus
}
#endif

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

@ -0,0 +1,305 @@
#include "shoot.h"
#include <string.h>
#include "component/filter.h"
#include "component/user_math.h"
#include <math.h>
#include "bsp/time.h"
static bool last_firecmd;
static inline void ScaleSumTo1(float *a, float *b) {
float sum = *a + *b;
if (sum > 1.0f) {
float scale = 1.0f / sum;
*a *= scale;
*b *= scale;
}
}
int8_t Shoot_SetMode(Shoot_t *s, Shoot_Mode_t mode)
{
if (s == NULL) {
return -1; // 参数错误
}
s->mode=mode;
return 0;
}
int8_t Shoot_ResetIntegral(Shoot_t *s)
{
if (s == NULL) {
return -1; // 参数错误
}
for(int i=0;i<SHOOT_FRIC_NUM;i++)
{
PID_ResetIntegral(&s->pid.fric_follow[i]);
PID_ResetIntegral(&s->pid.fric_err[i]);
}
PID_ResetIntegral(&s->pid.trig);
PID_ResetIntegral(&s->pid.trig_omg);
return 0;
}
int8_t Shoot_ResetCalu(Shoot_t *s)
{
if (s == NULL) {
return -1; // 参数错误
}
for(int i=0;i<SHOOT_FRIC_NUM;i++)
{
PID_Reset(&s->pid.fric_follow[i]);
PID_Reset(&s->pid.fric_err[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 0;
}
int8_t Shoot_ResetOutput(Shoot_t *s)
{
if (s == NULL) {
return -1; // 参数错误
}
for(int i=0;i<SHOOT_FRIC_NUM;i++)
{
s->output.out_follow[i]=0.0f;
s->output.out_err[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 0;
}
int8_t Shoot_CaluTargetRPM(Shoot_t *s, float target_speed)
{
if (s == NULL) {
return -1; // 参数错误
}
s->target_variable.target_rpm=4000.0f/MAX_FRIC_RPM;
return 0;
}
/**
* \brief 根据发射弹丸数量及发射频率计算拨弹电机目标角度
*
* \param s 包含发射数据的结构体
* \param num 需要发射的弹丸数量
*/
int8_t Shoot_CaluTargetAngle(Shoot_t *s, Shoot_CMD_t *cmd)
{
if (s == NULL || s->shoot_Anglecalu.num_to_shoot == 0) {
return -1;
}
if(s->now - s->shoot_Anglecalu.time_last_shoot >= s->param->shot_delay_time && cmd->firecmd)
{
s->shoot_Anglecalu.time_last_shoot=s->now;
s->target_variable.target_angle += s->param->trig_step_angle;
if(s->target_variable.target_angle>M_PI)s->target_variable.target_angle-=M_2PI;
else if((s->target_variable.target_angle<-M_PI))s->target_variable.target_angle+=M_2PI;
s->shoot_Anglecalu.num_to_shoot--;
}
return 0;
}
int8_t Shoot_Init(Shoot_t *s, Shoot_Params_t *param, float target_freq)
{
if (s == NULL || param == NULL || target_freq <= 0.0f) {
return -1; // 参数错误
}
s->param=param;
BSP_CAN_Init();
for(int i=0;i<SHOOT_FRIC_NUM;i++){
MOTOR_RM_Register(&param->fric_motor_param[i]);
PID_Init(&s->pid.fric_follow[i], KPID_MODE_CALC_D, target_freq,&param->fric_follow);
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);
}
MOTOR_RM_Register(&param->trig_motor_param);
PID_Init(&s->pid.trig, KPID_MODE_CALC_D, target_freq,&param->trig);
PID_Init(&s->pid.trig_omg, KPID_MODE_CALC_D, target_freq,&param->trig_omg);
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->shoot_Anglecalu,0,sizeof(s->shoot_Anglecalu));
memset(&s->output,0,sizeof(s->output));
s->target_variable.target_rpm=5000.0f;
return 0;
}
int8_t Shoot_UpdateFeedback(Shoot_t *s)
{
if (s == NULL) {
return -1; // 参数错误
}
float rpm_sum=0.0f;
for(int i = 0; i < SHOOT_FRIC_NUM; i++) {
/* 更新摩擦电机反馈 */
MOTOR_RM_Update(&s->param->fric_motor_param[i]);
MOTOR_RM_t *motor_fed = MOTOR_RM_GetMotor(&s->param->fric_motor_param[i]);
if(motor_fed!=NULL)
{
s->feedback.fric[i]=motor_fed->motor.feedback;
}
/* 滤波反馈rpm */
s->feedback.fil_fric_rpm[i] = LowPassFilter2p_Apply(&s->filter.fric.in[i], s->feedback.fric[i].rotor_speed);
/* 归一化rpm */
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;
/* 计算平均rpm */
rpm_sum+=s->feedback.fric_rpm[i];
}
s->feedback.fric_avgrpm=rpm_sum/SHOOT_FRIC_NUM;
/* 更新拨弹电机反馈 */
MOTOR_RM_Update(&s->param->trig_motor_param);
MOTOR_RM_t *motor_fed = MOTOR_RM_GetMotor(&s->param->trig_motor_param);
if(motor_fed!=NULL)
{
s->feedback.trig=motor_fed->feedback;
}
/* 将多圈角度归化到单圈 (-M_PI, M_PI) */
s->feedback.trig_angle_cicle = s->feedback.trig.rotor_abs_angle;
s->feedback.trig_angle_cicle = fmod(s->feedback.trig_angle_cicle, M_2PI); // 将角度限制在 [-2π, 2π]
if (s->feedback.trig_angle_cicle > M_PI) {
s->feedback.trig_angle_cicle -= M_2PI; // 调整到 [-π, π]
}else if (s->feedback.trig_angle_cicle < -M_PI) {
s->feedback.trig_angle_cicle += M_2PI; // 调整到 [-π, π]
}
s->feedback.fil_trig_rpm = LowPassFilter2p_Apply(&s->filter.trig.in, s->feedback.trig.rotor_speed);
s->feedback.trig_rpm = s->feedback.trig.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 0;
}
int8_t Shoot_Control(Shoot_t *s, Shoot_CMD_t *cmd)
{
if (s == NULL || cmd == NULL) {
return -1; // 参数错误
}
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;
if(!s->online /*|| s->mode==SHOOT_MODE_SAFE*/){
for(int i=0;i<SHOOT_FRIC_NUM;i++)
{
MOTOR_RM_Relax(&s->param->fric_motor_param[i]);
}
MOTOR_RM_Relax(&s->param->trig_motor_param);
}
else{
switch(s->running_state)
{
case SHOOT_STATE_IDLE:/*熄火等待*/
for(int i=0;i<SHOOT_FRIC_NUM;i++)
{
PID_ResetIntegral(&s->pid.fric_follow[i]);
s->output.out_follow[i]=PID_Calc(&s->pid.fric_follow[i],0.0f,s->feedback.fric_rpm[i],0,s->dt);
s->output.out_fric[i]=s->output.out_follow[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], s->output.lpfout_fric[i]);
}
s->output.outagl_trig =PID_Calc(&s->pid.trig,s->target_variable.target_angle,s->feedback.trig_angle_cicle,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:/*准备射击*/
for(int i=0;i<SHOOT_FRIC_NUM;i++)
{ /* 计算跟随输出、计算修正输出 */
s->output.out_follow[i]=PID_Calc(&s->pid.fric_follow[i],s->target_variable.target_rpm/MAX_FRIC_RPM,s->feedback.fric_rpm[i],0,s->dt);
s->output.out_err[i]=PID_Calc(&s->pid.fric_err[i],s->feedback.fric_avgrpm,s->feedback.fric_rpm[i],0,s->dt);
/* 按比例缩放并加和输出 */
ScaleSumTo1(&s->output.out_follow[i], &s->output.out_err[i]);
s->output.out_fric[i]=s->output.out_follow[i]+s->output.out_err[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], s->output.lpfout_fric[i]);
}
/* 拨弹电机输出 */
s->output.outagl_trig =PID_Calc(&s->pid.trig,s->target_variable.target_angle,s->feedback.trig_angle_cicle,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)
{
Shoot_ResetCalu(s);
Shoot_ResetOutput(s);
s->running_state=SHOOT_STATE_FIRE;
s->shoot_Anglecalu.num_to_shoot+=s->param->shot_burst_num;
}
break;
case SHOOT_STATE_FIRE:
Shoot_CaluTargetAngle(s, cmd);
for(int i=0;i<SHOOT_FRIC_NUM;i++)
{
s->output.out_follow[i]=PID_Calc(&s->pid.fric_follow[i],s->target_variable.target_rpm/MAX_FRIC_RPM,s->feedback.fric_rpm[i],0,s->dt);
s->output.out_err[i]=PID_Calc(&s->pid.fric_err[i],s->feedback.fric_avgrpm,s->feedback.fric_rpm[i],0,s->dt);
ScaleSumTo1(&s->output.out_follow[i], &s->output.out_err[i]);
s->output.out_fric[i]=s->output.out_follow[i]+s->output.out_err[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], s->output.lpfout_fric[i]);
}
s->output.outagl_trig =PID_Calc(&s->pid.trig,s->target_variable.target_angle,s->feedback.trig_angle_cicle,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)
{
Shoot_ResetCalu(s);
Shoot_ResetOutput(s);
s->running_state=SHOOT_STATE_READY;
}
break;
default:
s->running_state=SHOOT_STATE_IDLE;
break;
}
}
MOTOR_RM_Ctrl(&s->param->fric_motor_param[0]);
MOTOR_RM_Ctrl(&s->param->trig_motor_param);
last_firecmd = cmd->firecmd;
return 0;
}

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

@ -0,0 +1,200 @@
/*
* 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 (-1) /* 运行时发现了其他错误 */
#define SHOOT_ERR_NULL (-2) /* 运行时发现NULL指针 */
#define SHOOT_ERR_MODE (-3) /* 运行时配置了错误的CMD_ChassisMode_t */
#define SHOOT_ERR_TYPE (-4) /* 运行时配置了错误的Chassis_Type_t */
#define SHOOT_FRIC_NUM (2) /* 摩擦轮数量 */
#define MAX_FRIC_RPM 7000.0f
#define MAX_TRIG_RPM 5000.0f
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef enum {
SHOOT_STATE_IDLE = 0, // 熄火
SHOOT_STATE_READY, // 准备射击
SHOOT_STATE_FIRE // 射击
} Shoot_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_Feedback_t fric[SHOOT_FRIC_NUM]; /* 摩擦轮电机反馈 */
MOTOR_Feedback_t trig; /* 拨弹电机反馈 */
float fil_fric_rpm[SHOOT_FRIC_NUM]; /* 滤波后的摩擦轮转速 */
float fil_trig_rpm; /* 滤波后的拨弹电机转速*/
float trig_angle_cicle; /* 拨弹电机减速输出轴单圈角度（0~M_2PI） */
float fric_rpm[SHOOT_FRIC_NUM]; /* 归一化摩擦轮转速 */
float fric_avgrpm; /* 归一化摩擦轮平均转速*/
float trig_rpm; /* 归一化拨弹电机转速*/
}Shoot_Feedback_t;
typedef struct{
float time_last_shoot;
uint8_t num_to_shoot;
uint8_t num_shooted;
}Shoot_AngleCalu_t;
typedef struct {
float out_follow[SHOOT_FRIC_NUM];
float out_err[SHOOT_FRIC_NUM];
float out_fric[SHOOT_FRIC_NUM];
float lpfout_fric[SHOOT_FRIC_NUM];
float outagl_trig;
float outomg_trig;
float outlpf_trig;
}Shoot_Output_t;
/* 底盘参数的结构体，包含所有初始化用的参数，通常是const，存好几组 */
typedef struct {
float trig_step_angle; /* 每发弹丸拨弹电机转动的角度 */
float shot_delay_time; /* 射击间隔时间，单位秒 */
uint8_t shot_burst_num; /* 多发模式下一次射击的发数 */
MOTOR_RM_Param_t fric_motor_param[SHOOT_FRIC_NUM];
MOTOR_RM_Param_t trig_motor_param;
KPID_Params_t fric_follow; /* 摩擦轮电机PID控制参数，用于跟随目标速度 */
KPID_Params_t fric_err; /* 摩擦轮电机PID控制参数，用于消除转速误差 */
KPID_Params_t trig; /* 拨弹电机PID控制参数 */
KPID_Params_t trig_omg; /* 拨弹电机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_State_t running_state; /* 运行状态机 */
Shoot_Mode_t mode;
/* 反馈信息 */
Shoot_Feedback_t feedback;
/* 控制信息*/
Shoot_AngleCalu_t shoot_Anglecalu;
Shoot_Output_t output;
/* 目标控制量 */
struct {
float target_rpm; /* 目标摩擦轮转速 */
float target_angle; /* 目标拨弹位置 */
}target_variable;
/* 反馈控制用的PID */
struct {
KPID_t fric_follow[SHOOT_FRIC_NUM]; /* */
KPID_t fric_err[SHOOT_FRIC_NUM]; /* */
KPID_t trig;
KPID_t trig_omg;
} pid;
/* 滤波器 */
struct {
struct{
LowPassFilter2p_t in[SHOOT_FRIC_NUM]; /* 反馈值滤波器 */
LowPassFilter2p_t out[SHOOT_FRIC_NUM]; /* 输出值滤波器 */
}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 包含发射数据的结构体
*
* \return 函数运行结果
*/
int8_t Shoot_UpdateFeedback(Shoot_t *s);
/**
* \brief 初始化发射
*
* \param s 包含发射数据的结构体
* \param cmd 包含发射命令的结构体
*
* \return 函数运行结果
*/
int8_t Shoot_Control(Shoot_t *s, Shoot_CMD_t *cmd);
#ifdef __cplusplus
}
#endif

15
User/task/ctrl_shoot.c
View File

@ -4,11 +4,11 @@
*/ */
/* Includes ----------------------------------------------------------------- */ /* Includes ----------------------------------------------------------------- */
#include "device/motor_rm.h"
#include "task/user_task.h" #include "task/user_task.h"
/* USER INCLUDE BEGIN */ /* USER INCLUDE BEGIN */
#include "bsp/can.h" #include "module/shoot.h"
#include "device/motor.h" #include "module/config.h"
#include "device/motor_dm.h"
/* USER INCLUDE END */ /* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */ /* Private typedef ---------------------------------------------------------- */
@ -16,7 +16,8 @@
/* Private macro ------------------------------------------------------------ */ /* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */ /* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN */ /* USER STRUCT BEGIN */
Shoot_t shoot;
Shoot_CMD_t shoot_cmd;
/* USER STRUCT END */ /* USER STRUCT END */
/* Private function --------------------------------------------------------- */ /* Private function --------------------------------------------------------- */
@ -32,13 +33,17 @@ void Task_ctrl_shoot(void *argument) {
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
/* USER CODE INIT BEGIN */ /* USER CODE INIT BEGIN */
Shoot_Init(&shoot,&Config_GetRobotParam()->shoot_param,CTRL_SHOOT_FREQ);
/* USER CODE INIT END */ /* USER CODE INIT END */
while (1) { while (1) {
tick += delay_tick; /* 计算下一个唤醒时刻 */ tick += delay_tick; /* 计算下一个唤醒时刻 */
/* USER CODE BEGIN */ /* USER CODE BEGIN */
osMessageQueueGet(task_runtime.msgq.shoot.shoot_cmd, &shoot_cmd, NULL, 0);
Shoot_UpdateFeedback(&shoot);
// Shoot_Test(&shoot);
Shoot_Control(&shoot,&shoot_cmd);
/* USER CODE END */ /* USER CODE END */
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
} }

3
User/task/init.c
View File

@ -7,7 +7,7 @@
#include "task/user_task.h" #include "task/user_task.h"
/* USER INCLUDE BEGIN */ /* USER INCLUDE BEGIN */
#include "module/shoot.h"
/* USER INCLUDE END */ /* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */ /* Private typedef ---------------------------------------------------------- */
@ -41,6 +41,7 @@ void Task_Init(void *argument) {
// 创建消息队列 // 创建消息队列
/* USER MESSAGE BEGIN */ /* USER MESSAGE BEGIN */
task_runtime.msgq.user_msg= osMessageQueueNew(2u, 10, NULL); task_runtime.msgq.user_msg= osMessageQueueNew(2u, 10, NULL);
task_runtime.msgq.shoot.shoot_cmd = osMessageQueueNew(2u, sizeof(Shoot_CMD_t), NULL);
/* USER MESSAGE END */ /* USER MESSAGE END */
osKernelUnlock(); // 解锁内核 osKernelUnlock(); // 解锁内核

80
User/task/rc.c
View File

@ -7,6 +7,7 @@
#include "task/user_task.h" #include "task/user_task.h"
/* USER INCLUDE BEGIN */ /* USER INCLUDE BEGIN */
#include "device/dr16.h" #include "device/dr16.h"
#include "module/shoot.h"
/* USER INCLUDE END */ /* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */ /* Private typedef ---------------------------------------------------------- */
@ -15,6 +16,8 @@
/* Private variables -------------------------------------------------------- */ /* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN */ /* USER STRUCT BEGIN */
DR16_t dr16; DR16_t dr16;
Shoot_CMD_t for_shoot;
/* USER STRUCT END */ /* USER STRUCT END */
/* Private function --------------------------------------------------------- */ /* Private function --------------------------------------------------------- */
@ -39,9 +42,86 @@ void Task_rc(void *argument) {
DR16_StartDmaRecv(&dr16); DR16_StartDmaRecv(&dr16);
if (DR16_WaitDmaCplt(20)) { if (DR16_WaitDmaCplt(20)) {
DR16_ParseData(&dr16); DR16_ParseData(&dr16);
} else { } else {
DR16_Offline(&dr16); DR16_Offline(&dr16);
} }
// switch (dr16.data.sw_l) {
// case DR16_SW_UP:
// cmd_for_chassis.mode = CHASSIS_MODE_RELAX;
// break;
// case DR16_SW_MID:
// // cmd_for_chassis.mode = CHASSIS_MODE_RECOVER;
// cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
// break;
// case DR16_SW_DOWN:
// // cmd_for_chassis.mode = CHASSIS_MODE_ROTOR;
// // cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
// cmd_for_chassis.mode = CHASSIS_MODE_JUMP;
// break;
// default:
// cmd_for_chassis.mode = CHASSIS_MODE_RELAX;
// break;
// }
// cmd_for_chassis.move_vec.vx = dr16.data.ch_l_y;
// cmd_for_chassis.move_vec.vy = dr16.data.ch_l_x;
// cmd_for_chassis.move_vec.wz = dr16.data.ch_r_x;
// cmd_for_chassis.height = dr16.data.ch_res;
// osMessageQueueReset(
// task_runtime.msgq.chassis.cmd); // 重置消息队列，防止阻塞
// osMessageQueuePut(task_runtime.msgq.chassis.cmd, &cmd_for_chassis, 0,
// 0); // 非阻塞发送底盘控制命令
// switch (dr16.data.sw_l) {
// case DR16_SW_UP:
// cmd_for_gimbal.mode = GIMBAL_MODE_RELAX;
// cmd_for_gimbal.delta_yaw = 0.0f;
// cmd_for_gimbal.delta_pit = 0.0f;
// break;
// case DR16_SW_MID:
// cmd_for_gimbal.mode = GIMBAL_MODE_ABSOLUTE;
// cmd_for_gimbal.delta_yaw = -dr16.data.ch_r_x * 5.0f;
// cmd_for_gimbal.delta_pit = dr16.data.ch_r_y * 5.0f;
// break;
// case DR16_SW_DOWN:
// cmd_for_gimbal.mode = GIMBAL_MODE_ABSOLUTE;
// cmd_for_gimbal.delta_yaw = -dr16.data.ch_r_x * 5.0f;
// cmd_for_gimbal.delta_pit = dr16.data.ch_r_y * 5.0f;
// break;
// default:
// cmd_for_gimbal.mode = GIMBAL_MODE_RELAX;
// cmd_for_gimbal.delta_yaw = 0.0f;
// cmd_for_gimbal.delta_pit = 0.0f;
// break;
// }
// osMessageQueueReset(task_runtime.msgq.gimbal.cmd);
// osMessageQueuePut(task_runtime.msgq.gimbal.cmd, &cmd_for_gimbal, 0, 0);
for_shoot.online = dr16.header.online;
switch (dr16.data.sw_r) {
case DR16_SW_UP:
for_shoot.ready = false;
for_shoot.firecmd = false;
break;
case DR16_SW_MID:
for_shoot.ready = true;
for_shoot.firecmd = false;
break;
case DR16_SW_DOWN:
for_shoot.ready = true;
for_shoot.firecmd = true;
break;
default:
for_shoot.ready = false;
for_shoot.firecmd = false;
break;
}
osMessageQueueReset(task_runtime.msgq.shoot.shoot_cmd);
osMessageQueuePut(task_runtime.msgq.shoot.shoot_cmd, &for_shoot, 0, 0);
/* USER CODE END */ /* USER CODE END */
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
} }

23
User/task/user_task.h
View File

@ -50,6 +50,29 @@ typedef struct {
/* USER MESSAGE BEGIN */ /* USER MESSAGE BEGIN */
struct { struct {
osMessageQueueId_t user_msg; /* 用户自定义任务消息队列 */ osMessageQueueId_t user_msg; /* 用户自定义任务消息队列 */
struct {
osMessageQueueId_t imu;
osMessageQueueId_t cmd;
osMessageQueueId_t yaw;
}chassis;
struct {
osMessageQueueId_t imu;
osMessageQueueId_t cmd;
}gimbal;
struct {
osMessageQueueId_t shoot_cmd; /* 发射命令队列 */
}shoot;
struct {
osMessageQueueId_t rc;
osMessageQueueId_t ref;
osMessageQueueId_t ai;
}cmd;
struct {
osMessageQueueId_t quat;
osMessageQueueId_t move_vec;
osMessageQueueId_t eulr;
osMessageQueueId_t fire;
}ai;
} msgq; } msgq;
/* USER MESSAGE END */ /* USER MESSAGE END */