Compare commits
5 Commits
51f899bada
...
20b386a53b
| Author | SHA1 | Date | |
|---|---|---|---|
| 20b386a53b | |||
| 5ffc6504b8 | |||
| 71466bc61e | |||
| dd27f0ba8b | |||
| e0dd2ed84a |
@ -74,10 +74,19 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
|
||||
User/device/motor_lk.c
|
||||
User/device/motor_lz.c
|
||||
User/device/motor_rm.c
|
||||
|
||||
# User/module sources
|
||||
User/module/config.c
|
||||
User/module/shoot.c
|
||||
|
||||
# User/task sources
|
||||
User/task/atti_esit.c
|
||||
User/task/blink.c
|
||||
User/task/ctrl_chassis.c
|
||||
User/task/ctrl_gimbal.c
|
||||
User/task/ctrl_shoot.c
|
||||
User/task/init.c
|
||||
User/task/monitor.c
|
||||
User/task/rc.c
|
||||
User/task/user_task.c
|
||||
)
|
||||
|
||||
@ -67,6 +67,8 @@ void Error_Handler(void);
|
||||
#define ACCL_CS_GPIO_Port GPIOC
|
||||
#define GYRO_CS_Pin GPIO_PIN_3
|
||||
#define GYRO_CS_GPIO_Port GPIOC
|
||||
#define WS2812_Pin GPIO_PIN_7
|
||||
#define WS2812_GPIO_Port GPIOA
|
||||
#define DCMI_PWDN_Pin GPIO_PIN_5
|
||||
#define DCMI_PWDN_GPIO_Port GPIOC
|
||||
#define IMU_HEAT_Pin GPIO_PIN_1
|
||||
|
||||
@ -180,6 +180,10 @@ void MX_TIM3_Init(void)
|
||||
sConfigOC.Pulse = 0;
|
||||
sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
|
||||
sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
|
||||
if (HAL_TIM_PWM_ConfigChannel(&htim3, &sConfigOC, TIM_CHANNEL_2) != HAL_OK)
|
||||
{
|
||||
Error_Handler();
|
||||
}
|
||||
if (HAL_TIM_PWM_ConfigChannel(&htim3, &sConfigOC, TIM_CHANNEL_4) != HAL_OK)
|
||||
{
|
||||
Error_Handler();
|
||||
@ -336,10 +340,19 @@ void HAL_TIM_MspPostInit(TIM_HandleTypeDef* timHandle)
|
||||
|
||||
/* USER CODE END TIM3_MspPostInit 0 */
|
||||
|
||||
__HAL_RCC_GPIOA_CLK_ENABLE();
|
||||
__HAL_RCC_GPIOB_CLK_ENABLE();
|
||||
/**TIM3 GPIO Configuration
|
||||
PA7 ------> TIM3_CH2
|
||||
PB1 ------> TIM3_CH4
|
||||
*/
|
||||
GPIO_InitStruct.Pin = WS2812_Pin;
|
||||
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
|
||||
GPIO_InitStruct.Pull = GPIO_NOPULL;
|
||||
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
|
||||
GPIO_InitStruct.Alternate = GPIO_AF2_TIM3;
|
||||
HAL_GPIO_Init(WS2812_GPIO_Port, &GPIO_InitStruct);
|
||||
|
||||
GPIO_InitStruct.Pin = IMU_HEAT_Pin;
|
||||
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
|
||||
GPIO_InitStruct.Pull = GPIO_NOPULL;
|
||||
|
||||
@ -182,63 +182,64 @@ Mcu.Pin12=PA1
|
||||
Mcu.Pin13=PA2
|
||||
Mcu.Pin14=PA3
|
||||
Mcu.Pin15=PA5
|
||||
Mcu.Pin16=PC4
|
||||
Mcu.Pin17=PC5
|
||||
Mcu.Pin18=PB0
|
||||
Mcu.Pin19=PB1
|
||||
Mcu.Pin16=PA7
|
||||
Mcu.Pin17=PC4
|
||||
Mcu.Pin18=PC5
|
||||
Mcu.Pin19=PB0
|
||||
Mcu.Pin2=PC13
|
||||
Mcu.Pin20=PB2
|
||||
Mcu.Pin21=PE7
|
||||
Mcu.Pin22=PE8
|
||||
Mcu.Pin23=PE9
|
||||
Mcu.Pin24=PE10
|
||||
Mcu.Pin25=PE11
|
||||
Mcu.Pin26=PE12
|
||||
Mcu.Pin27=PE13
|
||||
Mcu.Pin28=PE15
|
||||
Mcu.Pin29=PB10
|
||||
Mcu.Pin20=PB1
|
||||
Mcu.Pin21=PB2
|
||||
Mcu.Pin22=PE7
|
||||
Mcu.Pin23=PE8
|
||||
Mcu.Pin24=PE9
|
||||
Mcu.Pin25=PE10
|
||||
Mcu.Pin26=PE11
|
||||
Mcu.Pin27=PE12
|
||||
Mcu.Pin28=PE13
|
||||
Mcu.Pin29=PE15
|
||||
Mcu.Pin3=PC14-OSC32_IN
|
||||
Mcu.Pin30=PB11
|
||||
Mcu.Pin31=PB12
|
||||
Mcu.Pin32=PB13
|
||||
Mcu.Pin33=PB14
|
||||
Mcu.Pin34=PB15
|
||||
Mcu.Pin35=PD8
|
||||
Mcu.Pin36=PD9
|
||||
Mcu.Pin37=PD10
|
||||
Mcu.Pin38=PD11
|
||||
Mcu.Pin39=PD12
|
||||
Mcu.Pin30=PB10
|
||||
Mcu.Pin31=PB11
|
||||
Mcu.Pin32=PB12
|
||||
Mcu.Pin33=PB13
|
||||
Mcu.Pin34=PB14
|
||||
Mcu.Pin35=PB15
|
||||
Mcu.Pin36=PD8
|
||||
Mcu.Pin37=PD9
|
||||
Mcu.Pin38=PD10
|
||||
Mcu.Pin39=PD11
|
||||
Mcu.Pin4=PC15-OSC32_OUT
|
||||
Mcu.Pin40=PD13
|
||||
Mcu.Pin41=PA8
|
||||
Mcu.Pin42=PA9
|
||||
Mcu.Pin43=PA10
|
||||
Mcu.Pin44=PA11
|
||||
Mcu.Pin45=PA12
|
||||
Mcu.Pin46=PA13(JTMS/SWDIO)
|
||||
Mcu.Pin47=PA14(JTCK/SWCLK)
|
||||
Mcu.Pin48=PA15(JTDI)
|
||||
Mcu.Pin49=PC12
|
||||
Mcu.Pin40=PD12
|
||||
Mcu.Pin41=PD13
|
||||
Mcu.Pin42=PA8
|
||||
Mcu.Pin43=PA9
|
||||
Mcu.Pin44=PA10
|
||||
Mcu.Pin45=PA11
|
||||
Mcu.Pin46=PA12
|
||||
Mcu.Pin47=PA13(JTMS/SWDIO)
|
||||
Mcu.Pin48=PA14(JTCK/SWCLK)
|
||||
Mcu.Pin49=PA15(JTDI)
|
||||
Mcu.Pin5=PH0-OSC_IN
|
||||
Mcu.Pin50=PD0
|
||||
Mcu.Pin51=PD1
|
||||
Mcu.Pin52=PD2
|
||||
Mcu.Pin53=PD4
|
||||
Mcu.Pin54=PD5
|
||||
Mcu.Pin55=PD6
|
||||
Mcu.Pin56=PD7
|
||||
Mcu.Pin57=PB3(JTDO/TRACESWO)
|
||||
Mcu.Pin58=PB5
|
||||
Mcu.Pin59=PB6
|
||||
Mcu.Pin50=PC12
|
||||
Mcu.Pin51=PD0
|
||||
Mcu.Pin52=PD1
|
||||
Mcu.Pin53=PD2
|
||||
Mcu.Pin54=PD4
|
||||
Mcu.Pin55=PD5
|
||||
Mcu.Pin56=PD6
|
||||
Mcu.Pin57=PD7
|
||||
Mcu.Pin58=PB3(JTDO/TRACESWO)
|
||||
Mcu.Pin59=PB5
|
||||
Mcu.Pin6=PH1-OSC_OUT
|
||||
Mcu.Pin60=VP_FREERTOS_VS_CMSIS_V2
|
||||
Mcu.Pin61=VP_OCTOSPI1_VS_octo
|
||||
Mcu.Pin62=VP_SYS_VS_tim23
|
||||
Mcu.Pin63=VP_MEMORYMAP_VS_MEMORYMAP
|
||||
Mcu.Pin60=PB6
|
||||
Mcu.Pin61=VP_FREERTOS_VS_CMSIS_V2
|
||||
Mcu.Pin62=VP_OCTOSPI1_VS_octo
|
||||
Mcu.Pin63=VP_SYS_VS_tim23
|
||||
Mcu.Pin64=VP_MEMORYMAP_VS_MEMORYMAP
|
||||
Mcu.Pin7=PC0
|
||||
Mcu.Pin8=PC1
|
||||
Mcu.Pin9=PC2_C
|
||||
Mcu.PinsNb=64
|
||||
Mcu.PinsNb=65
|
||||
Mcu.ThirdPartyNb=0
|
||||
Mcu.UserConstants=
|
||||
Mcu.UserName=STM32H723VGTx
|
||||
@ -306,6 +307,10 @@ PA3.Mode=OCTOSPI1_IOL_Port1L
|
||||
PA3.Signal=OCTOSPIM_P1_IO2
|
||||
PA5.Locked=true
|
||||
PA5.Signal=ADCx_INP19
|
||||
PA7.GPIOParameters=GPIO_Label
|
||||
PA7.GPIO_Label=WS2812
|
||||
PA7.Locked=true
|
||||
PA7.Signal=S_TIM3_CH2
|
||||
PA8.Locked=true
|
||||
PA8.Mode=Clock-out-1
|
||||
PA8.Signal=RCC_MCO_1
|
||||
@ -609,6 +614,8 @@ SH.S_TIM2_CH1_ETR.0=TIM2_CH1,PWM Generation1 CH1
|
||||
SH.S_TIM2_CH1_ETR.ConfNb=1
|
||||
SH.S_TIM2_CH3.0=TIM2_CH3,PWM Generation3 CH3
|
||||
SH.S_TIM2_CH3.ConfNb=1
|
||||
SH.S_TIM3_CH2.0=TIM3_CH2,PWM Generation2 CH2
|
||||
SH.S_TIM3_CH2.ConfNb=1
|
||||
SH.S_TIM3_CH4.0=TIM3_CH4,PWM Generation4 CH4
|
||||
SH.S_TIM3_CH4.ConfNb=1
|
||||
SPI1.BaudRatePrescaler=SPI_BAUDRATEPRESCALER_4
|
||||
@ -647,8 +654,9 @@ TIM2.Prescaler=24
|
||||
TIM2.Pulse-PWM\ Generation1\ CH1=5000
|
||||
TIM2.Pulse-PWM\ Generation3\ CH3=5000
|
||||
TIM3.AutoReloadPreload=TIM_AUTORELOAD_PRELOAD_ENABLE
|
||||
TIM3.Channel-PWM\ Generation2\ CH2=TIM_CHANNEL_2
|
||||
TIM3.Channel-PWM\ Generation4\ CH4=TIM_CHANNEL_4
|
||||
TIM3.IPParameters=Channel-PWM Generation4 CH4,Prescaler,Period,AutoReloadPreload
|
||||
TIM3.IPParameters=Channel-PWM Generation4 CH4,Prescaler,Period,AutoReloadPreload,Channel-PWM Generation2 CH2
|
||||
TIM3.Period=10000-1
|
||||
TIM3.Prescaler=24-1
|
||||
UART5.BaudRate=100000
|
||||
@ -683,3 +691,4 @@ VP_OCTOSPI1_VS_octo.Signal=OCTOSPI1_VS_octo
|
||||
VP_SYS_VS_tim23.Mode=TIM23
|
||||
VP_SYS_VS_tim23.Signal=SYS_VS_tim23
|
||||
board=custom
|
||||
rtos.0.ip=FREERTOS
|
||||
|
||||
@ -92,6 +92,10 @@ pwm:
|
||||
custom_name: TIM2_CH3
|
||||
label: TIM2_CH3
|
||||
timer: TIM2
|
||||
- channel: TIM_CHANNEL_2
|
||||
custom_name: WS2812
|
||||
label: WS2812
|
||||
timer: TIM3
|
||||
- channel: TIM_CHANNEL_4
|
||||
custom_name: IMU_HEAT
|
||||
label: IMU_HEAT
|
||||
|
||||
@ -27,6 +27,7 @@ typedef struct {
|
||||
static const BSP_PWM_Config_t PWM_Map[BSP_PWM_NUM] = {
|
||||
{&htim2, TIM_CHANNEL_1},
|
||||
{&htim2, TIM_CHANNEL_3},
|
||||
{&htim3, TIM_CHANNEL_2},
|
||||
{&htim3, TIM_CHANNEL_4},
|
||||
{&htim12, TIM_CHANNEL_2},
|
||||
{&htim1, TIM_CHANNEL_3},
|
||||
|
||||
@ -25,6 +25,7 @@ extern "C" {
|
||||
typedef enum {
|
||||
BSP_PWM_TIM2_CH1,
|
||||
BSP_PWM_TIM2_CH3,
|
||||
BSP_PWM_WS2812,
|
||||
BSP_PWM_IMU_HEAT,
|
||||
BSP_PWM_BUZZER,
|
||||
BSP_PWM_TIM1_CH3,
|
||||
|
||||
@ -8,7 +8,7 @@ bmi088:
|
||||
enabled: true
|
||||
buzzer:
|
||||
bsp_config:
|
||||
BSP_PWM_BUZZER: BSP_PWM_TIM2_CH1
|
||||
BSP_PWM_BUZZER: BSP_PWM_BUZZER
|
||||
enabled: true
|
||||
dr16:
|
||||
bsp_config:
|
||||
|
||||
112
User/module/config.c
Normal file
112
User/module/config.c
Normal 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
35
User/module/config.h
Normal 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
305
User/module/shoot.c
Normal 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(¶m->fric_motor_param[i]);
|
||||
PID_Init(&s->pid.fric_follow[i], KPID_MODE_CALC_D, target_freq,¶m->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(¶m->trig_motor_param);
|
||||
PID_Init(&s->pid.trig, KPID_MODE_CALC_D, target_freq,¶m->trig);
|
||||
PID_Init(&s->pid.trig_omg, KPID_MODE_CALC_D, target_freq,¶m->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
200
User/module/shoot.h
Normal 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
|
||||
|
||||
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
/*
|
||||
atti_esit Task
|
||||
|
||||
|
||||
*/
|
||||
|
||||
/* Includes ----------------------------------------------------------------- */
|
||||
@ -49,6 +49,7 @@ static const KPID_Params_t imu_temp_ctrl_pid_param = {
|
||||
void Task_atti_esit(void *argument) {
|
||||
(void)argument; /* 未使用argument，消除警告 */
|
||||
|
||||
|
||||
osDelay(ATTI_ESIT_INIT_DELAY); /* 延时一段时间再开启任务 */
|
||||
|
||||
/* USER CODE INIT BEGIN */
|
||||
@ -86,4 +87,5 @@ void Task_atti_esit(void *argument) {
|
||||
|
||||
/* USER CODE END */
|
||||
}
|
||||
|
||||
}
|
||||
57
User/task/blink.c
Normal file
57
User/task/blink.c
Normal file
@ -0,0 +1,57 @@
|
||||
/*
|
||||
blink Task
|
||||
|
||||
*/
|
||||
|
||||
/* Includes ----------------------------------------------------------------- */
|
||||
#include "task/user_task.h"
|
||||
/* USER INCLUDE BEGIN */
|
||||
#include "device/buzzer.h"
|
||||
#include <stdint.h>
|
||||
/* USER INCLUDE END */
|
||||
|
||||
/* Private typedef ---------------------------------------------------------- */
|
||||
/* Private define ----------------------------------------------------------- */
|
||||
/* Private macro ------------------------------------------------------------ */
|
||||
/* Private variables -------------------------------------------------------- */
|
||||
/* USER STRUCT BEGIN */
|
||||
BUZZER_t buzzer;
|
||||
static uint16_t count;
|
||||
/* USER STRUCT END */
|
||||
|
||||
/* Private function --------------------------------------------------------- */
|
||||
/* Exported functions ------------------------------------------------------- */
|
||||
void Task_blink(void *argument) {
|
||||
(void)argument; /* 未使用argument，消除警告 */
|
||||
|
||||
|
||||
/* 计算任务运行到指定频率需要等待的tick数 */
|
||||
const uint32_t delay_tick = osKernelGetTickFreq() / BLINK_FREQ;
|
||||
|
||||
osDelay(BLINK_INIT_DELAY); /* 延时一段时间再开启任务 */
|
||||
|
||||
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
|
||||
/* USER CODE INIT BEGIN */
|
||||
/* 播放启动音乐 */
|
||||
BUZZER_Init(&buzzer, BSP_PWM_BUZZER);
|
||||
BUZZER_PlayMusic(&buzzer, MUSIC_RM);
|
||||
/* USER CODE INIT END */
|
||||
|
||||
while (1) {
|
||||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||||
/* USER CODE BEGIN */
|
||||
count++;
|
||||
uint16_t phase = count % 1000;
|
||||
if (phase == 0) {
|
||||
/* 每秒开始播放C4音符 */
|
||||
BUZZER_Set(&buzzer, 261.63f, 0.5f); // C4音符频率约261.63Hz
|
||||
BUZZER_Start(&buzzer);
|
||||
} else if (phase == 50) {
|
||||
/* 播放100ms后停止 (50/500Hz = 0.1s) */
|
||||
BUZZER_Stop(&buzzer);
|
||||
}
|
||||
/* USER CODE END */
|
||||
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
|
||||
}
|
||||
|
||||
}
|
||||
@ -11,3 +11,38 @@
|
||||
function: Task_atti_esit
|
||||
name: atti_esit
|
||||
stack: 256
|
||||
- delay: 0
|
||||
description: ''
|
||||
freq_control: true
|
||||
frequency: 500.0
|
||||
function: Task_ctrl_chassis
|
||||
name: ctrl_chassis
|
||||
stack: 256
|
||||
- delay: 0
|
||||
description: ''
|
||||
freq_control: true
|
||||
frequency: 500.0
|
||||
function: Task_ctrl_gimbal
|
||||
name: ctrl_gimbal
|
||||
stack: 256
|
||||
- delay: 0
|
||||
description: ''
|
||||
freq_control: true
|
||||
frequency: 500.0
|
||||
function: Task_monitor
|
||||
name: monitor
|
||||
stack: 256
|
||||
- delay: 0
|
||||
description: ''
|
||||
freq_control: true
|
||||
frequency: 500.0
|
||||
function: Task_blink
|
||||
name: blink
|
||||
stack: 256
|
||||
- delay: 0
|
||||
description: ''
|
||||
freq_control: true
|
||||
frequency: 500.0
|
||||
function: Task_ctrl_shoot
|
||||
name: ctrl_shoot
|
||||
stack: 256
|
||||
|
||||
60
User/task/ctrl_chassis.c
Normal file
60
User/task/ctrl_chassis.c
Normal file
@ -0,0 +1,60 @@
|
||||
/*
|
||||
ctrl_chassis Task
|
||||
|
||||
*/
|
||||
|
||||
/* Includes ----------------------------------------------------------------- */
|
||||
#include "task/user_task.h"
|
||||
/* USER INCLUDE BEGIN */
|
||||
#include "bsp/can.h"
|
||||
#include "device/motor.h"
|
||||
#include "device/motor_lk.h"
|
||||
#include "device/motor_lz.h"
|
||||
/* USER INCLUDE END */
|
||||
|
||||
/* Private typedef ---------------------------------------------------------- */
|
||||
/* Private define ----------------------------------------------------------- */
|
||||
/* Private macro ------------------------------------------------------------ */
|
||||
/* Private variables -------------------------------------------------------- */
|
||||
/* USER STRUCT BEGIN */
|
||||
MOTOR_LZ_Param_t motor_lz_param = {
|
||||
.can = BSP_CAN_3,
|
||||
.motor_id = 125,
|
||||
.host_id = 0xFF,
|
||||
.module = MOTOR_LZ_RSO3,
|
||||
.reverse = false,
|
||||
.mode = MOTOR_LZ_MODE_MOTION,
|
||||
};
|
||||
|
||||
float out = 0.0f;
|
||||
/* USER STRUCT END */
|
||||
|
||||
/* Private function --------------------------------------------------------- */
|
||||
/* Exported functions ------------------------------------------------------- */
|
||||
void Task_ctrl_chassis(void *argument) {
|
||||
(void)argument; /* 未使用argument，消除警告 */
|
||||
|
||||
|
||||
/* 计算任务运行到指定频率需要等待的tick数 */
|
||||
const uint32_t delay_tick = osKernelGetTickFreq() / CTRL_CHASSIS_FREQ;
|
||||
|
||||
osDelay(CTRL_CHASSIS_INIT_DELAY); /* 延时一段时间再开启任务 */
|
||||
|
||||
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
|
||||
/* USER CODE INIT BEGIN */
|
||||
BSP_CAN_Init();
|
||||
MOTOR_LZ_Init();
|
||||
MOTOR_LZ_Register(&motor_lz_param);
|
||||
|
||||
/* USER CODE INIT END */
|
||||
|
||||
while (1) {
|
||||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||||
/* USER CODE BEGIN */
|
||||
MOTOR_LZ_Enable(&motor_lz_param);
|
||||
|
||||
/* USER CODE END */
|
||||
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
|
||||
}
|
||||
|
||||
}
|
||||
53
User/task/ctrl_gimbal.c
Normal file
53
User/task/ctrl_gimbal.c
Normal file
@ -0,0 +1,53 @@
|
||||
/*
|
||||
ctrl_gimbal Task
|
||||
|
||||
*/
|
||||
|
||||
/* Includes ----------------------------------------------------------------- */
|
||||
#include "task/user_task.h"
|
||||
/* USER INCLUDE BEGIN */
|
||||
#include "bsp/can.h"
|
||||
#include "device/motor.h"
|
||||
#include "device/motor_dm.h"
|
||||
/* USER INCLUDE END */
|
||||
|
||||
/* Private typedef ---------------------------------------------------------- */
|
||||
/* Private define ----------------------------------------------------------- */
|
||||
/* Private macro ------------------------------------------------------------ */
|
||||
/* Private variables -------------------------------------------------------- */
|
||||
/* USER STRUCT BEGIN */
|
||||
MOTOR_DM_Param_t motor_dm_yaw = {
|
||||
.can = BSP_CAN_1,
|
||||
.can_id = 0x1,
|
||||
.master_id = 0x11,
|
||||
.module = MOTOR_DM_J4310,
|
||||
.reverse = false,
|
||||
};
|
||||
/* USER STRUCT END */
|
||||
|
||||
/* Private function --------------------------------------------------------- */
|
||||
/* Exported functions ------------------------------------------------------- */
|
||||
void Task_ctrl_gimbal(void *argument) {
|
||||
(void)argument; /* 未使用argument，消除警告 */
|
||||
|
||||
|
||||
/* 计算任务运行到指定频率需要等待的tick数 */
|
||||
const uint32_t delay_tick = osKernelGetTickFreq() / CTRL_GIMBAL_FREQ;
|
||||
|
||||
osDelay(CTRL_GIMBAL_INIT_DELAY); /* 延时一段时间再开启任务 */
|
||||
|
||||
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
|
||||
/* USER CODE INIT BEGIN */
|
||||
BSP_CAN_Init();
|
||||
MOTOR_DM_Register(&motor_dm_yaw);
|
||||
/* USER CODE INIT END */
|
||||
|
||||
while (1) {
|
||||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||||
/* USER CODE BEGIN */
|
||||
MOTOR_DM_Enable(&motor_dm_yaw);
|
||||
/* USER CODE END */
|
||||
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
|
||||
}
|
||||
|
||||
}
|
||||
51
User/task/ctrl_shoot.c
Normal file
51
User/task/ctrl_shoot.c
Normal file
@ -0,0 +1,51 @@
|
||||
/*
|
||||
ctrl_shoot Task
|
||||
|
||||
*/
|
||||
|
||||
/* Includes ----------------------------------------------------------------- */
|
||||
#include "device/motor_rm.h"
|
||||
#include "task/user_task.h"
|
||||
/* USER INCLUDE BEGIN */
|
||||
#include "module/shoot.h"
|
||||
#include "module/config.h"
|
||||
/* USER INCLUDE END */
|
||||
|
||||
/* Private typedef ---------------------------------------------------------- */
|
||||
/* Private define ----------------------------------------------------------- */
|
||||
/* Private macro ------------------------------------------------------------ */
|
||||
/* Private variables -------------------------------------------------------- */
|
||||
/* USER STRUCT BEGIN */
|
||||
Shoot_t shoot;
|
||||
Shoot_CMD_t shoot_cmd;
|
||||
/* USER STRUCT END */
|
||||
|
||||
/* Private function --------------------------------------------------------- */
|
||||
/* Exported functions ------------------------------------------------------- */
|
||||
void Task_ctrl_shoot(void *argument) {
|
||||
(void)argument; /* 未使用argument，消除警告 */
|
||||
|
||||
|
||||
/* 计算任务运行到指定频率需要等待的tick数 */
|
||||
const uint32_t delay_tick = osKernelGetTickFreq() / CTRL_SHOOT_FREQ;
|
||||
|
||||
osDelay(CTRL_SHOOT_INIT_DELAY); /* 延时一段时间再开启任务 */
|
||||
|
||||
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
|
||||
/* USER CODE INIT BEGIN */
|
||||
Shoot_Init(&shoot,&Config_GetRobotParam()->shoot_param,CTRL_SHOOT_FREQ);
|
||||
|
||||
/* USER CODE INIT END */
|
||||
|
||||
while (1) {
|
||||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||||
/* 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 */
|
||||
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
|
||||
}
|
||||
|
||||
}
|
||||
@ -7,7 +7,7 @@
|
||||
#include "task/user_task.h"
|
||||
|
||||
/* USER INCLUDE BEGIN */
|
||||
|
||||
#include "module/shoot.h"
|
||||
/* USER INCLUDE END */
|
||||
|
||||
/* Private typedef ---------------------------------------------------------- */
|
||||
@ -32,10 +32,16 @@ void Task_Init(void *argument) {
|
||||
/* 创建任务线程 */
|
||||
task_runtime.thread.rc = osThreadNew(Task_rc, NULL, &attr_rc);
|
||||
task_runtime.thread.atti_esit = osThreadNew(Task_atti_esit, NULL, &attr_atti_esit);
|
||||
task_runtime.thread.ctrl_chassis = osThreadNew(Task_ctrl_chassis, NULL, &attr_ctrl_chassis);
|
||||
task_runtime.thread.ctrl_gimbal = osThreadNew(Task_ctrl_gimbal, NULL, &attr_ctrl_gimbal);
|
||||
task_runtime.thread.monitor = osThreadNew(Task_monitor, NULL, &attr_monitor);
|
||||
task_runtime.thread.blink = osThreadNew(Task_blink, NULL, &attr_blink);
|
||||
task_runtime.thread.ctrl_shoot = osThreadNew(Task_ctrl_shoot, NULL, &attr_ctrl_shoot);
|
||||
|
||||
// 创建消息队列
|
||||
/* USER MESSAGE BEGIN */
|
||||
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 */
|
||||
|
||||
osKernelUnlock(); // 解锁内核
|
||||
|
||||
44
User/task/monitor.c
Normal file
44
User/task/monitor.c
Normal file
@ -0,0 +1,44 @@
|
||||
/*
|
||||
monitor Task
|
||||
|
||||
*/
|
||||
|
||||
/* Includes ----------------------------------------------------------------- */
|
||||
#include "task/user_task.h"
|
||||
/* USER INCLUDE BEGIN */
|
||||
|
||||
/* USER INCLUDE END */
|
||||
|
||||
/* Private typedef ---------------------------------------------------------- */
|
||||
/* Private define ----------------------------------------------------------- */
|
||||
/* Private macro ------------------------------------------------------------ */
|
||||
/* Private variables -------------------------------------------------------- */
|
||||
/* USER STRUCT BEGIN */
|
||||
|
||||
/* USER STRUCT END */
|
||||
|
||||
/* Private function --------------------------------------------------------- */
|
||||
/* Exported functions ------------------------------------------------------- */
|
||||
void Task_monitor(void *argument) {
|
||||
(void)argument; /* 未使用argument，消除警告 */
|
||||
|
||||
|
||||
/* 计算任务运行到指定频率需要等待的tick数 */
|
||||
const uint32_t delay_tick = osKernelGetTickFreq() / MONITOR_FREQ;
|
||||
|
||||
osDelay(MONITOR_INIT_DELAY); /* 延时一段时间再开启任务 */
|
||||
|
||||
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
|
||||
/* USER CODE INIT BEGIN */
|
||||
|
||||
/* USER CODE INIT END */
|
||||
|
||||
while (1) {
|
||||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||||
/* USER CODE BEGIN */
|
||||
|
||||
/* USER CODE END */
|
||||
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
|
||||
}
|
||||
|
||||
}
|
||||
@ -4,10 +4,10 @@
|
||||
*/
|
||||
|
||||
/* Includes ----------------------------------------------------------------- */
|
||||
#include "device/dr16.h"
|
||||
#include "task/user_task.h"
|
||||
/* USER INCLUDE BEGIN */
|
||||
#include "device/dr16.h"
|
||||
#include "module/shoot.h"
|
||||
/* USER INCLUDE END */
|
||||
|
||||
/* Private typedef ---------------------------------------------------------- */
|
||||
@ -16,6 +16,8 @@
|
||||
/* Private variables -------------------------------------------------------- */
|
||||
/* USER STRUCT BEGIN */
|
||||
DR16_t dr16;
|
||||
Shoot_CMD_t for_shoot;
|
||||
|
||||
/* USER STRUCT END */
|
||||
|
||||
/* Private function --------------------------------------------------------- */
|
||||
@ -40,11 +42,88 @@ void Task_rc(void *argument) {
|
||||
DR16_StartDmaRecv(&dr16);
|
||||
if (DR16_WaitDmaCplt(20)) {
|
||||
DR16_ParseData(&dr16);
|
||||
|
||||
} else {
|
||||
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 */
|
||||
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@ -18,4 +18,29 @@ const osThreadAttr_t attr_atti_esit = {
|
||||
.name = "atti_esit",
|
||||
.priority = osPriorityNormal,
|
||||
.stack_size = 256 * 4,
|
||||
};
|
||||
const osThreadAttr_t attr_ctrl_chassis = {
|
||||
.name = "ctrl_chassis",
|
||||
.priority = osPriorityNormal,
|
||||
.stack_size = 256 * 4,
|
||||
};
|
||||
const osThreadAttr_t attr_ctrl_gimbal = {
|
||||
.name = "ctrl_gimbal",
|
||||
.priority = osPriorityNormal,
|
||||
.stack_size = 256 * 4,
|
||||
};
|
||||
const osThreadAttr_t attr_monitor = {
|
||||
.name = "monitor",
|
||||
.priority = osPriorityNormal,
|
||||
.stack_size = 256 * 4,
|
||||
};
|
||||
const osThreadAttr_t attr_blink = {
|
||||
.name = "blink",
|
||||
.priority = osPriorityNormal,
|
||||
.stack_size = 256 * 4,
|
||||
};
|
||||
const osThreadAttr_t attr_ctrl_shoot = {
|
||||
.name = "ctrl_shoot",
|
||||
.priority = osPriorityNormal,
|
||||
.stack_size = 256 * 4,
|
||||
};
|
||||
@ -14,11 +14,21 @@ extern "C" {
|
||||
/* Exported constants ------------------------------------------------------- */
|
||||
/* 任务运行频率 */
|
||||
#define RC_FREQ (500.0)
|
||||
#define CTRL_CHASSIS_FREQ (500.0)
|
||||
#define CTRL_GIMBAL_FREQ (500.0)
|
||||
#define MONITOR_FREQ (500.0)
|
||||
#define BLINK_FREQ (500.0)
|
||||
#define CTRL_SHOOT_FREQ (500.0)
|
||||
|
||||
/* 任务初始化延时ms */
|
||||
#define TASK_INIT_DELAY (100u)
|
||||
#define RC_INIT_DELAY (0)
|
||||
#define ATTI_ESIT_INIT_DELAY (0)
|
||||
#define CTRL_CHASSIS_INIT_DELAY (0)
|
||||
#define CTRL_GIMBAL_INIT_DELAY (0)
|
||||
#define MONITOR_INIT_DELAY (0)
|
||||
#define BLINK_INIT_DELAY (0)
|
||||
#define CTRL_SHOOT_INIT_DELAY (0)
|
||||
|
||||
/* Exported defines --------------------------------------------------------- */
|
||||
/* Exported macro ----------------------------------------------------------- */
|
||||
@ -30,11 +40,39 @@ typedef struct {
|
||||
struct {
|
||||
osThreadId_t rc;
|
||||
osThreadId_t atti_esit;
|
||||
osThreadId_t ctrl_chassis;
|
||||
osThreadId_t ctrl_gimbal;
|
||||
osThreadId_t monitor;
|
||||
osThreadId_t blink;
|
||||
osThreadId_t ctrl_shoot;
|
||||
} thread;
|
||||
|
||||
/* USER MESSAGE BEGIN */
|
||||
struct {
|
||||
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;
|
||||
/* USER MESSAGE END */
|
||||
|
||||
@ -53,16 +91,31 @@ typedef struct {
|
||||
struct {
|
||||
UBaseType_t rc;
|
||||
UBaseType_t atti_esit;
|
||||
UBaseType_t ctrl_chassis;
|
||||
UBaseType_t ctrl_gimbal;
|
||||
UBaseType_t monitor;
|
||||
UBaseType_t blink;
|
||||
UBaseType_t ctrl_shoot;
|
||||
} stack_water_mark;
|
||||
|
||||
/* 各任务运行频率 */
|
||||
struct {
|
||||
float rc;
|
||||
float ctrl_chassis;
|
||||
float ctrl_gimbal;
|
||||
float monitor;
|
||||
float blink;
|
||||
float ctrl_shoot;
|
||||
} freq;
|
||||
|
||||
/* 任务最近运行时间 */
|
||||
struct {
|
||||
float rc;
|
||||
float ctrl_chassis;
|
||||
float ctrl_gimbal;
|
||||
float monitor;
|
||||
float blink;
|
||||
float ctrl_shoot;
|
||||
} last_up_time;
|
||||
|
||||
} Task_Runtime_t;
|
||||
@ -74,11 +127,21 @@ extern Task_Runtime_t task_runtime;
|
||||
extern const osThreadAttr_t attr_init;
|
||||
extern const osThreadAttr_t attr_rc;
|
||||
extern const osThreadAttr_t attr_atti_esit;
|
||||
extern const osThreadAttr_t attr_ctrl_chassis;
|
||||
extern const osThreadAttr_t attr_ctrl_gimbal;
|
||||
extern const osThreadAttr_t attr_monitor;
|
||||
extern const osThreadAttr_t attr_blink;
|
||||
extern const osThreadAttr_t attr_ctrl_shoot;
|
||||
|
||||
/* 任务函数声明 */
|
||||
void Task_Init(void *argument);
|
||||
void Task_rc(void *argument);
|
||||
void Task_atti_esit(void *argument);
|
||||
void Task_ctrl_chassis(void *argument);
|
||||
void Task_ctrl_gimbal(void *argument);
|
||||
void Task_monitor(void *argument);
|
||||
void Task_blink(void *argument);
|
||||
void Task_ctrl_shoot(void *argument);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user