保存一下

.vscode/settings.json

@@ -1,6 +1,7 @@
 {
     "C_Cpp.errorSquiggles": "disabled",
     "files.associations": {
-        "pid.h": "c"
+        "pid.h": "c",
+        "main.h": "c"
     }
 }

MDK-ARM/shoot.uvoptx

@@ -157,7 +157,7 @@
         <Bp>
           <Number>0</Number>
           <Type>0</Type>
-          <LineNumber>133</LineNumber>
+          <LineNumber>130</LineNumber>
           <EnabledFlag>1</EnabledFlag>
           <Address>0</Address>
           <ByteObject>0</ByteObject>
@@ -173,7 +173,7 @@
         <Bp>
           <Number>1</Number>
           <Type>0</Type>
-          <LineNumber>130</LineNumber>
+          <LineNumber>133</LineNumber>
           <EnabledFlag>1</EnabledFlag>
           <Address>0</Address>
           <ByteObject>0</ByteObject>

MDK-ARM/shoot/shoot.build_log.htm

@@ -52,7 +52,7 @@ Package Vendor: Keil
 
    * Component: ARM::CMSIS:CORE:5.4.0
       Include file:  CMSIS\Core\Include\tz_context.h
-Build Time Elapsed:  00:00:01
+Build Time Elapsed:  00:00:00
 </pre>
 </body>
 </html>

MDK-ARM/shoot/shoot_shoot.dep

@@ -1309,8 +1309,8 @@ I (D:\cangming\ARM\ARMCLANG\include\math.h)(0x6035A4A8)
 I (D:\cangming\ARM\ARMCLANG\include\stdbool.h)(0x6035A4A8)
 I (D:\cangming\ARM\ARMCLANG\include\stddef.h)(0x6035A4A8)
 F (..\User\component\pid.h)(0x68DFDF27)()
-F (..\User\module\shoot_control.c)(0x68DFEF4E)(-xc -std=c99 --target=arm-arm-none-eabi -mcpu=cortex-m4 -mfpu=fpv4-sp-d16 -mfloat-abi=hard -c

-fno-rtti -funsigned-char -fshort-enums -fshort-wchar

-gdwarf-3 -O1 -ffunction-sections -Wno-packed -Wno-missing-variable-declarations -Wno-missing-prototypes -Wno-missing-noreturn -Wno-sign-conversion -Wno-nonportable-include-path -Wno-reserved-id-macro -Wno-unused-macros -Wno-documentation-unknown-command -Wno-documentation -Wno-license-management -Wno-parentheses-equality -I ../Core/Inc -I ../Drivers/STM32F4xx_HAL_Driver/Inc -I ../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy -I ../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I ../Drivers/CMSIS/Include -I ../Middlewares/Third_Party/FreeRTOS/Source/include -I ../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 -I ../Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM4F -I ../User

-I./RTE/_shoot

-ID:/cangming/ARM/CMSIS/5.7.0/CMSIS/Core/Include

-ID:/cangming/keil/STM32F4xx_DFP/2.15.0/Drivers/CMSIS/Device/ST/STM32F4xx/Include

-D__UVISION_VERSION="534" -D_RTE_ -DSTM32F407xx -D_RTE_ -DUSE_HAL_DRIVER -DSTM32F407xx

-o shoot/shoot_control.o -MD)
+F (..\User\module\shoot_control.c)(0x68E62342)(-xc -std=c99 --target=arm-arm-none-eabi -mcpu=cortex-m4 -mfpu=fpv4-sp-d16 -mfloat-abi=hard -c

-fno-rtti -funsigned-char -fshort-enums -fshort-wchar

-gdwarf-3 -O1 -ffunction-sections -Wno-packed -Wno-missing-variable-declarations -Wno-missing-prototypes -Wno-missing-noreturn -Wno-sign-conversion -Wno-nonportable-include-path -Wno-reserved-id-macro -Wno-unused-macros -Wno-documentation-unknown-command -Wno-documentation -Wno-license-management -Wno-parentheses-equality -I ../Core/Inc -I ../Drivers/STM32F4xx_HAL_Driver/Inc -I ../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy -I ../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I ../Drivers/CMSIS/Include -I ../Middlewares/Third_Party/FreeRTOS/Source/include -I ../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 -I ../Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM4F -I ../User

-I./RTE/_shoot

-ID:/cangming/ARM/CMSIS/5.7.0/CMSIS/Core/Include

-ID:/cangming/keil/STM32F4xx_DFP/2.15.0/Drivers/CMSIS/Device/ST/STM32F4xx/Include

-D__UVISION_VERSION="534" -D_RTE_ -DSTM32F407xx -D_RTE_ -DUSE_HAL_DRIVER -DSTM32F407xx

-o shoot/shoot_control.o -MD)
-I (..\User\module\shoot_control.h)(0x68E00092)
+I (..\User\module\shoot_control.h)(0x68E621FB)
 I (..\Core\Inc\main.h)(0x68D1384A)
 I (..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal.h)(0x68AD823F)
 I (..\Core\Inc\stm32f4xx_hal_conf.h)(0x68D1384A)
@@ -1367,8 +1367,8 @@ I (..\Middlewares\Third_Party\FreeRTOS\Source\include\list.h)(0x68AD8208)
 I (..\Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.h)(0x68AD8208)
 I (D:\cangming\ARM\ARMCLANG\include\string.h)(0x6035A4A8)
 I (..\User\bsp\time.h)(0x68DFDF27)
-F (..\User\module\shoot_control.h)(0x68E00092)()
+F (..\User\module\shoot_control.h)(0x68E621FB)()
-F (..\User\module\config.c)(0x68DFEF4E)(-xc -std=c99 --target=arm-arm-none-eabi -mcpu=cortex-m4 -mfpu=fpv4-sp-d16 -mfloat-abi=hard -c

-fno-rtti -funsigned-char -fshort-enums -fshort-wchar

-gdwarf-3 -O1 -ffunction-sections -Wno-packed -Wno-missing-variable-declarations -Wno-missing-prototypes -Wno-missing-noreturn -Wno-sign-conversion -Wno-nonportable-include-path -Wno-reserved-id-macro -Wno-unused-macros -Wno-documentation-unknown-command -Wno-documentation -Wno-license-management -Wno-parentheses-equality -I ../Core/Inc -I ../Drivers/STM32F4xx_HAL_Driver/Inc -I ../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy -I ../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I ../Drivers/CMSIS/Include -I ../Middlewares/Third_Party/FreeRTOS/Source/include -I ../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 -I ../Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM4F -I ../User

-I./RTE/_shoot

-ID:/cangming/ARM/CMSIS/5.7.0/CMSIS/Core/Include

-ID:/cangming/keil/STM32F4xx_DFP/2.15.0/Drivers/CMSIS/Device/ST/STM32F4xx/Include

-D__UVISION_VERSION="534" -D_RTE_ -DSTM32F407xx -D_RTE_ -DUSE_HAL_DRIVER -DSTM32F407xx

-o shoot/config.o -MD)
+F (..\User\module\config.c)(0x68E142A3)(-xc -std=c99 --target=arm-arm-none-eabi -mcpu=cortex-m4 -mfpu=fpv4-sp-d16 -mfloat-abi=hard -c

-fno-rtti -funsigned-char -fshort-enums -fshort-wchar

-gdwarf-3 -O1 -ffunction-sections -Wno-packed -Wno-missing-variable-declarations -Wno-missing-prototypes -Wno-missing-noreturn -Wno-sign-conversion -Wno-nonportable-include-path -Wno-reserved-id-macro -Wno-unused-macros -Wno-documentation-unknown-command -Wno-documentation -Wno-license-management -Wno-parentheses-equality -I ../Core/Inc -I ../Drivers/STM32F4xx_HAL_Driver/Inc -I ../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy -I ../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I ../Drivers/CMSIS/Include -I ../Middlewares/Third_Party/FreeRTOS/Source/include -I ../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 -I ../Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM4F -I ../User

-I./RTE/_shoot

-ID:/cangming/ARM/CMSIS/5.7.0/CMSIS/Core/Include

-ID:/cangming/keil/STM32F4xx_DFP/2.15.0/Drivers/CMSIS/Device/ST/STM32F4xx/Include

-D__UVISION_VERSION="534" -D_RTE_ -DSTM32F407xx -D_RTE_ -DUSE_HAL_DRIVER -DSTM32F407xx

-o shoot/config.o -MD)
 I (..\User\module\config.h)(0x68DABE70)
 I (D:\cangming\ARM\ARMCLANG\include\stdint.h)(0x6035A4A8)
 I (..\User\component\pid.h)(0x68DFDF27)
@@ -1424,7 +1424,7 @@ I (..\Middlewares\Third_Party\FreeRTOS\Source\include\mpu_wrappers.h)(0x68AD8208
 I (..\Middlewares\Third_Party\FreeRTOS\Source\include\task.h)(0x68AD8208)
 I (..\Middlewares\Third_Party\FreeRTOS\Source\include\list.h)(0x68AD8208)
 I (..\Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.h)(0x68AD8208)
-I (..\User\module\shoot_control.h)(0x68E00092)
+I (..\User\module\shoot_control.h)(0x68E621FB)
 F (..\User\module\config.h)(0x68DABE70)()
 F (..\User\task\init.c)(0x68D4AE2D)(-xc -std=c99 --target=arm-arm-none-eabi -mcpu=cortex-m4 -mfpu=fpv4-sp-d16 -mfloat-abi=hard -c

-fno-rtti -funsigned-char -fshort-enums -fshort-wchar

-gdwarf-3 -O1 -ffunction-sections -Wno-packed -Wno-missing-variable-declarations -Wno-missing-prototypes -Wno-missing-noreturn -Wno-sign-conversion -Wno-nonportable-include-path -Wno-reserved-id-macro -Wno-unused-macros -Wno-documentation-unknown-command -Wno-documentation -Wno-license-management -Wno-parentheses-equality -I ../Core/Inc -I ../Drivers/STM32F4xx_HAL_Driver/Inc -I ../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy -I ../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I ../Drivers/CMSIS/Include -I ../Middlewares/Third_Party/FreeRTOS/Source/include -I ../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 -I ../Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM4F -I ../User

-I./RTE/_shoot

-ID:/cangming/ARM/CMSIS/5.7.0/CMSIS/Core/Include

-ID:/cangming/keil/STM32F4xx_DFP/2.15.0/Drivers/CMSIS/Device/ST/STM32F4xx/Include

-D__UVISION_VERSION="534" -D_RTE_ -DSTM32F407xx -D_RTE_ -DUSE_HAL_DRIVER -DSTM32F407xx

-o shoot/init.o -MD)
 I (..\User\task\user_task.h)(0x68D3E4B9)
@@ -1489,7 +1489,7 @@ I (..\User\component\user_math.h)(0x68DFE123)
 I (D:\cangming\ARM\ARMCLANG\include\float.h)(0x6035A4A0)
 I (D:\cangming\ARM\ARMCLANG\include\math.h)(0x6035A4A8)
 I (..\User\device\device.h)(0x68DFDFA7)
-I (..\User\module\shoot_control.h)(0x68E00092)
+I (..\User\module\shoot_control.h)(0x68E621FB)
 I (..\Core\Inc\main.h)(0x68D1384A)
 I (..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal.h)(0x68AD823F)
 I (..\Core\Inc\stm32f4xx_hal_conf.h)(0x68D1384A)

Ozone/shoot.jdebug.user

@@ -4,13 +4,15 @@
 Breakpoint=D:/CUBEMX/shoot/User/device/motor_rm.c:146:32, State=BP_STATE_DISABLED
 Breakpoint=D:/CUBEMX/shoot/User/device/motor_rm.c:149:49, State=BP_STATE_DISABLED
 Breakpoint=D:/CUBEMX/shoot/User/module/shoot_control.c:116:26, State=BP_STATE_DISABLED
-GraphedExpression="((shoot).target_variable).target_angle", Color=#e56a6f
+GraphedExpression="((shoot).target_variable).target_angle", Color=#e56a6f, Show=0
-GraphedExpression="(((shoot).feedback).trig).rotor_abs_angle", Color=#35792b
+GraphedExpression="(((shoot).feedback).trig).rotor_abs_angle", Color=#35792b, Show=0
-GraphedExpression="dpos", Color=#769dda
+GraphedExpression="dpos", Color=#769dda, Show=0
 GraphedExpression="((shoot).output).outagl_trig", Color=#b14f0d, Show=0
 GraphedExpression="((shoot).output).outomg_trig", Color=#b3c38e, Show=0
+GraphedExpression="(((shoot).feedback).trig).torque_current", Color=#ab7b05
+GraphedExpression="(((shoot).feedback).fric[0]).rotor_speed", Color=#7fd3b7, Show=0
 OpenDocument="port.c", FilePath="D:/CUBEMX/shoot/Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM4F/port.c", Line=402
-OpenDocument="tasks.c", FilePath="D:/CUBEMX/shoot/Middlewares/Third_Party/FreeRTOS/Source/tasks.c", Line=3407
+OpenDocument="tasks.c", FilePath="D:/CUBEMX/shoot/Middlewares/Third_Party/FreeRTOS/Source/tasks.c", Line=3419
 OpenDocument="math.h", FilePath="D:/cangming/ARM/ARMCLANG/include/math.h", Line=160
 OpenDocument="shoot_control.c", FilePath="D:/CUBEMX/shoot/User/module/shoot_control.c", Line=128
 OpenDocument="startup_stm32f407xx.s", FilePath="D:/CUBEMX/shoot/MDK-ARM/startup_stm32f407xx.s", Line=161
@@ -25,24 +27,24 @@ OpenDocument="rc.c", FilePath="D:/CUBEMX/shoot/User/task/rc.c", Line=0
 OpenDocument="motor.h", FilePath="D:/CUBEMX/shoot/User/device/motor.h", Line=21
 OpenDocument="motor_rm.c", FilePath="D:/CUBEMX/shoot/User/device/motor_rm.c", Line=148
 OpenToolbar="Debug", Floating=0, x=0, y=0
-OpenWindow="Registers 1", DockArea=RIGHT, x=0, y=1, w=726, h=728, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, FilteredItems=[], RefreshRate=1
+OpenWindow="Registers 1", DockArea=RIGHT, x=0, y=1, w=726, h=706, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, FilteredItems=[], RefreshRate=1
 OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=301, h=919, TabPos=0, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
-OpenWindow="Disassembly", DockArea=RIGHT, x=0, y=0, w=726, h=190, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
+OpenWindow="Disassembly", DockArea=RIGHT, x=0, y=0, w=726, h=212, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
-OpenWindow="Watched Data 1", DockArea=RIGHT, x=0, y=1, w=726, h=728, TabPos=0, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
+OpenWindow="Watched Data 1", DockArea=RIGHT, x=0, y=1, w=726, h=706, TabPos=0, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
 OpenWindow="Functions", DockArea=LEFT, x=0, y=0, w=301, h=919, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
 OpenWindow="Data Sampling", DockArea=BOTTOM, x=0, y=0, w=2560, h=536, TabPos=0, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0
-OpenWindow="Timeline", DockArea=FLOATING, x=997, y=976, w=1552, h=555, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=0, CodePaneShown=0, PinCursor="Cursor Movable", TimePerDiv="50 ms / Div", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="1094;288", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="1327;-69", CodeGraphLegendShown=0, CodeGraphLegendPosition="1343;0"
+OpenWindow="Timeline", DockArea=FLOATING, x=997, y=976, w=1552, h=555, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=0, CodePaneShown=0, PinCursor="Cursor Movable", TimePerDiv="1 s / Div", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="1094;240", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="70;20", CodeGraphLegendShown=0, CodeGraphLegendPosition="80;20"
 OpenWindow="Console", DockArea=BOTTOM, x=0, y=0, w=2560, h=536, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
 SmartViewPlugin="", Page="", Toolbar="Hidden", Window="SmartView 1"
-TableHeader="Registers 1", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Description"], ColWidths=[100;144;294]
+TableHeader="Registers 1", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Description"], ColWidths=[100;144;482]
-TableHeader="Functions", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Address";"Size";"#Insts";"Source"], ColWidths=[1594;104;100;100;286]
+TableHeader="Functions", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Address";"Size";"#Insts";"Source"], ColWidths=[1594;104;100;100;100]
 TableHeader="Power Sampling", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";"Ch 0"], ColWidths=[100;100;100]
 TableHeader="Task List", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Name";"Run Count";"Priority";"Status";"Timeout";"Stack Info";"ID";"Mutex Count";"Notified Value";"Notify State"], ColWidths=[110;110;110;110;110;110;110;110;110;110]
 TableHeader="RegisterSelectionDialog", SortCol="None", SortOrder="ASCENDING", VisibleCols=[], ColWidths=[]
 TableHeader="Source Files", SortCol="File", SortOrder="ASCENDING", VisibleCols=["File";"Status";"Size";"#Insts";"Path"], ColWidths=[215;100;100;100;734]
 TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh"], ColWidths=[250;282;91;100]
-TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";" ((shoot).target_variable).target_angle";" (((shoot).feedback).trig).rotor_abs_angle";" dpos";" ((shoot).output).outagl_trig";" ((shoot).output).outomg_trig"], ColWidths=[100;100;100;100;100;100;100]
+TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";" ((shoot).target_variable).target_angle";" (((shoot).feedback).trig).rotor_abs_angle";" dpos";" ((shoot).output).outagl_trig";" ((shoot).output).outomg_trig";" (((shoot).feedback).trig).torque_current";" (((shoot).feedback).fric[0]).rotor_speed"], ColWidths=[100;100;100;100;100;100;100;100;100]
-TableHeader="Data Sampling Setup", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[118;100;154;124;114;144;110;126;1570]
+TableHeader="Data Sampling Setup", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[118;100;144;124;114;154;110;126;1570]
 TableHeader="TargetExceptionDialog", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Address";"Description"], ColWidths=[200;100;100;340]
 WatchedExpression="shoot_ctrl_cmd_rc", RefreshRate=5, Window=Watched Data 1
 WatchedExpression="shoot_cmd", RefreshRate=5, Window=Watched Data 1

User/module/config.c

@@ -19,11 +19,13 @@ Config_RobotParam_t robot_config = {
     
 
     .shoot_param = {
-        .trig_step_angle=M_2PI/8,
+        .num_trig_tooth=8,
         .shot_freq=20.0f,
-        .shot_burst_num=4,
+        .shot_burst_num=1,
+        .jam_enable=true,
         .jam_threshold=120.0f, 
         .jam_suspected_time=0.5f, 
+        
         .fric_motor_param[0] = {
             .can = BSP_CAN_2,
             .id = 0x201,
@@ -70,7 +72,7 @@ Config_RobotParam_t robot_config = {
             .can = BSP_CAN_2,
             .id = 0x207,
             .module = MOTOR_M2006,
-            .reverse = false,
+            .reverse = true,
             .gear=true,
         },
         .fric_follow = {
@@ -93,7 +95,7 @@ Config_RobotParam_t robot_config = {
             .d_cutoff_freq=40.0f,
             .range=-1.0f, 
         },
-        .trig = {
+        .trig_2006 = {
             .k=2.5f,
             .p=1.0f,
             .i=0.1f,
@@ -103,7 +105,7 @@ Config_RobotParam_t robot_config = {
             .d_cutoff_freq=-1.0f,
             .range=M_2PI,
         },
-        .trig_omg = {
+        .trig_omg_2006 = {
             .k=1.0f,
             .p=1.5f,
             .i=0.3f,
@@ -113,6 +115,26 @@ Config_RobotParam_t robot_config = {
             .d_cutoff_freq=-1.0f,
             .range=-1.0f,
         },
+        .trig_3508 = {
+            .k=0.0f,
+            .p=0.0f,
+            .i=0.0f,
+            .d=0.00f,
+            .i_limit=0.0f,
+            .out_limit=0.0f,
+            .d_cutoff_freq=-1.0f,
+            .range=M_2PI,
+        },
+        .trig_omg_3508 = {
+            .k=0.0f,
+            .p=0.0f,
+            .i=0.0f,
+            .d=0.0f,
+            .i_limit=0.0f,
+            .out_limit=0.0f,
+            .d_cutoff_freq=-1.0f,
+            .range=-1.0f,
+        },
         .filter.fric = {
             .in = 30.0f,
             .out = 30.0f,

User/module/shoot_control.c

@@ -37,11 +37,11 @@ static inline void ScaleSumTo1(float *a, float *b) {
 int8_t Shoot_SetMode(Shoot_t *s, Shoot_Mode_t mode)
 {
     if (s == NULL) {
-        return -1; // 参数错误
+        return SHOOT_ERR_NULL; // 参数错误
     }
     s->mode=mode;
 	s->anglecalu.num_to_shoot=0;
-    return 0;
+    return SHOOT_OK;
 }
 
 /**
@@ -54,7 +54,7 @@ int8_t Shoot_SetMode(Shoot_t *s, Shoot_Mode_t mode)
 int8_t Shoot_ResetIntegral(Shoot_t *s)
 {
     if (s == NULL) {
-        return -1; // 参数错误
+        return SHOOT_ERR_NULL; // 参数错误
     }
     for(int i=0;i<SHOOT_FRIC_NUM;i++)
     {	
@@ -63,7 +63,7 @@ int8_t Shoot_ResetIntegral(Shoot_t *s)
     }
     PID_ResetIntegral(&s->pid.trig);
     PID_ResetIntegral(&s->pid.trig_omg);
-    return 0;
+    return SHOOT_OK;
 }
 
 /**
@@ -76,7 +76,7 @@ int8_t Shoot_ResetIntegral(Shoot_t *s)
 int8_t Shoot_ResetCalu(Shoot_t *s)
 {
     if (s == NULL) {
-        return -1; // 参数错误
+        return SHOOT_ERR_NULL; // 参数错误
     }
     for(int i=0;i<SHOOT_FRIC_NUM;i++)
     {	
@@ -89,7 +89,7 @@ int8_t Shoot_ResetCalu(Shoot_t *s)
     PID_Reset(&s->pid.trig_omg);
     LowPassFilter2p_Reset(&s->filter.trig.in, 0.0f);
     LowPassFilter2p_Reset(&s->filter.trig.out, 0.0f);
-    return 0;
+    return SHOOT_OK;
 }
 
 /**
@@ -102,7 +102,7 @@ int8_t Shoot_ResetCalu(Shoot_t *s)
 int8_t Shoot_ResetOutput(Shoot_t *s)
 {
     if (s == NULL) {
-        return -1; // 参数错误
+        return SHOOT_ERR_NULL; // 参数错误
     }
     for(int i=0;i<SHOOT_FRIC_NUM;i++)
     {	
@@ -114,7 +114,7 @@ int8_t Shoot_ResetOutput(Shoot_t *s)
 	s->output.outagl_trig=0.0f;
 	s->output.outomg_trig=0.0f;
     s->output.outlpf_trig=0.0f;
-    return 0;
+    return SHOOT_OK;
 }
 
 /**
@@ -128,10 +128,10 @@ int8_t Shoot_ResetOutput(Shoot_t *s)
 int8_t Shoot_CaluTargetRPM(Shoot_t *s, float target_speed)
 {
     if (s == NULL) {
-        return -1; // 参数错误
+        return SHOOT_ERR_NULL; // 参数错误
     }
     s->target_variable.target_rpm=4000.0f/MAX_FRIC_RPM;
-    return 0;
+    return SHOOT_OK;
 }
 
 /**
@@ -145,17 +145,17 @@ int8_t Shoot_CaluTargetRPM(Shoot_t *s, float target_speed)
 int8_t Shoot_CaluTargetAngle(Shoot_t *s, Shoot_CMD_t *cmd)
 {
     if (s == NULL  || s->anglecalu.num_to_shoot == 0) {
-        return -1; 
+        return SHOOT_ERR_NULL; 
     }
     float dt = s->now - s->anglecalu.time_last_shoot;
     dpos = CircleError(s->target_variable.target_angle, s->feedback.trig.rotor_abs_angle, M_2PI);
     if(dt >= 1.0f/s->param->shot_freq && cmd->firecmd && dpos<=1.0f)
     {
         s->anglecalu.time_last_shoot=s->now;
-        CircleAdd(&s->target_variable.target_angle, s->param->trig_step_angle, M_2PI);
+        CircleAdd(&s->target_variable.target_angle, M_2PI/s->param->num_trig_tooth, M_2PI);
 		s->anglecalu.num_to_shoot--;
     }
-    return 0;
+    return SHOOT_OK;
 }
 
 /**
@@ -168,7 +168,7 @@ int8_t Shoot_CaluTargetAngle(Shoot_t *s, Shoot_CMD_t *cmd)
 int8_t Chassis_UpdateFeedback(Shoot_t *s)
 {
     if (s == NULL) {
-        return -1; // 参数错误
+        return SHOOT_ERR_NULL; // 参数错误
     }
     float rpm_sum=0.0f;
     for(int i = 0; i < SHOOT_FRIC_NUM; i++) {
@@ -198,11 +198,11 @@ int8_t Chassis_UpdateFeedback(Shoot_t *s)
 		}   
     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;
     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;
+	return SHOOT_OK;
 }
 
 /**
@@ -216,7 +216,7 @@ int8_t Chassis_UpdateFeedback(Shoot_t *s)
 int8_t Shoot_RunningFSM(Shoot_t *s, Shoot_CMD_t *cmd)
 {
     if (s == NULL || cmd == NULL) {
-        return -1; // 参数错误
+        return SHOOT_ERR_NULL; // 参数错误
     }
     if(!s->online /*|| s->mode==SHOOT_MODE_SAFE*/){
         for(int i=0;i<SHOOT_FRIC_NUM;i++)
@@ -284,6 +284,7 @@ int8_t Shoot_RunningFSM(Shoot_t *s, Shoot_CMD_t *cmd)
                     else if(last_firecmd==false&&cmd->firecmd==true)
                     {
                         s->running_state=SHOOT_STATE_FIRE;
+                        /* 根据模式设置待发射弹数 */
                         switch(s->mode)
                         {
                             case SHOOT_MODE_SINGLE:
@@ -336,10 +337,13 @@ int8_t Shoot_RunningFSM(Shoot_t *s, Shoot_CMD_t *cmd)
         }
     }
     /* 输出 */
-	MOTOR_RM_Ctrl(&s->param->fric_motor_param[0]);
+    for(int i=SHOOT_FRIC_NUM-1;i>3;i-=4)
-	MOTOR_RM_Ctrl(&s->param->fric_motor_param[4]);
+    {
+        MOTOR_RM_Ctrl(&s->param->fric_motor_param[i]);
+    }
+    MOTOR_RM_Ctrl(&s->param->trig_motor_param);
     last_firecmd = cmd->firecmd;
-    return 0;
+    return SHOOT_OK;
 }
 
 /**
@@ -353,7 +357,7 @@ int8_t Shoot_RunningFSM(Shoot_t *s, Shoot_CMD_t *cmd)
 int8_t Shoot_JamDetectionFSM(Shoot_t *s, Shoot_CMD_t *cmd)
 {
     if (s == NULL) {
-        return -1; // 参数错误
+        return SHOOT_ERR_NULL; // 参数错误
     }
     
     switch (s->jamdetection.jamfsm_state) {
@@ -387,9 +391,11 @@ int8_t Shoot_JamDetectionFSM(Shoot_t *s, Shoot_CMD_t *cmd)
             /* 清空待发射弹 */
             s->anglecalu.num_to_shoot=0;
             /* 修改拨弹盘目标角度 */
-            s->target_variable.target_angle = s->feedback.trig.rotor_abs_angle-s->param->trig_step_angle;
+            s->target_variable.target_angle = s->feedback.trig.rotor_abs_angle-(0.5f*M_2PI/s->param->num_trig_tooth);
+            /* 切换状态 */
             s->jamdetection.jamfsm_state = SHOOT_JAMFSM_STATE_DEAL;
-            s->jamdetection.jam_last_time = s->now; /* 记录处理开始时间 */
+            /* 记录处理开始时间 */
+            s->jamdetection.jam_last_time = s->now; 
 
         case SHOOT_JAMFSM_STATE_DEAL:/* 堵塞处理 */
             /* 正常运行射击状态机 */
@@ -405,7 +411,7 @@ int8_t Shoot_JamDetectionFSM(Shoot_t *s, Shoot_CMD_t *cmd)
             break;
     }
 
-    return 0;
+    return SHOOT_OK;
 }
 /* Exported functions ------------------------------------------------------- */
 /**
@@ -420,7 +426,7 @@ int8_t Shoot_JamDetectionFSM(Shoot_t *s, Shoot_CMD_t *cmd)
 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; // 参数错误
+        return SHOOT_ERR_NULL; // 参数错误
     }
     s->param=param;
 
@@ -435,8 +441,20 @@ int8_t Shoot_Init(Shoot_t *s, Shoot_Params_t *param, float target_freq)
     }
     /* 初始化拨弹PID和滤波器 */
 	MOTOR_RM_Register(&param->trig_motor_param);
-    PID_Init(&s->pid.trig, KPID_MODE_CALC_D, target_freq,&param->trig);
+    switch(s->param->trig_motor_param.module)
-    PID_Init(&s->pid.trig_omg, KPID_MODE_CALC_D, target_freq,&param->trig_omg);
+    {
+        case MOTOR_M3508:
+            PID_Init(&s->pid.trig, KPID_MODE_CALC_D, target_freq,&param->trig_3508);
+            PID_Init(&s->pid.trig_omg, KPID_MODE_CALC_D, target_freq,&param->trig_omg_3508);
+            break;
+        case MOTOR_M2006:
+            PID_Init(&s->pid.trig, KPID_MODE_CALC_D, target_freq,&param->trig_2006);
+            PID_Init(&s->pid.trig_omg, KPID_MODE_CALC_D, target_freq,&param->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);
 
@@ -444,7 +462,7 @@ int8_t Shoot_Init(Shoot_t *s, Shoot_Params_t *param, float target_freq)
     memset(&s->anglecalu,0,sizeof(s->anglecalu));
     memset(&s->output,0,sizeof(s->output));
 
-	return 0;
+	return SHOOT_OK;
 }
 
 /**
@@ -458,15 +476,15 @@ int8_t Shoot_Init(Shoot_t *s, Shoot_Params_t *param, float target_freq)
 int8_t Shoot_Control(Shoot_t *s, Shoot_CMD_t *cmd)
 {
     if (s == NULL || cmd == NULL) {
-        return -1; // 参数错误
+        return SHOOT_ERR_NULL; // 参数错误
     }
     s->now = BSP_TIME_Get_us() / 1000000.0f;
     s->dt = (BSP_TIME_Get_us() - s->lask_wakeup) / 1000000.0f;
     s->lask_wakeup = BSP_TIME_Get_us();
-	s->online = cmd->online;//改成检测电机是否在线
+	s->online = cmd->online;
     //电机在线检测函数
     Shoot_JamDetectionFSM(s, cmd);
-    return 0;
+    return SHOOT_OK;
 }
 
 

User/module/shoot_control.h

@@ -16,33 +16,33 @@ extern "C" {
 
 /* Exported constants ------------------------------------------------------- */
 #define SHOOT_OK (0)        /* 运行正常 */
-#define SHOOT_ERR (-1)      /* 运行时发现了其他错误 */
+#define SHOOT_ERR_NULL (-1) /* 运行时发现NULL指针 */    
-#define SHOOT_ERR_NULL (-2) /* 运行时发现NULL指针 */
+#define SHOOT_ERR_ERR (-2)  /* 运行时发现了其他错误 */
 #define SHOOT_ERR_MODE (-3) /* 运行时配置了错误的CMD_ChassisMode_t */
-#define SHOOT_ERR_TYPE (-4) /* 运行时配置了错误的Chassis_Type_t */
+#define SHOOT_ERR_MOTOR (-4) /* 运行时配置了不存在的电机类型 */
 
 #define SHOOT_FRIC_NUM (6) /* 摩擦轮数量 */
 #define MAX_FRIC_RPM 7000.0f
-#define MAX_TRIG_RPM 5000.0f
+#define MAX_TRIG_RPM 5000.0f//这里可能也会影响最高发射频率，待测试
 /* Exported macro ----------------------------------------------------------- */
 /* Exported types ----------------------------------------------------------- */
 typedef enum {
-    SHOOT_JAMFSM_STATE_NORMAL = 0,      // 常规状态
+    SHOOT_JAMFSM_STATE_NORMAL = 0,      /* 常规状态 */
-    SHOOT_JAMFSM_STATE_SUSPECTED,       // 怀疑状态
+    SHOOT_JAMFSM_STATE_SUSPECTED,       /* 怀疑状态 */
-    SHOOT_JAMFSM_STATE_CONFIRMED,       // 确认状态
+    SHOOT_JAMFSM_STATE_CONFIRMED,       /* 确认状态 */
-    SHOOT_JAMFSM_STATE_DEAL             // 处理状态
+    SHOOT_JAMFSM_STATE_DEAL             /* 处理状态 */
 } Shoot_JamDetectionFSM_State_t;
 typedef enum {
-    SHOOT_STATE_IDLE = 0,      // 熄火
+    SHOOT_STATE_IDLE = 0,      /* 熄火 */
-    SHOOT_STATE_READY,         // 准备射击
+    SHOOT_STATE_READY,         /* 准备射击 */
-    SHOOT_STATE_FIRE           // 射击
+    SHOOT_STATE_FIRE           /* 射击 */
 } Shoot_Running_State_t;
 
 typedef enum {
-    SHOOT_MODE_SAFE = 0,      // 安全模式
+    SHOOT_MODE_SAFE = 0,       /* 安全模式 */
-    SHOOT_MODE_SINGLE,         // 单发模式
+    SHOOT_MODE_SINGLE,         /* 单发模式 */
-    SHOOT_MODE_BURST,          // 多发模式
+    SHOOT_MODE_BURST,          /* 多发模式 */
-    SHOOT_MODE_CONTINUE         // 连发模式
+    SHOOT_MODE_CONTINUE        /* 连发模式 */
 } Shoot_Mode_t;
 typedef struct {
   bool online;	
@@ -73,7 +73,7 @@ typedef struct{
 typedef struct {
   bool jam_detected; /* 卡弹检测结果 */
   float jam_last_time;/* 用于记录怀疑状态或处理状态的开始时间 */
-  Shoot_JamDetectionFSM_State_t jamfsm_state; /* 卡弹检测状态机 */
+  Shoot_JamDetectionFSM_State_t jamfsm_state; /* 卡弹检测状态 */
 }Shoot_JamDetection_t;
 typedef struct {
   float out_follow[SHOOT_FRIC_NUM];
@@ -90,22 +90,27 @@ typedef struct {
 
 /* 底盘参数的结构体，包含所有初始化用的参数，通常是const，存好几组 */
 typedef struct {
-  float trig_step_angle;  /* 每发弹丸拨弹电机转动的角度 */
+  float num_trig_tooth;  /* 拨弹盘每圈弹丸数量 */
   float shot_freq;        /* 射击频率，单位Hz */
-  uint8_t shot_burst_num; /* 多发模式下一次射击的发数 */
+  uint8_t shot_burst_num; /* 多发模式一次射击的数量 */
 
-  float jam_threshold;    /* 卡弹检测阈值，单位A */
+  uint8_t  num_multilevel;      /* 多级发射级数 (默认每级摩擦轮的数量相等)*/
+  float multilevel_ratio[1]; /* 多级发射各级速度比例 */
+
+  bool  jam_enable; /* 是否启用卡弹检测 */  //还没加到逻辑里
+  float jam_threshold;    /* 卡弹检测阈值，单位A (dji2006建议设置为120A，dji3508建议设置为300A,根据实际测试调整)*/
   float jam_suspected_time; /* 卡弹怀疑时间，单位秒 */
 
-  MOTOR_RM_Param_t          fric_motor_param[SHOOT_FRIC_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_follow;            /* 摩擦轮电机PID控制参数，用于跟随目标速度 */
   KPID_Params_t fric_err;               /* 摩擦轮电机PID控制参数，用于消除转速误差 */
-  KPID_Params_t trig;              /* 拨弹电机PID控制参数 */
+  KPID_Params_t trig_2006;              /* 拨弹电机PID控制参数 */
-  KPID_Params_t trig_omg;              /* 拨弹电机PID控制参数 */
+  KPID_Params_t trig_omg_2006;          /* 拨弹电机PID控制参数 */
-
+  KPID_Params_t trig_3508;              /* 拨弹电机PID控制参数 */
+  KPID_Params_t trig_omg_3508;          /* 拨弹电机PID控制参数 */
   /* 低通滤波器截止频率 */
   struct {
     struct{
@@ -136,9 +141,9 @@ typedef struct {
   /* 反馈信息 */           
   Shoot_Feedback_t          feedback;                      /* 反馈信息 */
   /* 控制信息*/          
-  Shoot_JamDetection_t      jamdetection;          
+  Shoot_JamDetection_t      jamdetection;                  /* 卡弹检测控制信息 */
-  Shoot_AngleCalu_t         anglecalu;           
+  Shoot_AngleCalu_t         anglecalu;                     /* 角度计算控制信息 */
-  Shoot_Output_t            output;          
+  Shoot_Output_t            output;                        /* 输出信息 */
   /* 目标控制量 */           
   struct {           
     float                   target_rpm;                    /* 目标摩擦轮转速 */
@@ -147,10 +152,10 @@ typedef struct {
 
   /* 反馈控制用的PID */
   struct {
-    KPID_t                  fric_follow[SHOOT_FRIC_NUM];   /* */
+    KPID_t                  fric_follow[SHOOT_FRIC_NUM];   /* 摩擦轮PID主结构体 */
-    KPID_t                  fric_err[SHOOT_FRIC_NUM];      /*  */
+    KPID_t                  fric_err[SHOOT_FRIC_NUM];      /* 摩擦轮PID主结构体 */
-    KPID_t                  trig;
+    KPID_t                  trig;                          /* 拨弹PID主结构体 */
-    KPID_t                  trig_omg;
+    KPID_t                  trig_omg;                      /* 拨弹PID主结构体 */
   } pid;
 
   /* 滤波器 */
@@ -161,7 +166,7 @@ typedef struct {
     }fric;
     struct{
     LowPassFilter2p_t       in;                            /* 反馈值滤波器 */
-    LowPassFilter2p_t       out;                          /* 输出值滤波器 */
+    LowPassFilter2p_t       out;                           /* 输出值滤波器 */
     }trig;
   } filter;