From 1ae3860612c656166462b2c6d92e575143271758 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sat, 3 Jan 2026 00:15:19 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E5=A4=8Dgpio=E5=92=8Cflash=E7=9A=84?= =?UTF-8?q?=E9=97=AE=E9=A2=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- app/code_page/bsp_interface.py | 11 +- assets/User_code/.DS_Store | Bin 8196 -> 12292 bytes assets/User_code/STRUCTURE.md | 113 --- assets/User_code/bsp/flash/flash.c | 27 + assets/User_code/bsp/gpio/gpio.c | 2 - assets/User_code/module/describe.csv | 1 + .../User_code/module/shoot/3+3shoot/shoot.c | 658 ++++++++++++++++++ .../User_code/module/shoot/3+3shoot/shoot.h | 243 +++++++ 8 files changed, 938 insertions(+), 117 deletions(-) delete mode 100644 assets/User_code/STRUCTURE.md create mode 100644 assets/User_code/module/shoot/3+3shoot/shoot.c create mode 100644 assets/User_code/module/shoot/3+3shoot/shoot.h diff --git a/app/code_page/bsp_interface.py b/app/code_page/bsp_interface.py index 8faacdc..3457b66 100644 --- a/app/code_page/bsp_interface.py +++ b/app/code_page/bsp_interface.py @@ -1113,18 +1113,25 @@ class bsp_gpio(QWidget): ) # 生成EXTI使能代码 - 使用用户自定义的BSP枚举名称 + # 根据引脚编号获取正确的IRQn（适配不同MCU） enable_lines = [] disable_lines = [] for config in configs: if config['has_exti']: ioc_label = config['ioc_label'] custom_name = config['custom_name'] + # 尝试从IOC label中提取引脚编号，优先使用IOC定义的IRQn enable_lines.append(f" case BSP_GPIO_{custom_name}:") + enable_lines.append(f"#if defined({ioc_label}_EXTI_IRQn)") enable_lines.append(f" HAL_NVIC_EnableIRQ({ioc_label}_EXTI_IRQn);") - enable_lines.append(f" break;") + enable_lines.append(f"#endif") + enable_lines.append(f" return BSP_OK;") + disable_lines.append(f" case BSP_GPIO_{custom_name}:") + disable_lines.append(f"#if defined({ioc_label}_EXTI_IRQn)") disable_lines.append(f" HAL_NVIC_DisableIRQ({ioc_label}_EXTI_IRQn);") - disable_lines.append(f" break;") + disable_lines.append(f"#endif") + disable_lines.append(f" return BSP_OK;") content = CodeGenerator.replace_auto_generated( content, "AUTO GENERATED BSP_GPIO_ENABLE_IRQ", "\n".join(enable_lines) diff --git a/assets/User_code/.DS_Store b/assets/User_code/.DS_Store index 0f8f41bc99c66a52bca62641c16b21284e74bcf7..6386a06dd946fa998e126d527d221acf1b2232e4 100644 GIT binary patch literal 12292 zcmeHNO>Epm6n?Wwlieh3w@uQY6ws>6p{SuG+0Y~)=w_R!EPpN8&?dA^x%2CUCiF1WM^Mp@5|!2bQaPs? zxdz^#oSU;0d&@8MbYdrqsNl%O~F{ z)Z+A%sc@C?V>hH%?JnA@@$@|%#;d3+?~8UC`t9m ztcLMcF9kHPKwyFU(gIGf2>agm_oXd@K?fEHEa0^Oj}I|aPLmF;NGd`H4txvAuk$UW zC};x*h`yLZlMby&Dnub8L_wt}b07wj;^>cYcF?3lE0RiaVp5!#b7$rt6ejKtxtL%l zmXtI&1{Me`&}e~&(B)virHrX-hF(MuUu4^W2p35ujS6Ya$UCT>FRv6BcC3nmGolFP z$)FOIanwmcwS5wwf$NA%qD4sPGza}fAMZEdJ>lYQbMw*>oLRKwm0<@P^cV0POVFMt z#mD>3Kk??4t!n<_iz@#8kvZtMHQXLPp8>MW*jangN@dNn{A|)Nv|P2F&7B4?1b{tm z=yq1s6;lYP1qCeU&Zv69SWa=2Yzf(nybz9rBeT(cD=Py>hkIj>CWfx`##TlK`g&u7 zM+dK5iG;i3k3BgtyP%bH<0_sXg*kzcw#)O{@h!Ak$Tj|YPUhw@{q6$~tc~e8j^x<7 z4ubCiWbaN?)G$3@`C3?hz9KBWA(pSD1RPACp>y;aRp?#%kUpWW={o&Fztbw)&h`jQ z-^(6nLoCVC>{)h}U0_-Es&wi4nBMPb-vVyNBv7$fe2X!JT!Cl#w$k&IrsFhDFH(la zy~`yufV<4d<+!>k?pVSyFCa8%{-qV-?_~EfZ$llLSB&Kr<1SC8?(gYYC+@mK)c-f)uJQJ$W>;%*w-z|p ziocCvH~v-e1I#2MnMa^_}cJa51)DC+Gzb+Vcl!xd6&M9Pq|v5&llHue^3Go1QrM^5Lh6vz@{wF2I(ej zd&~YUF3sS|sB3@baNYGC+TaLB#lxH2p*f=o_Gk^?ySHYKCU!G-c8}(`*tdIX!DI&t z1-i2vu;>%MDevLE>l6RqfhL|Y-PZIEe)rfMM7izXJ?apJgL9M(QbVqZ?pp*-#&-=U z?@r%1T&{`#`5n)R?p;|K>FTT3O_x~+?6Aj+~Tl6pW->@?RQ~&?~ delta 212 zcmZokXmOBWU|?W$DortDU;r^WfEYvza8E20o2Vx_*+7Ry9LQ(j1!6{^I75DNQcivn zNXh1dl3r{Z8=@IEvvaU;F!E0Rr}}xb0Mlc}$vgs*ll=vZHm?=1WS(pzqBA*0jB~QB z7~f=NF)7ZJ;^ds9{QMlojfK{Xg3KVpfp&5O30IK4n*}+(Gf(E%ah&YVBQQBehhuU) X&(z89#k(f!GILHA5Wa@bKnq3ynKCjQ diff --git a/assets/User_code/STRUCTURE.md b/assets/User_code/STRUCTURE.md deleted file mode 100644 index 6a79ab6..0000000 --- a/assets/User_code/STRUCTURE.md +++ /dev/null @@ -1,113 +0,0 @@ -# User_code 目录结构说明 - -## 新的文件夹结构 - -所有外设、组件和设备的文件现在都存放在独立的子文件夹中，便于管理和维护。 - -### BSP (板级支持包) -``` -bsp/ -├── bsp.h # BSP 总头文件 -├── describe.csv # 外设描述文件 -├── .gitkeep -├── can/ -│ ├── can.c -│ └── can.h -├── fdcan/ -│ ├── fdcan.c -│ └── fdcan.h -├── uart/ -│ ├── uart.c -│ └── uart.h -├── spi/ -│ ├── spi.c -│ └── spi.h -├── i2c/ -│ ├── i2c.c -│ └── i2c.h -├── gpio/ -│ ├── gpio.c -│ └── gpio.h -├── pwm/ -│ ├── pwm.c -│ └── pwm.h -├── time/ -│ ├── time.c -│ └── time.h -├── dwt/ -│ ├── dwt.c -│ └── dwt.h -└── mm/ - ├── mm.c - └── mm.h -``` - -### Component (组件) -``` -component/ -├── describe.csv # 组件描述文件 -├── dependencies.csv # 组件依赖关系 -├── .gitkeep -├── ahrs/ -├── capacity/ -├── cmd/ -├── crc16/ -├── crc8/ -├── error_detect/ -├── filter/ -├── freertos_cli/ -├── limiter/ -├── mixer/ -├── pid/ -├── ui/ -└── user_math/ -``` - -### Device (设备) -``` -device/ -├── device.h # Device 总头文件 -├── config.yaml # 设备配置文件 -├── .gitkeep -├── bmi088/ -├── buzzer/ -├── dm_imu/ -├── dr16/ -├── ist8310/ -├── led/ -├── motor/ -├── motor_dm/ -├── motor_lk/ -├── motor_lz/ -├── motor_odrive/ -├── motor_rm/ -├── motor_vesc/ -├── oid/ -├── ops9/ -├── rc_can/ -├── servo/ -├── vofa/ -├── ws2812/ -└── lcd_driver/ # LCD 驱动（原有结构） -``` - -## 代码生成逻辑 - -代码生成器会： -1. 首先尝试从子文件夹加载模板（如 `bsp/can/can.c`） -2. 如果子文件夹不存在，回退到根目录加载（向后兼容） -3. 生成时将文件展开到项目的扁平目录结构中（如 `User/bsp/can.c`） - -## 优势 - -✅ **更好的组织**: 每个外设/组件的文件都在独立文件夹中 -✅ **便于管理**: 添加、删除、修改模板更加方便 -✅ **向后兼容**: 现有的扁平结构仍然可以正常工作 -✅ **清晰的结构**: 一目了然地看到所有可用的外设/组件 - -## 迁移说明 - -如果你添加新的外设/组件/设备： -1. 在对应目录下创建新的子文件夹（小写命名） -2. 将 .c 和 .h 文件放入子文件夹 -3. 代码生成器会自动识别并使用 diff --git a/assets/User_code/bsp/flash/flash.c b/assets/User_code/bsp/flash/flash.c index 0df01ca..87af81a 100644 --- a/assets/User_code/bsp/flash/flash.c +++ b/assets/User_code/bsp/flash/flash.c @@ -26,10 +26,18 @@ void BSP_Flash_EraseSector(uint32_t sector) { flash_erase.TypeErase = FLASH_TYPEERASE_SECTORS; flash_erase.VoltageRange = FLASH_VOLTAGE_RANGE_3; flash_erase.NbSectors = 1; +#if defined(STM32H7) + flash_erase.Banks = FLASH_BANK_1; // H7 requires Bank parameter +#endif HAL_FLASH_Unlock(); +#if defined(STM32H7) + while (FLASH_WaitForLastOperation(50, FLASH_BANK_1) != HAL_OK) + ; +#else while (FLASH_WaitForLastOperation(50) != HAL_OK) ; +#endif HAL_FLASHEx_Erase(&flash_erase, §or_error); HAL_FLASH_Lock(); } @@ -39,6 +47,24 @@ void BSP_Flash_EraseSector(uint32_t sector) { void BSP_Flash_WriteBytes(uint32_t address, const uint8_t *buf, size_t len) { HAL_FLASH_Unlock(); +#if defined(STM32H7) + // H7 uses FLASHWORD (32 bytes) programming + uint8_t flash_word[32] __attribute__((aligned(32))); + while (len > 0) { + size_t chunk = (len < 32) ? len : 32; + memset(flash_word, 0xFF, 32); + memcpy(flash_word, buf, chunk); + + while (FLASH_WaitForLastOperation(50, FLASH_BANK_1) != HAL_OK) + ; + HAL_FLASH_Program(FLASH_TYPEPROGRAM_FLASHWORD, address, (uint32_t)flash_word); + + address += 32; + buf += chunk; + len -= chunk; + } +#else + // F4/F7 use byte programming while (len > 0) { while (FLASH_WaitForLastOperation(50) != HAL_OK) ; @@ -47,6 +73,7 @@ void BSP_Flash_WriteBytes(uint32_t address, const uint8_t *buf, size_t len) { buf++; len--; } +#endif HAL_FLASH_Lock(); } diff --git a/assets/User_code/bsp/gpio/gpio.c b/assets/User_code/bsp/gpio/gpio.c index 5c3113c..2950620 100644 --- a/assets/User_code/bsp/gpio/gpio.c +++ b/assets/User_code/bsp/gpio/gpio.c @@ -69,7 +69,6 @@ int8_t BSP_GPIO_EnableIRQ(BSP_GPIO_t gpio) { default: return BSP_ERR; } - return BSP_OK; } int8_t BSP_GPIO_DisableIRQ(BSP_GPIO_t gpio) { @@ -78,7 +77,6 @@ int8_t BSP_GPIO_DisableIRQ(BSP_GPIO_t gpio) { default: return BSP_ERR; } - return BSP_OK; } int8_t BSP_GPIO_WritePin(BSP_GPIO_t gpio, bool value){ if (gpio >= BSP_GPIO_NUM) return BSP_ERR; diff --git a/assets/User_code/module/describe.csv b/assets/User_code/module/describe.csv index bbd755c..0a5327a 100644 --- a/assets/User_code/module/describe.csv +++ b/assets/User_code/module/describe.csv @@ -1,3 +1,4 @@ module_name,description cmd,命令系统，用于机器人指令处理和行为控制 2_axis_gimbal,双轴云台控制模块，支持pitch和yaw轴控制 +3+3shoot,双级三摩擦轮发射机构模块 \ No newline at end of file diff --git a/assets/User_code/module/shoot/3+3shoot/shoot.c b/assets/User_code/module/shoot/3+3shoot/shoot.c new file mode 100644 index 0000000..b8902a3 --- /dev/null +++ b/assets/User_code/module/shoot/3+3shoot/shoot.c @@ -0,0 +1,658 @@ +/* + * 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 +#include +#include "shoot.h" +#include "bsp/mm.h" +#include "bsp/time.h" +#include "component/filter.h" +#include "component/user_math.h" +/* Private typedef ---------------------------------------------------------- */ +/* Private define ----------------------------------------------------------- */ +#define MAX_FRIC_RPM 7000.0f +#define MAX_TRIG_RPM 1500.0f//这里可能也会影响最高发射频率，待测试 +/* Private macro ------------------------------------------------------------ */ +/* Private variables -------------------------------------------------------- */ +static bool last_firecmd; + +float maxTrigrpm=1500.0f; +/* 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; + 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->basic.fric_num; + for(int i=0;ipid.fric_follow[i]); + PID_ResetIntegral(&s->pid.fric_err[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->basic.fric_num; + for(int i=0;ipid.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 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->basic.fric_num; + for(int i=0;ioutput.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 SHOOT_OK; +} +//float last_angle=0.0f; +//float speed=0.0f; +//int8_t Shoot_CalufeedbackRPM(Shoot_t *s) +//{ +// if (s == NULL) { +// return SHOOT_ERR_NULL; // 参数错误 +// } +//// static +// float err; +// err=CircleError(s->feedback.fric[0].rotor_abs_angle,last_angle,M_2PI); +// speed=err/s->dt/M_2PI*60.0f; +// last_angle=s->feedback.fric->rotor_abs_angle; + + +// return SHOOT_OK; +//} + +/** + * \brief 根据目标弹丸速度计算摩擦轮目标转速 + * + * \param s 包含射击数据的结构体 + * \param target_speed 目标弹丸速度，单位m/s + * + * \return 函数运行结果 + */ +int8_t Shoot_CaluTargetRPM(Shoot_t *s, float target_speed) +{ + if (s == NULL) { + return SHOOT_ERR_NULL; // 参数错误 + } + switch(s->param->basic.projectileType) + { + case SHOOT_PROJECTILE_17MM: + s->target_variable.fric_rpm=5000.0f; + break; + case SHOOT_PROJECTILE_42MM: + s->target_variable.fric_rpm=4000.0f; + break; + } + return SHOOT_OK; +} + +/** + * \brief 根据发射弹丸数量及发射频率计算拨弹电机目标角度 + * + * \param s 包含发射数据的结构体 + * \param cmd 包含射击指令的结构体 + * + * \return 函数运行结果 + */ +int8_t Shoot_CaluTargetAngle(Shoot_t *s, Shoot_CMD_t *cmd) +{ + if (s == NULL || s->var_trig.num_toShoot == 0) { + return SHOOT_ERR_NULL; + } + float dt = s->timer.now - s->var_trig.time_lastShoot; + float dpos; + dpos = CircleError(s->target_variable.trig_angle, s->var_trig.trig_agl, M_2PI); + if(dt >= 1.0f/s->param->basic.shot_freq && cmd->firecmd && dpos<=1.0f) + { + s->var_trig.time_lastShoot=s->timer.now; + CircleAdd(&s->target_variable.trig_angle, M_2PI/s->param->basic.num_trig_tooth, M_2PI); + s->var_trig.num_toShoot--; + } + return SHOOT_OK; +} + +static float Shoot_CaluCoupledWeight(Shoot_t *s, uint8_t fric_index) +{ + if (s == NULL) { + return SHOOT_ERR_NULL; // 参数错误 + } + + float Threshold; + switch (s->param->basic.projectileType) { + case SHOOT_PROJECTILE_17MM: + Threshold=50.0f; + break; + case SHOOT_PROJECTILE_42MM: + Threshold=400.0f; + break; + default: + return 0.0f; + } + + float err; + err=fabs((s->param->basic.ratio_multilevel[fric_index] + *s->target_variable.fric_rpm) + -s->feedback.fric[fric_index].rotor_speed); + if (errvar_fric.coupled_control_weights=1.0f-(err*err)/(Threshold*Threshold); + } + else + { + s->var_fric.coupled_control_weights=0.0f; + } + return s->var_fric.coupled_control_weights; +} + +/** + * \brief 更新射击模块的电机反馈信息 + * + * \param s 包含射击数据的结构体 + * + * \return 函数运行结果 + */ +int8_t Shoot_UpdateFeedback(Shoot_t *s) +{ + if (s == NULL) { + return SHOOT_ERR_NULL; // 参数错误 + } + uint8_t fric_num = s->param->basic.fric_num; + for(int i = 0; i < fric_num; i++) { + /* 更新摩擦轮电机反馈 */ + MOTOR_RM_Update(&s->param->motor.fric[i].param); + MOTOR_RM_t *motor_fed = MOTOR_RM_GetMotor(&s->param->motor.fric[i].param); + if(motor_fed!=NULL) + { + s->feedback.fric[i]=motor_fed->motor.feedback; + } + /* 滤波摩擦轮电机转速反馈 */ + s->var_fric.fil_rpm[i] = LowPassFilter2p_Apply(&s->filter.fric.in[i], s->feedback.fric[i].rotor_speed); + /* 归一化摩擦轮电机转速反馈 */ + s->var_fric.normalized_fil_rpm[i] = s->var_fric.fil_rpm[i] / MAX_FRIC_RPM; + if(s->var_fric.normalized_fil_rpm[i]>1.0f)s->var_fric.normalized_fil_rpm[i]=1.0f; + if(s->var_fric.normalized_fil_rpm[i]<-1.0f)s->var_fric.normalized_fil_rpm[i]=-1.0f; + /* 计算平均摩擦轮电机转速反馈 */ + s->var_fric.normalized_fil_avgrpm[s->param->motor.fric[i].level-1]+=s->var_fric.normalized_fil_rpm[i]; + } + for (int i=1; ivar_fric.normalized_fil_avgrpm[i]=s->var_fric.normalized_fil_avgrpm[i]/fric_num/MAX_NUM_MULTILEVEL; + } + /* 更新拨弹电机反馈 */ + MOTOR_RM_Update(&s->param->motor.trig); + s->feedback.trig = *MOTOR_RM_GetMotor(&s->param->motor.trig); + s->var_trig.trig_agl=s->param->basic.extra_deceleration_ratio*s->feedback.trig.gearbox_total_angle; + while(s->var_trig.trig_agl<0)s->var_trig.trig_agl+=M_2PI; + while(s->var_trig.trig_agl>=M_2PI)s->var_trig.trig_agl-=M_2PI; + if (s->feedback.trig.motor.reverse) { + s->var_trig.trig_agl = M_2PI - s->var_trig.trig_agl; + } + s->var_trig.fil_trig_rpm = LowPassFilter2p_Apply(&s->filter.trig.in, s->feedback.trig.feedback.rotor_speed); + s->var_trig.trig_rpm = s->feedback.trig.feedback.rotor_speed / maxTrigrpm; + if(s->var_trig.trig_rpm>1.0f)s->var_trig.trig_rpm=1.0f; + if(s->var_trig.trig_rpm<-1.0f)s->var_trig.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 函数运行结果 + */float a; +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->basic.fric_num; + static float pos; + if(s->mode==SHOOT_MODE_SAFE){ + for(int i=0;iparam->motor.fric[i].param); + } + MOTOR_RM_Relax(&s->param->motor.trig);\ + pos=s->target_variable.trig_angle=s->var_trig.trig_agl; + } + else{ + switch(s->running_state) + { + case SHOOT_STATE_IDLE:/*熄火等待*/ + for(int i=0;ipid.fric_follow[i]); + s->output.out_follow[i]=PID_Calc(&s->pid.fric_follow[i],0.0f,s->var_fric.normalized_fil_rpm[i],0,s->timer.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->motor.fric[i].param, s->output.lpfout_fric[i]); + } + + s->output.outagl_trig =PID_Calc(&s->pid.trig,pos,s->var_trig.trig_agl,0,s->timer.dt); + s->output.outomg_trig =PID_Calc(&s->pid.trig_omg,s->output.outagl_trig,s->var_trig.trig_rpm,0,s->timer.dt); + s->output.outlpf_trig =LowPassFilter2p_Apply(&s->filter.trig.out, s->output.outomg_trig); + MOTOR_RM_SetOutput(&s->param->motor.trig, 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;iparam->motor.fric[i].level-1; + float target_rpm=s->param->basic.ratio_multilevel[level] + *s->target_variable.fric_rpm/MAX_FRIC_RPM; + /* 计算耦合控制权重 */ + float w=Shoot_CaluCoupledWeight(s,i); + /* 计算跟随输出、计算修正输出 */ + s->output.out_follow[i]=PID_Calc(&s->pid.fric_follow[i], + target_rpm, + s->var_fric.normalized_fil_rpm[i], + 0, + s->timer.dt); + s->output.out_err[i]=w*PID_Calc(&s->pid.fric_err[i], + s->var_fric.normalized_fil_avgrpm[s->param->motor.fric[i].level-1], + s->var_fric.normalized_fil_rpm[i], + 0, + s->timer.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->motor.fric[i].param, s->output.lpfout_fric[i]); + } + /* 设置拨弹电机输出 */ + s->output.outagl_trig =PID_Calc(&s->pid.trig, + pos, + s->var_trig.trig_agl, + 0, + s->timer.dt); + s->output.outomg_trig =PID_Calc(&s->pid.trig_omg, + s->output.outagl_trig, + s->var_trig.trig_rpm, + 0, + s->timer.dt); + s->output.outlpf_trig =LowPassFilter2p_Apply(&s->filter.trig.out, s->output.outomg_trig); + MOTOR_RM_SetOutput(&s->param->motor.trig, 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->var_trig.num_toShoot=1; + break; + case SHOOT_MODE_BURST: + s->var_trig.num_toShoot=s->param->basic.shot_burst_num; + break; + case SHOOT_MODE_CONTINUE: + s->var_trig.num_toShoot=6666; + break; + default: + s->var_trig.num_toShoot=0; + break; + } + } + break; + + case SHOOT_STATE_FIRE:/*射击*/ + Shoot_CaluTargetAngle(s, cmd); + for(int i=0;iparam->motor.fric[i].level-1; + float target_rpm=s->param->basic.ratio_multilevel[level] + *s->target_variable.fric_rpm/MAX_FRIC_RPM; + /* 计算耦合控制权重 */ + float w=Shoot_CaluCoupledWeight(s,i); + /* 计算跟随输出、计算修正输出 */ + s->output.out_follow[i]=PID_Calc(&s->pid.fric_follow[i], + target_rpm, + s->var_fric.normalized_fil_rpm[i], + 0, + s->timer.dt); + s->output.out_err[i]=w*PID_Calc(&s->pid.fric_err[i], + s->var_fric.normalized_fil_avgrpm[s->param->motor.fric[i].level-1], + s->var_fric.normalized_fil_rpm[i], + 0, + s->timer.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->motor.fric[i].param, s->output.lpfout_fric[i]); + } + /* 设置拨弹电机输出 */ + s->output.outagl_trig =PID_Calc(&s->pid.trig, + s->target_variable.trig_angle, + s->var_trig.trig_agl, + 0, + s->timer.dt); + s->output.outomg_trig =PID_Calc(&s->pid.trig_omg, + s->output.outagl_trig, + s->var_trig.trig_rpm, + 0, + s->timer.dt); + s->output.outlpf_trig =LowPassFilter2p_Apply(&s->filter.trig.out, s->output.outomg_trig); + MOTOR_RM_SetOutput(&s->param->motor.trig, s->output.outlpf_trig); + + /* 检查状态机 */ + if(!cmd->firecmd) + { + s->running_state=SHOOT_STATE_READY; + pos=s->var_trig.trig_agl; + s->var_trig.num_toShoot=0; + } + break; + + default: + s->running_state=SHOOT_STATE_IDLE; + break; + } + } + /* 输出 */ + MOTOR_RM_Ctrl(&s->param->motor.fric[0].param); + if(s->param->basic.fric_num>4) + { + MOTOR_RM_Ctrl(&s->param->motor.fric[4].param); + } + MOTOR_RM_Ctrl(&s->param->motor.trig); + 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->jamDetection.enable){ + switch (s->jamdetection.fsmState) { + case SHOOT_JAMFSM_STATE_NORMAL:/* 正常运行 */ + /* 检测电流是否超过阈值 */ + if (s->feedback.trig.feedback.torque_current/1000.0f > s->param->jamDetection.threshold) { + s->jamdetection.fsmState = SHOOT_JAMFSM_STATE_SUSPECTED; + s->jamdetection.lastTime = s->timer.now; /* 记录怀疑开始时间 */ + } + /* 正常运行射击状态机 */ + Shoot_RunningFSM(s, cmd); + break; + case SHOOT_JAMFSM_STATE_SUSPECTED:/* 怀疑堵塞 */ + /* 检测电流是否低于阈值 */ + if (s->feedback.trig.feedback.torque_current/1000.0f < s->param->jamDetection.threshold) { + s->jamdetection.fsmState = SHOOT_JAMFSM_STATE_NORMAL; + break; + } + /* 检测高阈值状态是否超过设定怀疑时间 */ + else if ((s->timer.now - s->jamdetection.lastTime) >= s->param->jamDetection.suspectedTime) { + s->jamdetection.detected =true; + s->jamdetection.fsmState = SHOOT_JAMFSM_STATE_CONFIRMED; + break; + } + /* 正常运行射击状态机 */ + Shoot_RunningFSM(s, cmd); + break; + case SHOOT_JAMFSM_STATE_CONFIRMED:/* 确认堵塞 */ + /* 清空待发射弹 */ + s->var_trig.num_toShoot=0; + /* 修改拨弹盘目标角度 */ + s->target_variable.trig_angle = s->var_trig.trig_agl-(M_2PI/s->param->basic.num_trig_tooth); + /* 切换状态 */ + s->jamdetection.fsmState = SHOOT_JAMFSM_STATE_DEAL; + /* 记录处理开始时间 */ + s->jamdetection.lastTime = s->timer.now; + case SHOOT_JAMFSM_STATE_DEAL:/* 堵塞处理 */ + /* 正常运行射击状态机 */ + Shoot_RunningFSM(s, cmd); + /* 给予0.3秒响应时间并检测电流小于20A，认为堵塞已解除 */ + if ((s->timer.now - s->jamdetection.lastTime)>=0.3f&&s->feedback.trig.feedback.torque_current/1000.0f < 20.0f) { + s->jamdetection.fsmState = SHOOT_JAMFSM_STATE_NORMAL; + } + break; + default: + s->jamdetection.fsmState = SHOOT_JAMFSM_STATE_NORMAL; + break; + } + } + else{ + s->jamdetection.fsmState = SHOOT_JAMFSM_STATE_NORMAL; + s->jamdetection.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->basic.fric_num; + s->param=param; + BSP_CAN_Init(); + /* 初始化摩擦轮PID和滤波器 */ + for(int i=0;imotor.fric[i].param); + PID_Init(&s->pid.fric_follow[i], + KPID_MODE_CALC_D, + target_freq, + ¶m->pid.fric_follow); + PID_Init(&s->pid.fric_err[i], + KPID_MODE_CALC_D, + target_freq, + ¶m->pid.fric_err); + 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->motor.trig); + switch(s->param->motor.trig.module) + { + case MOTOR_M3508: + PID_Init(&s->pid.trig, + KPID_MODE_CALC_D, + target_freq, + ¶m->pid.trig_3508); + PID_Init(&s->pid.trig_omg, + KPID_MODE_CALC_D, + target_freq, + ¶m->pid.trig_omg_3508); + break; + case MOTOR_M2006: + PID_Init(&s->pid.trig, + KPID_MODE_CALC_D, + target_freq, + ¶m->pid.trig_2006); + PID_Init(&s->pid.trig_omg, + KPID_MODE_CALC_D, + target_freq, + ¶m->pid.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->var_trig,0,sizeof(s->var_trig)); + 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->timer.now = BSP_TIME_Get_us() / 1000000.0f; + s->timer.dt = (BSP_TIME_Get_us() - s->timer.lask_wakeup) / 1000000.0f; + s->timer.lask_wakeup = BSP_TIME_Get_us(); + Shoot_CaluTargetRPM(s,233); + + Shoot_JamDetectionFSM(s, cmd); +// Shoot_CalufeedbackRPM(s); + return SHOOT_OK; +} + + + + + + + + diff --git a/assets/User_code/module/shoot/3+3shoot/shoot.h b/assets/User_code/module/shoot/3+3shoot/shoot.h new file mode 100644 index 0000000..1972543 --- /dev/null +++ b/assets/User_code/module/shoot/3+3shoot/shoot.h @@ -0,0 +1,243 @@ +/* + * far♂蛇模块 + */ + +#pragma once + +#include +#ifdef __cplusplus +extern "C" { +#endif + +#include "main.h" +#include "component/pid.h" +#include "device/motor_rm.h" +/* Exported constants ------------------------------------------------------- */ +#define MAX_FRIC_NUM 6 +#define MAX_NUM_MULTILEVEL 2 /* 多级发射级数 */ + +#define SHOOT_OK (0) /* 运行正常 */ +#define SHOOT_ERR_NULL (-1) /* 运行时发现NULL指针 */ +#define SHOOT_ERR_ERR (-2) /* 运行时发现了其他错误 */ +#define SHOOT_ERR_MODE (-3) /* 运行时配置了错误的Mode */ +#define SHOOT_ERR_MOTOR (-4) /* 运行时配置了不存在的电机类型 */ +#define SHOOT_ERR_MALLOC (-5) /* 内存分配失败 */ +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ +typedef enum { + SHOOT_JAMFSM_STATE_NORMAL = 0,/* 常规状态 */ + SHOOT_JAMFSM_STATE_SUSPECTED, /* 怀疑状态 */ + SHOOT_JAMFSM_STATE_CONFIRMED, /* 确认状态 */ + SHOOT_JAMFSM_STATE_DEAL /* 处理状态 */ +}Shoot_JamDetectionFSM_State_t; +typedef enum { + SHOOT_STATE_IDLE = 0,/* 熄火 */ + SHOOT_STATE_READY, /* 准备射击 */ + SHOOT_STATE_FIRE /* 射击 */ +}Shoot_Running_State_t; + +typedef enum { + SHOOT_MODE_SAFE = 0,/* 安全模式 */ + SHOOT_MODE_SINGLE, /* 单发模式 */ + SHOOT_MODE_BURST, /* 多发模式 */ + SHOOT_MODE_CONTINUE,/* 连发模式 */ + SHOOT_MODE_NUM +}Shoot_Mode_t; + +typedef enum { + SHOOT_PROJECTILE_17MM, + SHOOT_PROJECTILE_42MM, +}Shoot_Projectile_t; + +typedef struct{ + MOTOR_RM_Param_t param; + uint8_t level; /* 电机属于第几级发射；1起始 */ +}Shoot_MOTOR_RM_Param_t; + +typedef struct { + MOTOR_Feedback_t fric[MAX_FRIC_NUM];/* 摩擦轮电机反馈 */ + MOTOR_RM_t trig; /* 拨弹电机反馈 */ + +}Shoot_Feedback_t; + +typedef struct{ + float fil_rpm[MAX_FRIC_NUM]; /* 滤波后的摩擦轮原始转速 */ + float normalized_fil_rpm[MAX_FRIC_NUM]; /* 归一化摩擦轮转速 */ + float normalized_fil_avgrpm[MAX_NUM_MULTILEVEL]; /* 归一化摩擦轮平均转速 */ + float coupled_control_weights; /* 耦合控制权重 */ +}Shoot_VARSForFricCtrl_t; + +typedef struct{ + float time_lastShoot;/* 上次射击时间 */ + uint16_t num_toShoot;/* 剩余待发射弹数 */ + uint16_t num_shooted;/* 已发射弹数 */ + + float trig_agl; /*计算所有减速比后的拨弹盘的角度*/ + float fil_trig_rpm; /* 滤波后的拨弹电机转速*/ + float trig_rpm; /* 归一化拨弹电机转速 */ +}Shoot_VARSForTrigCtrl_t; + +typedef struct { + bool detected; /* 卡弹检测结果 */ + float lastTime;/* 用于记录怀疑状态或处理状态的开始时间 */ + Shoot_JamDetectionFSM_State_t fsmState; /* 卡弹检测状态机 */ +}Shoot_JamDetection_t; +typedef struct { + float out_follow[MAX_FRIC_NUM]; + float out_err[MAX_FRIC_NUM]; + float out_fric[MAX_FRIC_NUM]; + float lpfout_fric[MAX_FRIC_NUM]; + + float outagl_trig; + float outomg_trig; + float outlpf_trig; +}Shoot_Output_t; + +typedef struct { + Shoot_Mode_t mode;/* 射击模式 */ + bool ready; /* 准备射击 */ + bool firecmd; /* 射击 */ +}Shoot_CMD_t; +/* 底盘参数的结构体，包含所有初始化用的参数，通常是const，存好几组 */ +typedef struct { + struct{ + Shoot_Projectile_t projectileType; /* 发射弹丸类型 */; + size_t fric_num; /* 摩擦轮电机数量 */ + float ratio_multilevel[MAX_NUM_MULTILEVEL]; /* 多级发射各级速度比例 */ + float extra_deceleration_ratio; /*电机出轴到拨盘的额外减速比；没有写1*/ + size_t num_trig_tooth; /* 拨弹盘每圈弹丸数量 */ + float shot_freq; /* 射击频率，单位Hz */ + size_t shot_burst_num; /* 多发模式一次射击的数量 */ + }basic;/* 发射基础参数 */ + struct { + bool enable; /* 是否启用卡弹检测 */ + float threshold; /* 卡弹检测阈值，单位A (dji2006建议设置为120A，dji3508建议设置为235A,根据实际测试调整)*/ + float suspectedTime;/* 卡弹怀疑时间，单位秒 */ + }jamDetection;/* 卡弹检测参数 */ + struct { + Shoot_MOTOR_RM_Param_t fric[MAX_FRIC_NUM]; + MOTOR_RM_Param_t trig; + }motor; /* 电机参数 */ + struct{ + KPID_Params_t fric_follow; /* 摩擦轮电机PID控制参数，用于跟随目标速度 */ + KPID_Params_t fric_err; /* 摩擦轮电机PID控制参数，用于消除转速误差 */ + KPID_Params_t trig_2006; /* 拨弹电机PID控制参数 */ + KPID_Params_t trig_omg_2006;/* 拨弹电机PID控制参数 */ + KPID_Params_t trig_3508; /* 拨弹电机PID控制参数 */ + KPID_Params_t trig_omg_3508;/* 拨弹电机PID控制参数 */ + }pid; /* PID参数 */ + struct { + struct{ + float in; /* 反馈值滤波器截止频率 */ + float out; /* 输出值滤波器截止频率 */ + }fric; + struct{ + float in; /* 反馈值滤波器截止频率 */ + float out; /* 输出值滤波器截止频率 */ + }trig; + } filter;/* 滤波器截止频率参数 */ +} Shoot_Params_t; + +typedef struct { + float now; /* 当前时间，单位秒 */ + uint64_t lask_wakeup; /* 上次唤醒时间，单位微秒 */ + float dt; /* 两次唤醒间隔时间，单位秒 */ +}Shoot_Timer_t; + +/* + * 运行的主结构体，所有这个文件里的函数都在操作这个结构体 + * 包含了初始化参数，中间变量，输出变量 + */ +typedef struct { + Shoot_Timer_t timer; /* 计时器 */ + Shoot_Params_t *param; /* 发射参数 */ + /* 模块通用 */ + Shoot_Mode_t mode; /* 射击模式 */ + /* 反馈信息 */ + Shoot_Feedback_t feedback; + /* 控制信息*/ + Shoot_Running_State_t running_state; /* 运行状态机 */ + Shoot_JamDetection_t jamdetection; /* 卡弹检测控制信息 */ + Shoot_VARSForFricCtrl_t var_fric; /* 摩擦轮控制信息 */ + Shoot_VARSForTrigCtrl_t var_trig; /* 角度计算控制信息 */ + Shoot_Output_t output; /* 输出信息 */ + /* 目标控制量 */ + struct { + float fric_rpm; /* 目标摩擦轮转速 */ + float trig_angle;/* 目标拨弹位置 */ + }target_variable; + + /* 反馈控制用的PID */ + struct { + KPID_t fric_follow[MAX_FRIC_NUM];/* 摩擦轮PID主结构体 */ + KPID_t fric_err[MAX_FRIC_NUM]; /* 摩擦轮PID主结构体 */ + KPID_t trig; /* 拨弹PID主结构体 */ + KPID_t trig_omg; /* 拨弹PID主结构体 */ + } pid; + + /* 滤波器 */ + struct { + struct{ + LowPassFilter2p_t in[MAX_FRIC_NUM]; /* 反馈值滤波器 */ + LowPassFilter2p_t out[MAX_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 包含发射数据的结构体 + * \param mode 包含发射模式的枚举 + * + * \return 函数运行结果 + */ +int8_t Shoot_SetMode(Shoot_t *s, Shoot_Mode_t mode); + +/** + * \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 + + +