修复gpio和flash的问题

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)

assets/User_code/STRUCTURE.md

@@ -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. 代码生成器会自动识别并使用

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, &sector_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();
 }
 

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;

assets/User_code/module/describe.csv

@@ -1,3 +1,4 @@
 module_name,description
 cmd,命令系统，用于机器人指令处理和行为控制
 2_axis_gimbal,双轴云台控制模块，支持pitch和yaw轴控制
+3+3shoot,双级三摩擦轮发射机构模块

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 <math.h>
+#include <string.h>
+#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;i<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 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;i<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 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;i<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 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 (err<Threshold)
+    {
+        s->var_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; i<MAX_NUM_MULTILEVEL; i++)
+    {
+        s->var_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;i<fric_num;i++)
+      {	
+          MOTOR_RM_Relax(&s->param->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;i<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->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;i<fric_num;i++)
+					{	
+						uint8_t level=s->param->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;i<fric_num;i++)
+                {	
+					uint8_t level=s->param->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;i<fric_num;i++){
+	    MOTOR_RM_Register(&param->motor.fric[i].param);
+        PID_Init(&s->pid.fric_follow[i], 
+            KPID_MODE_CALC_D, 
+            target_freq,
+            &param->pid.fric_follow);
+	    PID_Init(&s->pid.fric_err[i], 
+            KPID_MODE_CALC_D, 
+            target_freq,
+            &param->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(&param->motor.trig);
+    switch(s->param->motor.trig.module)
+    {
+        case MOTOR_M3508:
+            PID_Init(&s->pid.trig, 
+                KPID_MODE_CALC_D, 
+                target_freq,
+                &param->pid.trig_3508);
+            PID_Init(&s->pid.trig_omg, 
+                KPID_MODE_CALC_D, 
+                target_freq,
+                &param->pid.trig_omg_3508);
+            break;
+        case MOTOR_M2006:
+            PID_Init(&s->pid.trig, 
+                KPID_MODE_CALC_D, 
+                target_freq,
+                &param->pid.trig_2006);
+            PID_Init(&s->pid.trig_omg, 
+                KPID_MODE_CALC_D, 
+                target_freq,
+                &param->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;
+}
+
+
+
+
+
+
+
+

assets/User_code/module/shoot/3+3shoot/shoot.h

@@ -0,0 +1,243 @@
+/*
+ * far♂蛇模块
+ */
+
+#pragma once
+
+#include <stddef.h>
+#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
+
+
+