xm/shoot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

拨弹双环控制，修复了滤波导致的抖动。添加了角度计算和装模做样的速度计算

Browse Source
This commit is contained in:
yxming66 2025-10-01 23:49:10 +08:00
parent 293600caaa
commit c38f98faac
18 changed files with 3130 additions and 3095 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
.vscode/settings.json vendored
View File

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

58
MDK-ARM/shoot.uvguix.yxm23
View File

File diff suppressed because one or more lines are too long

20
MDK-ARM/shoot.uvoptx
View File

@ -157,7 +157,7 @@
<Bp> <Bp>
<Number>0</Number> <Number>0</Number>
<Type>0</Type> <Type>0</Type>
<LineNumber>130</LineNumber> <LineNumber>131</LineNumber>
<EnabledFlag>1</EnabledFlag> <EnabledFlag>1</EnabledFlag>
<Address>0</Address> <Address>0</Address>
<ByteObject>0</ByteObject> <ByteObject>0</ByteObject>
@ -173,23 +173,7 @@
<Bp> <Bp>
<Number>1</Number> <Number>1</Number>
<Type>0</Type> <Type>0</Type>
<LineNumber>131</LineNumber> <LineNumber>130</LineNumber>
<EnabledFlag>1</EnabledFlag>
<Address>0</Address>
<ByteObject>0</ByteObject>
<HtxType>0</HtxType>
<ManyObjects>0</ManyObjects>
<SizeOfObject>0</SizeOfObject>
<BreakByAccess>0</BreakByAccess>
<BreakIfRCount>0</BreakIfRCount>
<Filename>..\User\module\shoot_control.c</Filename>
<ExecCommand></ExecCommand>
<Expression></Expression>
</Bp>
<Bp>
<Number>2</Number>
<Type>0</Type>
<LineNumber>132</LineNumber>
<EnabledFlag>1</EnabledFlag> <EnabledFlag>1</EnabledFlag>
<Address>0</Address> <Address>0</Address>
<ByteObject>0</ByteObject> <ByteObject>0</ByteObject>

BIN
MDK-ARM/shoot/config.o
View File

Binary file not shown.

BIN
MDK-ARM/shoot/motor_rm.o
View File

Binary file not shown.

BIN
MDK-ARM/shoot/shoot.axf
View File

Binary file not shown.

2
MDK-ARM/shoot/shoot.build_log.htm
View File

@ -30,7 +30,7 @@ Build target 'shoot'
Note: source file '..\User\bsp\can.c' - object file renamed from 'shoot\can.o' to 'shoot\can_1.o'. Note: source file '..\User\bsp\can.c' - object file renamed from 'shoot\can.o' to 'shoot\can_1.o'.
compiling shoot_control.c... compiling shoot_control.c...
linking... linking...
Program Size: Code=32088 RO-data=660 RW-data=164 ZI-data=21996 Program Size: Code=32304 RO-data=660 RW-data=196 ZI-data=22068
FromELF: creating hex file... FromELF: creating hex file...
"shoot\shoot.axf" - 0 Error(s), 0 Warning(s). "shoot\shoot.axf" - 0 Error(s), 0 Warning(s).

2677
MDK-ARM/shoot/shoot.hex
View File

File diff suppressed because it is too large Load Diff

1402
MDK-ARM/shoot/shoot.htm
View File

File diff suppressed because it is too large Load Diff

1902
MDK-ARM/shoot/shoot.map
View File

File diff suppressed because it is too large Load Diff

BIN
MDK-ARM/shoot/shoot_control.o
View File

Binary file not shown.

BIN
MDK-ARM/shoot/shoot_ctrl.o
View File

Binary file not shown.

14
MDK-ARM/shoot/shoot_shoot.dep
View File

@ -1129,7 +1129,7 @@ I (D:\cangming\ARM\ARMCLANG\include\stdbool.h)(0x6035A4A8)
I (D:\cangming\ARM\ARMCLANG\include\stdint.h)(0x6035A4A8) I (D:\cangming\ARM\ARMCLANG\include\stdint.h)(0x6035A4A8)
I (D:\cangming\ARM\ARMCLANG\include\string.h)(0x6035A4A8) I (D:\cangming\ARM\ARMCLANG\include\string.h)(0x6035A4A8)
F (..\User\device\motor.h)(0x68D3FFBB)() F (..\User\device\motor.h)(0x68D3FFBB)()
F (..\User\device\motor_rm.c)(0x68D3FFDB)(-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/motor_rm.o -MD) F (..\User\device\motor_rm.c)(0x68DD3BCB)(-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/motor_rm.o -MD)
I (..\User\device\motor_rm.h)(0x68D3FFBB) I (..\User\device\motor_rm.h)(0x68D3FFBB)
I (..\User\device\motor.h)(0x68D3FFBB) I (..\User\device\motor.h)(0x68D3FFBB)
I (..\User\device\device.h)(0x68D400B4) I (..\User\device\device.h)(0x68D400B4)
@ -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\stdbool.h)(0x6035A4A8)
I (D:\cangming\ARM\ARMCLANG\include\stddef.h)(0x6035A4A8) I (D:\cangming\ARM\ARMCLANG\include\stddef.h)(0x6035A4A8)
F (..\User\component\pid.h)(0x68DD0ECB)() F (..\User\component\pid.h)(0x68DD0ECB)()
F (..\User\module\shoot_control.c)(0x68DD247A)(-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)(0x68DD4CBD)(-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)(0x68DCDC49) I (..\User\module\shoot_control.h)(0x68DD29DA)
I (..\Core\Inc\main.h)(0x68D1384A) I (..\Core\Inc\main.h)(0x68D1384A)
I (..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal.h)(0x68AD823F) I (..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal.h)(0x68AD823F)
I (..\Core\Inc\stm32f4xx_hal_conf.h)(0x68D1384A) 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 (..\Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.h)(0x68AD8208)
I (D:\cangming\ARM\ARMCLANG\include\string.h)(0x6035A4A8) I (D:\cangming\ARM\ARMCLANG\include\string.h)(0x6035A4A8)
I (..\User\bsp\time.h)(0x68D3FFDB) I (..\User\bsp\time.h)(0x68D3FFDB)
F (..\User\module\shoot_control.h)(0x68DCDC49)() F (..\User\module\shoot_control.h)(0x68DD29DA)()
F (..\User\module\config.c)(0x68DD16FB)(-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)(0x68DD4776)(-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 (..\User\module\config.h)(0x68DABE70)
I (D:\cangming\ARM\ARMCLANG\include\stdint.h)(0x6035A4A8) I (D:\cangming\ARM\ARMCLANG\include\stdint.h)(0x6035A4A8)
I (..\User\component\pid.h)(0x68DD0ECB) I (..\User\component\pid.h)(0x68DD0ECB)
@ -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\task.h)(0x68AD8208)
I (..\Middlewares\Third_Party\FreeRTOS\Source\include\list.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 (..\Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.h)(0x68AD8208)
I (..\User\module\shoot_control.h)(0x68DCDC49) I (..\User\module\shoot_control.h)(0x68DD29DA)
F (..\User\module\config.h)(0x68DABE70)() 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) 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) I (..\User\task\user_task.h)(0x68D3E4B9)
@ -1489,7 +1489,7 @@ I (..\User\component\user_math.h)(0x68D3FFDB)
I (D:\cangming\ARM\ARMCLANG\include\float.h)(0x6035A4A0) I (D:\cangming\ARM\ARMCLANG\include\float.h)(0x6035A4A0)
I (D:\cangming\ARM\ARMCLANG\include\math.h)(0x6035A4A8) I (D:\cangming\ARM\ARMCLANG\include\math.h)(0x6035A4A8)
I (..\User\device\device.h)(0x68D400B4) I (..\User\device\device.h)(0x68D400B4)
I (..\User\module\shoot_control.h)(0x68DCDC49) I (..\User\module\shoot_control.h)(0x68DD29DA)
I (..\Core\Inc\main.h)(0x68D1384A) I (..\Core\Inc\main.h)(0x68D1384A)
I (..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal.h)(0x68AD823F) I (..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal.h)(0x68AD823F)
I (..\Core\Inc\stm32f4xx_hal_conf.h)(0x68D1384A) I (..\Core\Inc\stm32f4xx_hal_conf.h)(0x68D1384A)

48
Ozone/shoot.jdebug.user
View File

@ -1,35 +1,47 @@
GraphedExpression="(((shoot_c).feedback).fric[1]).rotor_speed", Color=#e56a6f, Show=0
GraphedExpression="(((shoot_c).feedback).fric[0]).rotor_speed", Color=#35792b, Show=0 Breakpoint=D:/CUBEMX/shoot/User/device/motor_rm.c:145:32, State=BP_STATE_DISABLED
GraphedExpression="(shoot_c).errtosee", Color=#769dda, Show=0 Breakpoint=D:/CUBEMX/shoot/User/device/motor_rm.c:148:22, State=BP_STATE_DISABLED
OpenDocument="config.c", FilePath="D:/CUBEMX/shoot/User/module/config.c", Line=6 Breakpoint=D:/CUBEMX/shoot/User/module/shoot_control.c:114, State=BP_STATE_DISABLED
OpenDocument="queue.c", FilePath="D:/CUBEMX/shoot/Middlewares/Third_Party/FreeRTOS/Source/queue.c", Line=854 GraphedExpression="((shoot).output).outomg_trig", Color=#e56a6f, Show=0
GraphedExpression="((shoot).output).outagl_trig", Color=#35792b, Show=0
GraphedExpression="((shoot).target_variable).target_angle", Color=#769dda
GraphedExpression="((shoot).feedback).trig_angle_cicle", Color=#b14f0d
GraphedExpression="(((shoot).feedback).trig).torque_current", Color=#b3c38e, Show=0
OpenDocument="motor.h", FilePath="D:/CUBEMX/shoot/User/device/motor.h", Line=0
OpenDocument="tasks.c", FilePath="D:/CUBEMX/shoot/Middlewares/Third_Party/FreeRTOS/Source/tasks.c", Line=3652
OpenDocument="main.c", FilePath="D:/CUBEMX/shoot/Core/Src/main.c", Line=66 OpenDocument="main.c", FilePath="D:/CUBEMX/shoot/Core/Src/main.c", Line=66
OpenDocument="shoot_ctrl.c", FilePath="D:/CUBEMX/shoot/User/task/shoot_ctrl.c", Line=7 OpenDocument="stm32f4xx_hal_can.c", FilePath="D:/CUBEMX/shoot/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_can.c", Line=1558
OpenDocument="shoot_control.h", FilePath="D:/CUBEMX/shoot/User/module/shoot_control.h", Line=42
OpenDocument="config.c", FilePath="D:/CUBEMX/shoot/User/module/config.c", Line=39
OpenDocument="queue.c", FilePath="D:/CUBEMX/shoot/Middlewares/Third_Party/FreeRTOS/Source/queue.c", Line=854
OpenDocument="filter.c", FilePath="D:/CUBEMX/shoot/User/component/filter.c", Line=49
OpenDocument="shoot_ctrl.c", FilePath="D:/CUBEMX/shoot/User/task/shoot_ctrl.c", Line=1
OpenDocument="rc.c", FilePath="D:/CUBEMX/shoot/User/task/rc.c", Line=0 OpenDocument="rc.c", FilePath="D:/CUBEMX/shoot/User/task/rc.c", Line=0
OpenDocument="shoot_control.c", FilePath="D:/CUBEMX/shoot/User/module/shoot_control.c", Line=0 OpenDocument="shoot_control.c", FilePath="D:/CUBEMX/shoot/User/module/shoot_control.c", Line=44
OpenDocument="startup_stm32f407xx.s", FilePath="D:/CUBEMX/shoot/MDK-ARM/startup_stm32f407xx.s", Line=161 OpenDocument="startup_stm32f407xx.s", FilePath="D:/CUBEMX/shoot/MDK-ARM/startup_stm32f407xx.s", Line=161
OpenDocument="tasks.c", FilePath="D:/CUBEMX/shoot/Middlewares/Third_Party/FreeRTOS/Source/tasks.c", Line=3419 OpenDocument="motor_rm.c", FilePath="D:/CUBEMX/shoot/User/device/motor_rm.c", Line=120
OpenToolbar="Debug", Floating=0, x=0, y=0 OpenToolbar="Debug", Floating=0, x=0, y=0
OpenWindow="Registers 1", DockArea=RIGHT, x=0, y=0, w=726, h=919, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, FilteredItems=[], RefreshRate=1 OpenWindow="Registers 1", DockArea=RIGHT, x=0, y=0, w=726, h=919, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, FilteredItems=[], RefreshRate=1
OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=300, h=919, TabPos=0, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=301, h=919, TabPos=0, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
OpenWindow="Watched Data 1", DockArea=RIGHT, x=0, y=0, w=726, h=919, TabPos=0, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 OpenWindow="Watched Data 1", DockArea=RIGHT, x=0, y=0, w=726, h=919, TabPos=0, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
OpenWindow="Functions", DockArea=LEFT, x=0, y=0, w=300, h=919, TabPos=1, TopOfStack=0, 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=1007, h=536, TabPos=0, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0 OpenWindow="Data Sampling", DockArea=BOTTOM, x=0, y=0, w=1007, h=536, TabPos=0, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0
OpenWindow="Timeline", DockArea=BOTTOM, x=1, y=0, w=1552, h=555, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=0, CodePaneShown=0, PinCursor="Cursor Movable", TimePerDiv="200 ms / Div", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="61;0", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="1328;0", CodeGraphLegendShown=0, CodeGraphLegendPosition="1344;0" OpenWindow="Timeline", DockArea=BOTTOM, x=1, y=0, w=1552, h=555, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=0, CodePaneShown=0, PinCursor="Cursor Movable", TimePerDiv="100 ms / Div", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="927;0", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="1328;-69", CodeGraphLegendShown=0, CodeGraphLegendPosition="1344;0"
OpenWindow="Console", DockArea=BOTTOM, x=0, y=0, w=1007, h=536, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 OpenWindow="Console", DockArea=BOTTOM, x=0, y=0, w=1007, h=536, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
SmartViewPlugin="", Page="", Toolbar="Hidden", Window="SmartView 1" SmartViewPlugin="", Page="", Toolbar="Hidden", Window="SmartView 1"
TableHeader="Registers 1", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Description"], ColWidths=[100;144;482] TableHeader="Registers 1", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Description"], ColWidths=[100;144;456]
TableHeader="Functions", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Address";"Size";"#Insts";"Source"], ColWidths=[1594;104;100;100;100] TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh"], ColWidths=[250;282;91;100]
TableHeader="Functions", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Address";"Size";"#Insts";"Source"], ColWidths=[1594;104;100;100;286]
TableHeader="Power Sampling", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";"Ch 0"], ColWidths=[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="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="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="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;103] TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";" ((shoot).output).outomg_trig";" ((shoot).output).outagl_trig";" ((shoot).target_variable).target_angle";" ((shoot).feedback).trig_angle_cicle";" (((shoot).feedback).trig).torque_current"], ColWidths=[100;100;100;100;100;100;100]
TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";" (((shoot_c).feedback).fric[1]).rotor_speed";" (((shoot_c).feedback).fric[0]).rotor_speed";" (shoot_c).errtosee"], ColWidths=[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;134;134;124;144;110;126;126]
TableHeader="Data Sampling Setup", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[406;100;100;100;100;100;110;126;126]
TableHeader="TargetExceptionDialog", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Address";"Description"], ColWidths=[200;100;100;340] 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_ctrl_cmd_rc", RefreshRate=5, Window=Watched Data 1
WatchedExpression="shoot_c", RefreshRate=5, Window=Watched Data 1 WatchedExpression="shoot_cmd", RefreshRate=5, Window=Watched Data 1
WatchedExpression="shoot_cmd", RefreshRate=5, Window=Watched Data 1 WatchedExpression="shoot", RefreshRate=5, Window=Watched Data 1
WatchedExpression="a", Window=Watched Data 1

5
User/device/motor_rm.c
View File

@ -139,13 +139,14 @@ static void Motor_RM_Decode(MOTOR_RM_t *motor, BSP_CAN_Message_t *msg) {
motor->feedback.rotor_speed = rotor_speed; motor->feedback.rotor_speed = rotor_speed;
motor->feedback.torque_current = torque_current; motor->feedback.torque_current = torque_current;
} }
if (motor->motor.reverse) { while (motor->feedback.rotor_abs_angle < 0) {
while (motor->feedback.rotor_abs_angle < 0) {
motor->feedback.rotor_abs_angle += M_2PI; motor->feedback.rotor_abs_angle += M_2PI;
} }
while (motor->feedback.rotor_abs_angle >= M_2PI) { while (motor->feedback.rotor_abs_angle >= M_2PI) {
motor->feedback.rotor_abs_angle -= M_2PI; motor->feedback.rotor_abs_angle -= M_2PI;
} }
if (motor->motor.reverse) {
motor->feedback.rotor_abs_angle = M_2PI - motor->feedback.rotor_abs_angle; motor->feedback.rotor_abs_angle = M_2PI - motor->feedback.rotor_abs_angle;
motor->feedback.rotor_speed = -motor->feedback.rotor_speed; motor->feedback.rotor_speed = -motor->feedback.rotor_speed;
motor->feedback.torque_current = -motor->feedback.torque_current; motor->feedback.torque_current = -motor->feedback.torque_current;

20
User/module/config.c
View File

@ -21,7 +21,7 @@ Config_RobotParam_t robot_config = {
.shoot_param = { .shoot_param = {
.trig_step_angle=M_2PI/8, .trig_step_angle=M_2PI/8,
.shot_delay_time=0.2f, .shot_delay_time=0.2f,
.shot_burst_num=3, .shot_burst_num=4,
.fric_motor_param[0] = { .fric_motor_param[0] = {
.can = BSP_CAN_2, .can = BSP_CAN_2,
@ -66,13 +66,23 @@ Config_RobotParam_t robot_config = {
}, },
.trig = { .trig = {
.k=1.0f, .k=1.0f,
.p=1.2f, .p=1.0f,
.i=0.0f, .i=0.1f,
.d=0.05f, .d=0.05f,
.i_limit=0.8f,
.out_limit=0.5f,
.d_cutoff_freq=-1.0f,
.range=M_2PI,
},
.trig_omg = {
.k=1.0f,
.p=1.5f,
.i=0.3f,
.d=0.5f,
.i_limit=0.2f, .i_limit=0.2f,
.out_limit=0.9f, .out_limit=0.9f,
.d_cutoff_freq=30.0f, .d_cutoff_freq=-1.0f,
.range=M_2PI, .range=-1.0f,
}, },
.filter.fric = { .filter.fric = {
.in = 30.0f, .in = 30.0f,

62
User/module/shoot_control.c
View File

@ -39,6 +39,7 @@ int8_t Shoot_ResetIntegral(shoot_t *s)
PID_ResetIntegral(&s->pid.fric_err[i]); PID_ResetIntegral(&s->pid.fric_err[i]);
} }
PID_ResetIntegral(&s->pid.trig); PID_ResetIntegral(&s->pid.trig);
PID_ResetIntegral(&s->pid.trig_omg);
return 0; return 0;
} }
@ -54,7 +55,8 @@ int8_t Shoot_ResetCalu(shoot_t *s)
LowPassFilter2p_Reset(&s->filter.fric.in[i], 0.0f); LowPassFilter2p_Reset(&s->filter.fric.in[i], 0.0f);
LowPassFilter2p_Reset(&s->filter.fric.out[i], 0.0f); LowPassFilter2p_Reset(&s->filter.fric.out[i], 0.0f);
} }
// PID_Reset(&s->pid.trig); PID_Reset(&s->pid.trig);
PID_Reset(&s->pid.trig_omg);
LowPassFilter2p_Reset(&s->filter.trig.in, 0.0f); LowPassFilter2p_Reset(&s->filter.trig.in, 0.0f);
LowPassFilter2p_Reset(&s->filter.trig.out, 0.0f); LowPassFilter2p_Reset(&s->filter.trig.out, 0.0f);
return 0; return 0;
@ -72,8 +74,9 @@ int8_t Shoot_ResetOutput(shoot_t *s)
s->output.out_fric[i]=0.0f; s->output.out_fric[i]=0.0f;
s->output.lpfout_fric[i]=0.0f; s->output.lpfout_fric[i]=0.0f;
} }
s->output.out_trig=0.0f; s->output.outagl_trig=0.0f;
s->output.lpfout_trig=0.0f; s->output.outomg_trig=0.0f;
s->output.outlpf_trig=0.0f;
return 0; return 0;
} }
@ -82,7 +85,7 @@ int8_t Shoot_CaluTargetRPM(shoot_t *s, float target_speed)
if (s == NULL) { if (s == NULL) {
return -1; // 参数错误 return -1; // 参数错误
} }
s->target_variable.target_rpm=target_speed; s->target_variable.target_rpm=4000.0f/MAX_FRIC_RPM;
return 0; return 0;
} }
/** /**
@ -123,7 +126,8 @@ int8_t Shoot_Init(shoot_t *s, Shoot_Params_t *param, float target_freq)
} }
MOTOR_RM_Register(&param->trig_motor_param); MOTOR_RM_Register(&param->trig_motor_param);
PID_Init(&s->pid.trig, KPID_MODE_CALC_D, target_freq,&param->trig); PID_Init(&s->pid.trig, KPID_MODE_CALC_D, target_freq,&param->trig);
PID_Init(&s->pid.trig_omg, KPID_MODE_CALC_D, target_freq,&param->trig_omg);
LowPassFilter2p_Init(&s->filter.trig.in, target_freq, s->param->filter.trig.in); LowPassFilter2p_Init(&s->filter.trig.in, target_freq, s->param->filter.trig.in);
LowPassFilter2p_Init(&s->filter.trig.out, target_freq, s->param->filter.trig.out); LowPassFilter2p_Init(&s->filter.trig.out, target_freq, s->param->filter.trig.out);
@ -162,7 +166,7 @@ int8_t Chassis_UpdateFeedback(shoot_t *s)
MOTOR_RM_t *motor_fed = MOTOR_RM_GetMotor(&s->param->trig_motor_param); MOTOR_RM_t *motor_fed = MOTOR_RM_GetMotor(&s->param->trig_motor_param);
if(motor_fed!=NULL) if(motor_fed!=NULL)
{ {
s->feedback.trig=motor_fed->motor.feedback; s->feedback.trig=motor_fed->feedback;
} }
/* 将多圈角度归化到单圈 (-M_PI, M_PI) */ /* 将多圈角度归化到单圈 (-M_PI, M_PI) */
s->feedback.trig_angle_cicle = s->feedback.trig.rotor_abs_angle; s->feedback.trig_angle_cicle = s->feedback.trig.rotor_abs_angle;
@ -172,25 +176,16 @@ int8_t Chassis_UpdateFeedback(shoot_t *s)
}else if (s->feedback.trig_angle_cicle < -M_PI) { }else if (s->feedback.trig_angle_cicle < -M_PI) {
s->feedback.trig_angle_cicle += M_2PI; // 调整到 [-π, π] s->feedback.trig_angle_cicle += M_2PI; // 调整到 [-π, π]
} }
// s->feedback.trig_angle_cicle = fmodf(s->feedback.trig.rotor_abs_angle, M_2PI);
// if(s->feedback.trig_angle_cicle<0.0f)
// {
// s->feedback.trig_angle_cicle+=M_2PI;
// }
// s->feedback.trig_angle_cicle = s->feedback.trig.rotor_abs_angle;
// CircleAdd(&s->feedback.trig_angle_cicle, 0.0f, M_2PI);
//
s->feedback.fil_trig_rpm = LowPassFilter2p_Apply(&s->filter.trig.in, s->feedback.trig.rotor_speed); 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; 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; 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; s->errtosee = s->feedback.fric[0].rotor_speed - s->feedback.fric[1].rotor_speed;
return 0; return 0;
} }
float a;
int8_t Shoot_Control(shoot_t *s, Shoot_CMD_t *cmd) int8_t Shoot_Control(shoot_t *s, Shoot_CMD_t *cmd)
{ {
if (s == NULL || cmd == NULL) { if (s == NULL || cmd == NULL) {
@ -199,7 +194,6 @@ int8_t Shoot_Control(shoot_t *s, Shoot_CMD_t *cmd)
s->now = BSP_TIME_Get_us() / 1000000.0f; s->now = BSP_TIME_Get_us() / 1000000.0f;
s->dt = (BSP_TIME_Get_us() - s->lask_wakeup) / 1000000.0f; s->dt = (BSP_TIME_Get_us() - s->lask_wakeup) / 1000000.0f;
s->lask_wakeup = BSP_TIME_Get_us(); s->lask_wakeup = BSP_TIME_Get_us();
// s->running_state = cmd->state;
s->online = cmd->online; s->online = cmd->online;
if(!s->online /*|| s->mode==SHOOT_MODE_SAFE*/){ if(!s->online /*|| s->mode==SHOOT_MODE_SAFE*/){
for(int i=0;i<SHOOT_FRIC_NUM;i++) for(int i=0;i<SHOOT_FRIC_NUM;i++)
@ -209,7 +203,6 @@ int8_t Shoot_Control(shoot_t *s, Shoot_CMD_t *cmd)
MOTOR_RM_Relax(&s->param->trig_motor_param); MOTOR_RM_Relax(&s->param->trig_motor_param);
} }
else{ else{
a=CircleError(s->target_variable.target_angle, s->feedback.trig_angle_cicle, M_2PI);
switch(s->running_state) switch(s->running_state)
{ {
case SHOOT_STATE_IDLE:/*熄火等待*/ case SHOOT_STATE_IDLE:/*熄火等待*/
@ -221,17 +214,20 @@ int8_t Shoot_Control(shoot_t *s, Shoot_CMD_t *cmd)
s->output.lpfout_fric[i] = LowPassFilter2p_Apply(&s->filter.fric.out[i], s->output.out_fric[i]); s->output.lpfout_fric[i] = LowPassFilter2p_Apply(&s->filter.fric.out[i], s->output.out_fric[i]);
MOTOR_RM_SetOutput(&s->param->fric_motor_param[i], s->output.lpfout_fric[i]); MOTOR_RM_SetOutput(&s->param->fric_motor_param[i], s->output.lpfout_fric[i]);
} }
s->output.out_trig=PID_Calc(&s->pid.trig,s->target_variable.target_angle,s->feedback.trig_angle_cicle,0,s->dt); s->output.outagl_trig =PID_Calc(&s->pid.trig,s->target_variable.target_angle,s->feedback.trig_angle_cicle,0,s->dt);
MOTOR_RM_SetOutput(&s->param->trig_motor_param, s->output.out_trig); s->output.outomg_trig =PID_Calc(&s->pid.trig_omg,s->output.outagl_trig,s->feedback.trig_rpm,0,s->dt);
s->output.outlpf_trig =LowPassFilter2p_Apply(&s->filter.trig.out, s->output.outomg_trig);
MOTOR_RM_SetOutput(&s->param->trig_motor_param, s->output.outlpf_trig);
if(cmd->ready) if(cmd->ready)
{ {
Shoot_ResetCalu(s); Shoot_ResetCalu(s);
// Shoot_ResetIntegral(s); Shoot_ResetIntegral(s);
// Shoot_ResetOutput(s); Shoot_ResetOutput(s);
s->running_state=SHOOT_STATE_READY; s->running_state=SHOOT_STATE_READY;
} }
break; break;
case SHOOT_STATE_READY:/*准备射击*/ case SHOOT_STATE_READY:/*准备射击*/
for(int i=0;i<SHOOT_FRIC_NUM;i++) for(int i=0;i<SHOOT_FRIC_NUM;i++)
{ /* 计算跟随输出、计算修正输出 */ { /* 计算跟随输出、计算修正输出 */
s->output.out_follow[i]=PID_Calc(&s->pid.fric_follow[i],s->target_variable.target_rpm/MAX_FRIC_RPM,s->feedback.fric_rpm[i],0,s->dt); s->output.out_follow[i]=PID_Calc(&s->pid.fric_follow[i],s->target_variable.target_rpm/MAX_FRIC_RPM,s->feedback.fric_rpm[i],0,s->dt);
@ -245,14 +241,16 @@ int8_t Shoot_Control(shoot_t *s, Shoot_CMD_t *cmd)
MOTOR_RM_SetOutput(&s->param->fric_motor_param[i], s->output.lpfout_fric[i]); MOTOR_RM_SetOutput(&s->param->fric_motor_param[i], s->output.lpfout_fric[i]);
} }
/* 拨弹电机输出 */ /* 拨弹电机输出 */
s->output.out_trig=PID_Calc(&s->pid.trig,s->target_variable.target_angle,s->feedback.trig_angle_cicle,0,s->dt); s->output.outagl_trig =PID_Calc(&s->pid.trig,s->target_variable.target_angle,s->feedback.trig_angle_cicle,0,s->dt);
MOTOR_RM_SetOutput(&s->param->trig_motor_param, s->output.out_trig); s->output.outomg_trig =PID_Calc(&s->pid.trig_omg,s->output.outagl_trig,s->feedback.trig_rpm,0,s->dt);
s->output.outlpf_trig =LowPassFilter2p_Apply(&s->filter.trig.out, s->output.outomg_trig);
MOTOR_RM_SetOutput(&s->param->trig_motor_param, s->output.outlpf_trig);
/* 检查状态机 */ /* 检查状态机 */
if(!cmd->ready) if(!cmd->ready)
{ {
// Shoot_ResetCalu(s); Shoot_ResetCalu(s);
// Shoot_ResetOutput(s); Shoot_ResetOutput(s);
s->running_state=SHOOT_STATE_IDLE; s->running_state=SHOOT_STATE_IDLE;
} }
else if(last_firecmd==false&&cmd->firecmd==true) else if(last_firecmd==false&&cmd->firecmd==true)
@ -260,7 +258,7 @@ int8_t Shoot_Control(shoot_t *s, Shoot_CMD_t *cmd)
Shoot_ResetCalu(s); Shoot_ResetCalu(s);
Shoot_ResetOutput(s); Shoot_ResetOutput(s);
s->running_state=SHOOT_STATE_FIRE; s->running_state=SHOOT_STATE_FIRE;
s->shoot_Anglecalu.num_to_shoot+=1; s->shoot_Anglecalu.num_to_shoot+=s->param->shot_burst_num;
} }
break; break;
@ -275,8 +273,10 @@ int8_t Shoot_Control(shoot_t *s, Shoot_CMD_t *cmd)
s->output.lpfout_fric[i] = LowPassFilter2p_Apply(&s->filter.fric.out[i], s->output.out_fric[i]); s->output.lpfout_fric[i] = LowPassFilter2p_Apply(&s->filter.fric.out[i], s->output.out_fric[i]);
MOTOR_RM_SetOutput(&s->param->fric_motor_param[i], s->output.lpfout_fric[i]); MOTOR_RM_SetOutput(&s->param->fric_motor_param[i], s->output.lpfout_fric[i]);
} }
s->output.out_trig=PID_Calc(&s->pid.trig,s->target_variable.target_angle,s->feedback.trig_angle_cicle,0,s->dt); s->output.outagl_trig =PID_Calc(&s->pid.trig,s->target_variable.target_angle,s->feedback.trig_angle_cicle,0,s->dt);
MOTOR_RM_SetOutput(&s->param->trig_motor_param, s->output.out_trig); s->output.outomg_trig =PID_Calc(&s->pid.trig_omg,s->output.outagl_trig,s->feedback.trig_rpm,0,s->dt);
s->output.outlpf_trig =LowPassFilter2p_Apply(&s->filter.trig.out, s->output.outomg_trig);
MOTOR_RM_SetOutput(&s->param->trig_motor_param, s->output.outlpf_trig);
if(!cmd->firecmd) if(!cmd->firecmd)
{ {
Shoot_ResetCalu(s); Shoot_ResetCalu(s);

10
User/module/shoot_control.h
View File

@ -23,7 +23,7 @@ extern "C" {
#define SHOOT_FRIC_NUM (2) /* 摩擦轮数量 */ #define SHOOT_FRIC_NUM (2) /* 摩擦轮数量 */
#define MAX_FRIC_RPM 7000.0f #define MAX_FRIC_RPM 7000.0f
#define MAX_TRIG_RPM 1000.0f #define MAX_TRIG_RPM 5000.0f
/* Exported macro ----------------------------------------------------------- */ /* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */ /* Exported types ----------------------------------------------------------- */
@ -74,8 +74,9 @@ typedef struct {
float lpfout_fric[SHOOT_FRIC_NUM]; float lpfout_fric[SHOOT_FRIC_NUM];
float out_trig; float outagl_trig;
float lpfout_trig; float outomg_trig;
float outlpf_trig;
}Shoot_Output_t; }Shoot_Output_t;
@ -92,7 +93,7 @@ typedef struct {
KPID_Params_t fric_follow; /* 摩擦轮电机PID控制参数，用于跟随目标速度 */ KPID_Params_t fric_follow; /* 摩擦轮电机PID控制参数，用于跟随目标速度 */
KPID_Params_t fric_err; /* 摩擦轮电机PID控制参数，用于消除转速误差 */ KPID_Params_t fric_err; /* 摩擦轮电机PID控制参数，用于消除转速误差 */
KPID_Params_t trig; /* 拨弹电机PID控制参数 */ KPID_Params_t trig; /* 拨弹电机PID控制参数 */
KPID_Params_t trig_omg; /* 拨弹电机PID控制参数 */
/* 低通滤波器截止频率 */ /* 低通滤波器截止频率 */
struct { struct {
@ -138,6 +139,7 @@ typedef struct {
KPID_t fric_follow[SHOOT_FRIC_NUM]; /* */ KPID_t fric_follow[SHOOT_FRIC_NUM]; /* */
KPID_t fric_err[SHOOT_FRIC_NUM]; /* */ KPID_t fric_err[SHOOT_FRIC_NUM]; /* */
KPID_t trig; KPID_t trig;
KPID_t trig_omg;
} pid; } pid;
/* 滤波器 */ /* 滤波器 */