xiaocheng/duolun
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

一代版本

Browse Source
This commit is contained in:
Xiaocheng 2025-10-12 10:53:36 +08:00
parent 8b8a89be62
commit 4c7bbbb22d
20 changed files with 6707 additions and 18189 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
.vscode/settings.json vendored
View File

@ -3,6 +3,7 @@
"chassis.h": "c",
"buzzer.h": "c",
"math.h": "c",
"ledi.h": "c"
"ledi.h": "c",
"string.h": "c"
}
}

11580
MDK-ARM/JLinkLog.txt
View File

File diff suppressed because it is too large Load Diff

282
MDK-ARM/zzzzh.uvguix.zhaoruihong
View File

File diff suppressed because one or more lines are too long

82
MDK-ARM/zzzzh.uvoptx
View File

@ -145,7 +145,7 @@
<SetRegEntry>
<Number>0</Number>
<Key>CMSIS_AGDI</Key>
<Name>-X"Any" -UAny -O206 -S8 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO65554 -TC10000000 -TT10000000 -TP20 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO7 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM)</Name>
<Name>-X"Any" -UAny -O206 -S8 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO65554 -TC10000000 -TT10000000 -TP20 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM)</Name>
</SetRegEntry>
<SetRegEntry>
<Number>0</Number>
@ -158,24 +158,7 @@
<Name>-U-O142 -O2254 -S0 -C0 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO18 -TC10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO7 -FD20000000 -FC800 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM)</Name>
</SetRegEntry>
</TargetDriverDllRegistry>
<Breakpoint>
<Bp>
<Number>0</Number>
<Type>0</Type>
<LineNumber>357</LineNumber>
<EnabledFlag>1</EnabledFlag>
<Address>134227130</Address>
<ByteObject>0</ByteObject>
<HtxType>0</HtxType>
<ManyObjects>0</ManyObjects>
<SizeOfObject>0</SizeOfObject>
<BreakByAccess>0</BreakByAccess>
<BreakIfRCount>1</BreakIfRCount>
<Filename>..\User\bsp\can.c</Filename>
<ExecCommand></ExecCommand>
<Expression>\\zzzzh\../User/bsp/can.c\357</Expression>
</Bp>
</Breakpoint>
<Breakpoint/>
<WatchWindow1>
<Ww>
<count>0</count>
@ -185,57 +168,17 @@
<Ww>
<count>1</count>
<WinNumber>1</WinNumber>
<ItemText>bmi088,0x0A</ItemText>
<ItemText>ceshi,0x0A</ItemText>
</Ww>
<Ww>
<count>2</count>
<WinNumber>1</WinNumber>
<ItemText>cmd_chassis,0x0A</ItemText>
<ItemText>motor-&gt;param</ItemText>
</Ww>
<Ww>
<count>3</count>
<WinNumber>1</WinNumber>
<ItemText>remote_ready,0x0A</ItemText>
</Ww>
<Ww>
<count>4</count>
<WinNumber>1</WinNumber>
<ItemText>ld,0x0A</ItemText>
</Ww>
<Ww>
<count>5</count>
<WinNumber>1</WinNumber>
<ItemText>c_cmd,0x0A</ItemText>
</Ww>
<Ww>
<count>6</count>
<WinNumber>1</WinNumber>
<ItemText>chassis,0x10</ItemText>
</Ww>
<Ww>
<count>7</count>
<WinNumber>1</WinNumber>
<ItemText>cmd_chassis,0x0A</ItemText>
</Ww>
<Ww>
<count>8</count>
<WinNumber>1</WinNumber>
<ItemText>ld_data,0x0A</ItemText>
</Ww>
<Ww>
<count>9</count>
<WinNumber>1</WinNumber>
<ItemText>ceshi,0x0A</ItemText>
</Ww>
<Ww>
<count>10</count>
<WinNumber>1</WinNumber>
<ItemText>chassis_ceshi,0x0A</ItemText>
</Ww>
<Ww>
<count>11</count>
<WinNumber>1</WinNumber>
<ItemText>motor-&gt;param</ItemText>
<ItemText>chassis</ItemText>
</Ww>
</WatchWindow1>
<WatchWindow2>
@ -244,21 +187,6 @@
<WinNumber>2</WinNumber>
<ItemText>c-&gt;keeping_angle_flag,0x0A</ItemText>
</Ww>
<Ww>
<count>1</count>
<WinNumber>2</WinNumber>
<ItemText>cmd_chassis</ItemText>
</Ww>
<Ww>
<count>2</count>
<WinNumber>2</WinNumber>
<ItemText>task_runtime.msgq.cmd_chassis,0x0A</ItemText>
</Ww>
<Ww>
<count>3</count>
<WinNumber>2</WinNumber>
<ItemText>task_runtime</ItemText>
</Ww>
</WatchWindow2>
<Tracepoint>
<THDelay>0</THDelay>

BIN
MDK-ARM/zzzzh/chassis.o
View File

Binary file not shown.

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

Binary file not shown.

BIN
MDK-ARM/zzzzh/task2.o
View File

Binary file not shown.

BIN
MDK-ARM/zzzzh/task3.o
View File

Binary file not shown.

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

Binary file not shown.

5
MDK-ARM/zzzzh/zzzzh.build_log.htm
View File

@ -30,9 +30,10 @@ Build target 'zzzzh'
Note: source file '..\User\bsp\can.c' - object file renamed from 'zzzzh\can.o' to 'zzzzh\can_1.o'.
Note: source file '..\User\bsp\gpio.c' - object file renamed from 'zzzzh\gpio.o' to 'zzzzh\gpio_1.o'.
Note: source file '..\User\bsp\spi.c' - object file renamed from 'zzzzh\spi.o' to 'zzzzh\spi_1.o'.
compiling Task2.c...
compiling chassis.c...
linking...
Program Size: Code=81472 RO-data=1364 RW-data=260 ZI-data=23700
Program Size: Code=81904 RO-data=1364 RW-data=260 ZI-data=23988
FromELF: creating hex file...
"zzzzh\zzzzh.axf" - 0 Error(s), 0 Warning(s).
@ -58,7 +59,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:03
</pre>
</body>
</html>

9111
MDK-ARM/zzzzh/zzzzh.hex
View File

File diff suppressed because it is too large Load Diff

22
MDK-ARM/zzzzh/zzzzh.htm
View File

@ -3,9 +3,9 @@
<title>Static Call Graph - [zzzzh\zzzzh.axf]</title></head>
<body><HR>
<H1>Static Call Graph for image zzzzh\zzzzh.axf</H1><HR>
<BR><P>#&#060CALLGRAPH&#062# ARM Linker, 6160001: Last Updated: Sun Oct 5 17:36:04 2025
<BR><P>#&#060CALLGRAPH&#062# ARM Linker, 6160001: Last Updated: Sat Oct 11 19:35:16 2025
<BR><P>
<H3>Maximum Stack Usage = 1088 bytes + Unknown(Functions without stacksize, Cycles, Untraceable Function Pointers)</H3><H3>
<H3>Maximum Stack Usage = 1032 bytes + Unknown(Functions without stacksize, Cycles, Untraceable Function Pointers)</H3><H3>
Call chain for Maximum Stack Depth:</H3>
Task_Task3 &rArr; Chassis_Control &rArr; Chassis_speed_calculate &rArr; __hardfp_sin &rArr; __ieee754_rem_pio2 &rArr; __aeabi_d2iz
<P>
@ -994,8 +994,8 @@ Global Symbols
</UL>
<BR>[Address Reference Count : 1]<UL><LI> startup_stm32f407xx.o(RESET)
</UL>
<P><STRONG><a name="[e4]"></a>Chassis_Control</STRONG> (Thumb, 500 bytes, Stack size 56 bytes, chassis.o(.text.Chassis_Control))
<BR><BR>[Stack]<UL><LI>Max Depth = 1040<LI>Call Chain = Chassis_Control &rArr; Chassis_speed_calculate &rArr; __hardfp_sin &rArr; __ieee754_rem_pio2 &rArr; __aeabi_d2iz
<P><STRONG><a name="[e4]"></a>Chassis_Control</STRONG> (Thumb, 600 bytes, Stack size 40 bytes, chassis.o(.text.Chassis_Control))
<BR><BR>[Stack]<UL><LI>Max Depth = 984<LI>Call Chain = Chassis_Control &rArr; Chassis_speed_calculate &rArr; __hardfp_sin &rArr; __ieee754_rem_pio2 &rArr; __aeabi_d2iz
</UL>
<BR>[Calls]<UL><LI><a href="#[e9]">&gt;&gt;</a>&nbsp;&nbsp;&nbsp;Chassis_speed_calculate
<LI><a href="#[e7]">&gt;&gt;</a>&nbsp;&nbsp;&nbsp;Chassis_SetMode
@ -1016,8 +1016,8 @@ Global Symbols
<BR>[Called By]<UL><LI><a href="#[6f]">&gt;&gt;</a>&nbsp;&nbsp;&nbsp;Task_Task3
</UL>
<P><STRONG><a name="[e9]"></a>Chassis_speed_calculate</STRONG> (Thumb, 3960 bytes, Stack size 800 bytes, chassis.o(.text.Chassis_speed_calculate))
<BR><BR>[Stack]<UL><LI>Max Depth = 984<LI>Call Chain = Chassis_speed_calculate &rArr; __hardfp_sin &rArr; __ieee754_rem_pio2 &rArr; __aeabi_d2iz
<P><STRONG><a name="[e9]"></a>Chassis_speed_calculate</STRONG> (Thumb, 3976 bytes, Stack size 760 bytes, chassis.o(.text.Chassis_speed_calculate))
<BR><BR>[Stack]<UL><LI>Max Depth = 944<LI>Call Chain = Chassis_speed_calculate &rArr; __hardfp_sin &rArr; __ieee754_rem_pio2 &rArr; __aeabi_d2iz
</UL>
<BR>[Calls]<UL><LI><a href="#[f5]">&gt;&gt;</a>&nbsp;&nbsp;&nbsp;__hardfp_sqrt
<LI><a href="#[f1]">&gt;&gt;</a>&nbsp;&nbsp;&nbsp;__hardfp_sin
@ -1034,7 +1034,7 @@ Global Symbols
<BR>[Called By]<UL><LI><a href="#[e4]">&gt;&gt;</a>&nbsp;&nbsp;&nbsp;Chassis_Control
</UL>
<P><STRONG><a name="[f7]"></a>Chassis_update</STRONG> (Thumb, 272 bytes, Stack size 24 bytes, chassis.o(.text.Chassis_update))
<P><STRONG><a name="[f7]"></a>Chassis_update</STRONG> (Thumb, 348 bytes, Stack size 24 bytes, chassis.o(.text.Chassis_update))
<BR><BR>[Stack]<UL><LI>Max Depth = 380<LI>Call Chain = Chassis_update &rArr; MOTOR_RM_UpdateAll &rArr; MOTOR_RM_Update &rArr; BSP_CAN_GetMessage &rArr; osMutexRelease &rArr; xQueueGiveMutexRecursive &rArr; xQueueGenericSend &rArr; prvCopyDataToQueue &rArr; xTaskPriorityDisinherit &rArr; vListInsertEnd
</UL>
<BR>[Calls]<UL><LI><a href="#[f8]">&gt;&gt;</a>&nbsp;&nbsp;&nbsp;MOTOR_RM_UpdateAll
@ -2321,8 +2321,8 @@ Global Symbols
</UL>
<BR>[Address Reference Count : 1]<UL><LI> init.o(.text.Task_Init)
</UL>
<P><STRONG><a name="[6e]"></a>Task_Task2</STRONG> (Thumb, 376 bytes, Stack size 40 bytes, task2.o(.text.Task_Task2))
<BR><BR>[Stack]<UL><LI>Max Depth = 236<LI>Call Chain = Task_Task2 &rArr; osMessageQueuePut &rArr; xQueueGenericSendFromISR &rArr; prvCopyDataToQueue &rArr; xTaskPriorityDisinherit &rArr; vListInsertEnd
<P><STRONG><a name="[6e]"></a>Task_Task2</STRONG> (Thumb, 472 bytes, Stack size 48 bytes, task2.o(.text.Task_Task2))
<BR><BR>[Stack]<UL><LI>Max Depth = 244<LI>Call Chain = Task_Task2 &rArr; osMessageQueuePut &rArr; xQueueGenericSendFromISR &rArr; prvCopyDataToQueue &rArr; xTaskPriorityDisinherit &rArr; vListInsertEnd
</UL>
<BR>[Calls]<UL><LI><a href="#[c8]">&gt;&gt;</a>&nbsp;&nbsp;&nbsp;osMessageQueuePut
<LI><a href="#[dc]">&gt;&gt;</a>&nbsp;&nbsp;&nbsp;osKernelGetTickFreq
@ -2337,7 +2337,7 @@ Global Symbols
<BR>[Address Reference Count : 1]<UL><LI> init.o(.text.Task_Init)
</UL>
<P><STRONG><a name="[6f]"></a>Task_Task3</STRONG> (Thumb, 380 bytes, Stack size 48 bytes, task3.o(.text.Task_Task3))
<BR><BR>[Stack]<UL><LI>Max Depth = 1088 + Unknown Stack Size
<BR><BR>[Stack]<UL><LI>Max Depth = 1032 + Unknown Stack Size
<LI>Call Chain = Task_Task3 &rArr; Chassis_Control &rArr; Chassis_speed_calculate &rArr; __hardfp_sin &rArr; __ieee754_rem_pio2 &rArr; __aeabi_d2iz
</UL>
<BR>[Calls]<UL><LI><a href="#[b8]">&gt;&gt;</a>&nbsp;&nbsp;&nbsp;osMessageQueueGet
@ -2407,7 +2407,7 @@ Global Symbols
<P><STRONG><a name="[5]"></a>UsageFault_Handler</STRONG> (Thumb, 4 bytes, Stack size 0 bytes, stm32f4xx_it.o(.text.UsageFault_Handler))
<BR>[Address Reference Count : 1]<UL><LI> startup_stm32f407xx.o(RESET)
</UL>
<P><STRONG><a name="[19b]"></a>chassis_init</STRONG> (Thumb, 424 bytes, Stack size 72 bytes, chassis.o(.text.chassis_init))
<P><STRONG><a name="[19b]"></a>chassis_init</STRONG> (Thumb, 568 bytes, Stack size 72 bytes, chassis.o(.text.chassis_init))
<BR><BR>[Stack]<UL><LI>Max Depth = 396 + Unknown Stack Size
<LI>Call Chain = chassis_init &rArr; BSP_CAN_Init &rArr; osMutexNew &rArr; xQueueCreateMutexStatic &rArr; prvInitialiseMutex &rArr; xQueueGenericSend &rArr; prvCopyDataToQueue &rArr; xTaskPriorityDisinherit &rArr; vListInsertEnd
</UL>

3490
MDK-ARM/zzzzh/zzzzh.map
View File

File diff suppressed because it is too large Load Diff

28
MDK-ARM/zzzzh/zzzzh_zzzzh.dep
View File

@ -141,7 +141,7 @@ I (..\User\component\user_math.h)(0x68D3DC73)
I (C:\Keil_v5\ARM\ARMCLANG\include\float.h)(0x6035A4A0)
I (C:\Keil_v5\ARM\ARMCLANG\include\math.h)(0x6035A4A8)
I (..\User\device\device.h)(0x68D3DC74)
I (..\User\module\chassis.h)(0x68E23267)
I (..\User\module\chassis.h)(0x68EA13AA)
I (..\User\bsp\struct_typedef.h)(0x67C19BD3)
I (..\User\component\filter.h)(0x68D3DC73)
I (..\User\component\pid.h)(0x68D3DC73)
@ -1971,7 +1971,7 @@ I (C:\Keil_v5\ARM\ARMCLANG\include\stdbool.h)(0x6035A4A8)
I (C:\Keil_v5\ARM\ARMCLANG\include\stdint.h)(0x6035A4A8)
I (C:\Keil_v5\ARM\ARMCLANG\include\stddef.h)(0x6035A4A8)
I (C:\Keil_v5\ARM\ARMCLANG\include\string.h)(0x6035A4A8)
F (..\User\module\config.c)(0x68E237E0)(-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 -O0 -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 ../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 ../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I ../Drivers/CMSIS/Include -I ../User -I./RTE/_zzzzh -IC:/Users/zhaoruihong/AppData/Local/Arm/Packs/ARM/CMSIS/5.7.0/CMSIS/Core/Include -IC:/Users/zhaoruihong/AppData/Local/Arm/Packs/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 zzzzh/config.o -MD)
F (..\User\module\config.c)(0x68EA0C7C)(-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 -O0 -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 ../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 ../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I ../Drivers/CMSIS/Include -I ../User -I./RTE/_zzzzh -IC:/Users/zhaoruihong/AppData/Local/Arm/Packs/ARM/CMSIS/5.7.0/CMSIS/Core/Include -IC:/Users/zhaoruihong/AppData/Local/Arm/Packs/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 zzzzh/config.o -MD)
I (..\User\module\config.h)(0x68DFF67D)
I (..\User\device\bmi088.h)(0x68D3FAF0)
I (C:\Keil_v5\ARM\ARMCLANG\include\stdbool.h)(0x6035A4A8)
@ -1982,7 +1982,7 @@ I (C:\Keil_v5\ARM\ARMCLANG\include\float.h)(0x6035A4A0)
I (C:\Keil_v5\ARM\ARMCLANG\include\math.h)(0x6035A4A8)
I (C:\Keil_v5\ARM\ARMCLANG\include\stddef.h)(0x6035A4A8)
I (..\User\device\device.h)(0x68D3DC74)
I (..\User\module\chassis.h)(0x68E23267)
I (..\User\module\chassis.h)(0x68EA13AA)
I (..\User\bsp\struct_typedef.h)(0x67C19BD3)
I (..\User\component\filter.h)(0x68D3DC73)
I (..\User\component\pid.h)(0x68D3DC73)
@ -2038,8 +2038,8 @@ I (..\Middlewares\Third_Party\FreeRTOS\Source\include\task.h)(0x68D7DF14)
I (..\Middlewares\Third_Party\FreeRTOS\Source\include\list.h)(0x68D7DF14)
I (..\Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.h)(0x68D7DF14)
I (C:\Keil_v5\ARM\ARMCLANG\include\string.h)(0x6035A4A8)
F (..\User\module\chassis.c)(0x68E23C03)(-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 -O0 -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 ../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 ../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I ../Drivers/CMSIS/Include -I ../User -I./RTE/_zzzzh -IC:/Users/zhaoruihong/AppData/Local/Arm/Packs/ARM/CMSIS/5.7.0/CMSIS/Core/Include -IC:/Users/zhaoruihong/AppData/Local/Arm/Packs/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 zzzzh/chassis.o -MD)
I (..\User\module\chassis.h)(0x68E23267)
F (..\User\module\chassis.c)(0x68EA409E)(-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 -O0 -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 ../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 ../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I ../Drivers/CMSIS/Include -I ../User -I./RTE/_zzzzh -IC:/Users/zhaoruihong/AppData/Local/Arm/Packs/ARM/CMSIS/5.7.0/CMSIS/Core/Include -IC:/Users/zhaoruihong/AppData/Local/Arm/Packs/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 zzzzh/chassis.o -MD)
I (..\User\module\chassis.h)(0x68EA13AA)
I (..\User\bsp\struct_typedef.h)(0x67C19BD3)
I (..\User\component\filter.h)(0x68D3DC73)
I (..\User\component\user_math.h)(0x68D3DC73)
@ -2126,7 +2126,7 @@ I (..\User\component\user_math.h)(0x68D3DC73)
I (C:\Keil_v5\ARM\ARMCLANG\include\float.h)(0x6035A4A0)
I (C:\Keil_v5\ARM\ARMCLANG\include\math.h)(0x6035A4A8)
I (..\User\device\device.h)(0x68D3DC74)
I (..\User\module\chassis.h)(0x68E23267)
I (..\User\module\chassis.h)(0x68EA13AA)
I (..\User\bsp\struct_typedef.h)(0x67C19BD3)
I (..\User\component\filter.h)(0x68D3DC73)
I (..\User\component\pid.h)(0x68D3DC73)
@ -2198,7 +2198,7 @@ I (..\User\component\user_math.h)(0x68D3DC73)
I (C:\Keil_v5\ARM\ARMCLANG\include\float.h)(0x6035A4A0)
I (C:\Keil_v5\ARM\ARMCLANG\include\math.h)(0x6035A4A8)
I (..\User\device\device.h)(0x68D3DC74)
I (..\User\module\chassis.h)(0x68E23267)
I (..\User\module\chassis.h)(0x68EA13AA)
I (..\User\bsp\struct_typedef.h)(0x67C19BD3)
I (..\User\component\filter.h)(0x68D3DC73)
I (..\User\component\pid.h)(0x68D3DC73)
@ -2243,7 +2243,7 @@ I (..\User\bsp\can.h)(0x68D3DC73)
I (..\Core\Inc\can.h)(0x68D394E3)
I (..\User\bsp\mm.h)(0x68D3DC73)
I (..\Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os.h)(0x68D7DF14)
F (..\User\task\Task2.c)(0x68E23218)(-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 -O0 -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 ../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 ../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I ../Drivers/CMSIS/Include -I ../User -I./RTE/_zzzzh -IC:/Users/zhaoruihong/AppData/Local/Arm/Packs/ARM/CMSIS/5.7.0/CMSIS/Core/Include -IC:/Users/zhaoruihong/AppData/Local/Arm/Packs/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 zzzzh/task2.o -MD)
F (..\User\task\Task2.c)(0x68EA40EF)(-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 -O0 -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 ../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 ../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I ../Drivers/CMSIS/Include -I ../User -I./RTE/_zzzzh -IC:/Users/zhaoruihong/AppData/Local/Arm/Packs/ARM/CMSIS/5.7.0/CMSIS/Core/Include -IC:/Users/zhaoruihong/AppData/Local/Arm/Packs/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 zzzzh/task2.o -MD)
I (..\User\task\user_task.h)(0x68E0CECE)
I (..\Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.h)(0x68D7DF14)
I (C:\Keil_v5\ARM\ARMCLANG\include\stdint.h)(0x6035A4A8)
@ -2265,7 +2265,7 @@ I (..\User\component\user_math.h)(0x68D3DC73)
I (C:\Keil_v5\ARM\ARMCLANG\include\float.h)(0x6035A4A0)
I (C:\Keil_v5\ARM\ARMCLANG\include\math.h)(0x6035A4A8)
I (..\User\device\device.h)(0x68D3DC74)
I (..\User\module\chassis.h)(0x68E23267)
I (..\User\module\chassis.h)(0x68EA13AA)
I (..\User\bsp\struct_typedef.h)(0x67C19BD3)
I (..\User\component\filter.h)(0x68D3DC73)
I (..\User\component\pid.h)(0x68D3DC73)
@ -2315,7 +2315,7 @@ I (..\User\device\ledi.h)(0x68DF78CA)
I (..\Core\Inc\usart.h)(0x68D394E4)
I (C:\Keil_v5\ARM\ARMCLANG\include\string.h)(0x6035A4A8)
I (C:\Keil_v5\ARM\ARMCLANG\include\stdlib.h)(0x6035A4A8)
F (..\User\task\Task3.c)(0x68E22D67)(-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 -O0 -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 ../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 ../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I ../Drivers/CMSIS/Include -I ../User -I./RTE/_zzzzh -IC:/Users/zhaoruihong/AppData/Local/Arm/Packs/ARM/CMSIS/5.7.0/CMSIS/Core/Include -IC:/Users/zhaoruihong/AppData/Local/Arm/Packs/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 zzzzh/task3.o -MD)
F (..\User\task\Task3.c)(0x68E7C201)(-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 -O0 -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 ../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 ../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I ../Drivers/CMSIS/Include -I ../User -I./RTE/_zzzzh -IC:/Users/zhaoruihong/AppData/Local/Arm/Packs/ARM/CMSIS/5.7.0/CMSIS/Core/Include -IC:/Users/zhaoruihong/AppData/Local/Arm/Packs/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 zzzzh/task3.o -MD)
I (..\User\task\user_task.h)(0x68E0CECE)
I (..\Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.h)(0x68D7DF14)
I (C:\Keil_v5\ARM\ARMCLANG\include\stdint.h)(0x6035A4A8)
@ -2337,7 +2337,7 @@ I (..\User\component\user_math.h)(0x68D3DC73)
I (C:\Keil_v5\ARM\ARMCLANG\include\float.h)(0x6035A4A0)
I (C:\Keil_v5\ARM\ARMCLANG\include\math.h)(0x6035A4A8)
I (..\User\device\device.h)(0x68D3DC74)
I (..\User\module\chassis.h)(0x68E23267)
I (..\User\module\chassis.h)(0x68EA13AA)
I (..\User\bsp\struct_typedef.h)(0x67C19BD3)
I (..\User\component\filter.h)(0x68D3DC73)
I (..\User\component\pid.h)(0x68D3DC73)
@ -2408,7 +2408,7 @@ I (..\User\component\user_math.h)(0x68D3DC73)
I (C:\Keil_v5\ARM\ARMCLANG\include\float.h)(0x6035A4A0)
I (C:\Keil_v5\ARM\ARMCLANG\include\math.h)(0x6035A4A8)
I (..\User\device\device.h)(0x68D3DC74)
I (..\User\module\chassis.h)(0x68E23267)
I (..\User\module\chassis.h)(0x68EA13AA)
I (..\User\bsp\struct_typedef.h)(0x67C19BD3)
I (..\User\component\filter.h)(0x68D3DC73)
I (..\User\component\pid.h)(0x68D3DC73)
@ -2475,7 +2475,7 @@ I (..\User\component\user_math.h)(0x68D3DC73)
I (C:\Keil_v5\ARM\ARMCLANG\include\float.h)(0x6035A4A0)
I (C:\Keil_v5\ARM\ARMCLANG\include\math.h)(0x6035A4A8)
I (..\User\device\device.h)(0x68D3DC74)
I (..\User\module\chassis.h)(0x68E23267)
I (..\User\module\chassis.h)(0x68EA13AA)
I (..\User\bsp\struct_typedef.h)(0x67C19BD3)
I (..\User\component\filter.h)(0x68D3DC73)
I (..\User\component\pid.h)(0x68D3DC73)
@ -2542,7 +2542,7 @@ I (..\User\component\user_math.h)(0x68D3DC73)
I (C:\Keil_v5\ARM\ARMCLANG\include\float.h)(0x6035A4A0)
I (C:\Keil_v5\ARM\ARMCLANG\include\math.h)(0x6035A4A8)
I (..\User\device\device.h)(0x68D3DC74)
I (..\User\module\chassis.h)(0x68E23267)
I (..\User\module\chassis.h)(0x68EA13AA)
I (..\User\bsp\struct_typedef.h)(0x67C19BD3)
I (..\User\component\filter.h)(0x68D3DC73)
I (..\User\component\pid.h)(0x68D3DC73)

216
User/module/chassis.c
View File

@ -12,7 +12,6 @@
#include "component/pid.h"
#include "component/filter.h"
/*舵轮舵向校准方法：注释掉关于6020反馈角度的处理以及6020数据的发送这两处(define.h里有快捷方法)，
进debug将四个轮子编码器朝右（左右无所谓，可能会导致5065方向反，在解算里加个负号就行）
查看6020反馈值，将6020反馈值放入motor_offset中*/
@ -32,7 +31,7 @@ fp32 vofa_send[8]; //vofa输出数据
/*底盘初始化*/
int8_t chassis_init(Chassis_t *c, Chassis_Param_t *param, float target_freq)
int8_t chassis_init(Chassis_t *c, Chassis_Param_t *param, float target_freq)
{
if (c == NULL || param == NULL || target_freq <= 0.0f)
{
@ -54,23 +53,35 @@ int8_t chassis_init(Chassis_t *c, Chassis_Param_t *param, float target_freq)
}
// 舵轮安装时的6020机械误差，机械校准时1号轮在左前方，所有轮的编码器朝向右面
MotorOffset_t motor_offset = {{6.28088474/M_PI*180.0f, 2.08161187/M_PI*180.0f, 2.09618473/M_PI*180.0f, 5.25465155/M_PI*180.0f}};
MotorOffset_t motor_offset = {{3.14005876 / M_PI * 180.0f, 5.25618553 / M_PI * 180.0f, 2.10462165 / M_PI * 180.0f, 5.26108162 / M_PI * 180.0f}}; // 右右右右
// MotorOffset_t motor_offset = {{0.0 / M_PI * 180.0f, 2.07317519 / M_PI * 180.0f, 5.23394215 / M_PI * 180.0f, 2.12379646 / M_PI * 180.0f}};//左左左左
// MotorOffset_t motor_offset = {{6.28088474 / M_PI * 180.0f, 2.08161187 / M_PI * 180.0f, 2.09618473 / M_PI * 180.0f, 5.25465155 / M_PI * 180.0f}};//左左右右
c->motoroffset = motor_offset;
/*对3508的速度环和6020的角速度以及位置环pid进行初始化*/
for (int i = 0; i < 4; i++)
{
PID_Init(&c->pid.chassis_3508VPID[i], KPID_MODE_NO_D, target_freq, &c->param->M3508v_param);
PID_Init(&c->pid.chassis_3508VPID[i], KPID_MODE_CALC_D, target_freq, &c->param->M3508v_param);
}
for (int i = 0; i < 4; i++)
{
PID_Init(&c->pid.chassis_6020OmegaPid[i], KPID_MODE_NO_D, target_freq, &c->param->C6020Omega_param);
PID_Init(&c->pid.chassis_6020anglePid[i], KPID_MODE_NO_D, target_freq, &c->param->C6020Angle_param);
PID_Init(&c->pid.chassis_6020OmegaPid[i], KPID_MODE_CALC_D, target_freq, &c->param->C6020Omega_param);
PID_Init(&c->pid.chassis_6020anglePid[i], KPID_MODE_CALC_D, target_freq, &c->param->C6020Angle_param);
}
/*对遥控器的xyw进行低通滤波*/
LowPassFilter2p_Init(&c->filled[0], target_freq, 20.0f); // vx
LowPassFilter2p_Init(&c->filled[1], target_freq, 20.0f); // vy
LowPassFilter2p_Init(&c->filled[2], target_freq, 20.0f); // vw
LowPassFilter2p_Init(&c->filled[3], target_freq, 20.0f); // 3508-1
LowPassFilter2p_Init(&c->filled[4], target_freq, 20.0f); // 3508-2
LowPassFilter2p_Init(&c->filled[5], target_freq, 20.0f); // 3508-3
LowPassFilter2p_Init(&c->filled[6], target_freq, 20.0f); // 3508-4
LowPassFilter2p_Init(&c->filled[7], target_freq, 20.0f); // 6020-1
LowPassFilter2p_Init(&c->filled[8], target_freq, 20.0f); // 6020-2
LowPassFilter2p_Init(&c->filled[9], target_freq, 20.0f); // 6020-3
LowPassFilter2p_Init(&c->filled[10], target_freq, 20.0f); // 6020-4
// c->set_point.yaw = 0.0f;
return CHASSIS_OK;
}
@ -80,7 +91,6 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_CMD_t *c_cmd)
if (c == NULL)
return CHASSIS_ERR_NULL; /* 主结构体不能为空 */
for (int i = 0; i < 4; i++)
{
PID_Reset(&c->pid.chassis_3508VPID[i]);
@ -91,13 +101,13 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_CMD_t *c_cmd)
c->mode = c_cmd->mode;
return CHASSIS_OK;
}
float angle_error[4]; // 角度误差
// 底盘解算
void Chassis_speed_calculate(Chassis_t *c, Chassis_CMD_t *c_cmd)
{
// RC模式下松开遥控器防止6020回到默认位置导致侧翻
if (c->mode == RC && fabs(c->move_vec.Vx) < 100 && fabs(c->move_vec.Vy) < 100 && fabs(c->move_vec.Vw) < 100)
if (c->mode == RC && fabs(c->move_vec.Vx) < 0.1 && fabs(c->move_vec.Vy) < 0.1 && fabs(c->move_vec.Vw) < 0.1)
{
// 如果之前不处于保持模式，则记录当前角度
if (!c->keeping_angle_flag)
@ -110,6 +120,10 @@ void Chassis_speed_calculate(Chassis_t *c, Chassis_CMD_t *c_cmd)
c->hopemotorout.rotor6020_jiesuan_1[i] = c->keep_angle[i];
c->hopemotorout.rotor3508_jiesuan_1[i] = 0;
}
// c->hopemotorout.rotor6020_jiesuan_1[0] = 315;
// c->hopemotorout.rotor6020_jiesuan_1[1] = 45;
// c->hopemotorout.rotor6020_jiesuan_1[2] = 315;
// c->hopemotorout.rotor6020_jiesuan_1[3] = 45;
}
else
{
@ -123,66 +137,65 @@ void Chassis_speed_calculate(Chassis_t *c, Chassis_CMD_t *c_cmd)
switch (c->mode)
{
case RC:
// const double radians = atan(1.0f * 330 / 330);
c->hopemotorout.rotor3508_jiesuan_1[0] = -sqrt((c->move_vec.Vx + c->move_vec.Vw * sin(radians)) * (c->move_vec.Vx + c->move_vec.Vw * sin(radians))
+ (c->move_vec.Vy + c->move_vec.Vw * cos(radians)) * (c->move_vec.Vy + c->move_vec.Vw * cos(radians)));
c->hopemotorout.rotor3508_jiesuan_1[1] = sqrt((c->move_vec.Vx - c->move_vec.Vw * sin(radians)) * (c->move_vec.Vx - c->move_vec.Vw * sin(radians))
+ (c->move_vec.Vy + c->move_vec.Vw * cos(radians)) * (c->move_vec.Vy + c->move_vec.Vw * cos(radians)));
c->hopemotorout.rotor3508_jiesuan_1[2] = -sqrt((c->move_vec.Vx + c->move_vec.Vw * sin(radians)) * (c->move_vec.Vx + c->move_vec.Vw * sin(radians))
+ (c->move_vec.Vy - c->move_vec.Vw * cos(radians)) * (c->move_vec.Vy - c->move_vec.Vw * cos(radians)));
c->hopemotorout.rotor3508_jiesuan_1[3] = sqrt((c->move_vec.Vx - c->move_vec.Vw * sin(radians)) * (c->move_vec.Vx - c->move_vec.Vw * sin(radians))
+ (c->move_vec.Vy - c->move_vec.Vw * cos(radians)) * (c->move_vec.Vy - c->move_vec.Vw * cos(radians)));
// const double radians = atan(1.0f * 330 / 330);
c->hopemotorout.rotor6020_jiesuan_1[0] = atan2((c->move_vec.Vy + c->move_vec.Vw * cos(radians)),
(c->move_vec.Vx + c->move_vec.Vw * sin(radians))) *
(180.0f / M_PI);
c->hopemotorout.rotor6020_jiesuan_1[1] = atan2((c->move_vec.Vy + c->move_vec.Vw * cos(radians)),
(c->move_vec.Vx - c->move_vec.Vw * sin(radians))) *
(180.0f / M_PI);
c->hopemotorout.rotor6020_jiesuan_1[2] = atan2((c->move_vec.Vy - c->move_vec.Vw * cos(radians)),
(c->move_vec.Vx + c->move_vec.Vw * sin(radians))) *
(180.0f / M_PI);
c->hopemotorout.rotor6020_jiesuan_1[3] = atan2((c->move_vec.Vy - c->move_vec.Vw * cos(radians)),
(c->move_vec.Vx - c->move_vec.Vw * sin(radians))) *
(180.0f / M_PI);
break;
c->hopemotorout.rotor3508_jiesuan_1[0] = sqrt(
(c->move_vec.Vx + c->move_vec.Vw * sin(radians)) * (c->move_vec.Vx + c->move_vec.Vw * sin(radians)) + (c->move_vec.Vy + c->move_vec.Vw * cos(radians)) * (c->move_vec.Vy + c->move_vec.Vw * cos(radians)));
c->hopemotorout.rotor3508_jiesuan_1[1] = sqrt(
(c->move_vec.Vx + c->move_vec.Vw * sin(radians)) * (c->move_vec.Vx + c->move_vec.Vw * sin(radians)) + (c->move_vec.Vy - c->move_vec.Vw * cos(radians)) * (c->move_vec.Vy - c->move_vec.Vw * cos(radians)));
c->hopemotorout.rotor3508_jiesuan_1[2] = sqrt(
(c->move_vec.Vx - c->move_vec.Vw * sin(radians)) * (c->move_vec.Vx - c->move_vec.Vw * sin(radians)) + (c->move_vec.Vy - c->move_vec.Vw * cos(radians)) * (c->move_vec.Vy - c->move_vec.Vw * cos(radians)));
c->hopemotorout.rotor3508_jiesuan_1[3] = sqrt(
(c->move_vec.Vx - c->move_vec.Vw * sin(radians)) * (c->move_vec.Vx - c->move_vec.Vw * sin(radians)) + (c->move_vec.Vy + c->move_vec.Vw * cos(radians)) * (c->move_vec.Vy + c->move_vec.Vw * cos(radians)));
c->hopemotorout.rotor6020_jiesuan_1[0] = atan2((c->move_vec.Vy + c->move_vec.Vw * cos(radians)),
(c->move_vec.Vx + c->move_vec.Vw * sin(radians))) *
(180.0f / M_PI);
c->hopemotorout.rotor6020_jiesuan_1[1] = atan2((c->move_vec.Vy - c->move_vec.Vw * cos(radians)),
(c->move_vec.Vx + c->move_vec.Vw * sin(radians))) *
(180.0f / M_PI);
c->hopemotorout.rotor6020_jiesuan_1[2] = atan2((c->move_vec.Vy - c->move_vec.Vw * cos(radians)),
(c->move_vec.Vx - c->move_vec.Vw * sin(radians))) *
(180.0f / M_PI);
c->hopemotorout.rotor6020_jiesuan_1[3] = atan2((c->move_vec.Vy + c->move_vec.Vw * cos(radians)),
(c->move_vec.Vx - c->move_vec.Vw * sin(radians))) *
(180.0f / M_PI);
break;
case STOP:
for (int i = 0; i < 4; i++)
{
c->hopemotorout.rotor3508_jiesuan_1[i] = 0.0f;
}
// c->hopemotorout.rotor6020_jiesuan_1[0] = 6.26554489/M_PI*180.0f;
// c->hopemotorout.rotor6020_jiesuan_1[1] = 2.1099906/M_PI*180.0f;
// c->hopemotorout.rotor6020_jiesuan_1[2] = 2.08391285/M_PI*180.0f;
// c->hopemotorout.rotor6020_jiesuan_1[3] = 5.26845694/M_PI*180.0f;
c->hopemotorout.rotor6020_jiesuan_1[0] = -45/M_PI*180.0f;
c->hopemotorout.rotor6020_jiesuan_1[1] = 45/M_PI*180.0f;
c->hopemotorout.rotor6020_jiesuan_1[2] = -45/M_PI*180.0f;
c->hopemotorout.rotor6020_jiesuan_1[3] = 45/M_PI*180.0f;
// c->hopemotorout.rotor6020_jiesuan_1[0] = 6.26554489/M_PI*180.0f;
// c->hopemotorout.rotor6020_jiesuan_1[1] = 2.1099906/M_PI*180.0f;
// c->hopemotorout.rotor6020_jiesuan_1[2] = 2.08391285/M_PI*180.0f;
// c->hopemotorout.rotor6020_jiesuan_1[3] = 5.26845694/M_PI*180.0f;
c->hopemotorout.rotor6020_jiesuan_1[0] = 315;
c->hopemotorout.rotor6020_jiesuan_1[1] = 45;
c->hopemotorout.rotor6020_jiesuan_1[2] = 315;
c->hopemotorout.rotor6020_jiesuan_1[3] = 45;
break;
case LOCK:
for (int i = 0; i < 4; i++)
case LOCK:
for (int i = 0; i < 4; i++)
{
c->hopemotorout.rotor3508_jiesuan_1[i] = 0.0f;
}
c->hopemotorout.rotor6020_jiesuan_1[0] = 315;
c->hopemotorout.rotor6020_jiesuan_1[1] = 45;
c->hopemotorout.rotor6020_jiesuan_1[2] = 45;
c->hopemotorout.rotor6020_jiesuan_1[3] = 315;
c->hopemotorout.rotor6020_jiesuan_1[0] = 0;
c->hopemotorout.rotor6020_jiesuan_1[1] = 0;
c->hopemotorout.rotor6020_jiesuan_1[2] = 0;
c->hopemotorout.rotor6020_jiesuan_1[3] = 0;
break;
}
}
}
// 角度归化到0°——360°
for (uint8_t i = 0; i < 4; i++)
@ -192,8 +205,7 @@ void Chassis_speed_calculate(Chassis_t *c, Chassis_CMD_t *c_cmd)
c->hopemotorout.rotor6020_jiesuan_1[i] += 360;
}
}
float angle_error[4]; // 角度误差
for (uint8_t i = 0; i < 4; i++)
{
angle_error[i] = c->hopemotorout.rotor6020_jiesuan_1[i] - c->motorfeedback.rotor_angle6020[i];
@ -206,17 +218,17 @@ void Chassis_speed_calculate(Chassis_t *c, Chassis_CMD_t *c_cmd)
c->hopemotorout.rotor6020_jiesuan_1[i]会导致6020出现故障*/
if (angle_error[i] > 90 && angle_error[i] <= 180)
{
c->hopemotorout.rotor3508_jiesuan_2[i] = -c->hopemotorout.rotor3508_jiesuan_1[i];
c->hopemotorout.rotor3508_jiesuan_2[i] = c->hopemotorout.rotor3508_jiesuan_1[i];
c->hopemotorout.rotor6020_jiesuan_2[i] = c->motorfeedback.rotor_angle6020[i] + angle_error[i] - 180;
}
else if (angle_error[i] < -90 && angle_error[i] >= -180)
{
c->hopemotorout.rotor3508_jiesuan_2[i] = -c->hopemotorout.rotor3508_jiesuan_1[i];
c->hopemotorout.rotor3508_jiesuan_2[i] = c->hopemotorout.rotor3508_jiesuan_1[i];
c->hopemotorout.rotor6020_jiesuan_2[i] = c->motorfeedback.rotor_angle6020[i] + angle_error[i] + 180;
}
else
{
c->hopemotorout.rotor3508_jiesuan_2[i] = c->hopemotorout.rotor3508_jiesuan_1[i];
c->hopemotorout.rotor3508_jiesuan_2[i] = -c->hopemotorout.rotor3508_jiesuan_1[i];
c->hopemotorout.rotor6020_jiesuan_2[i] = c->motorfeedback.rotor_angle6020[i] + angle_error[i];
}
}
@ -224,23 +236,28 @@ void Chassis_speed_calculate(Chassis_t *c, Chassis_CMD_t *c_cmd)
int8_t Chassis_update(Chassis_t *c)
{
if (c == NULL) {
return CHASSIS_ERR_NULL; // 参数错误
}
/*更新所有电机数据*/
MOTOR_RM_UpdateAll();
/*更新电机反馈*/
for(int i=0;i<4;i++){
c->motorfeedback.rotor_angle6020[i] = ( MOTOR_GetRotorAbsAngle(&(MOTOR_RM_GetMotor(&c->param->motor_6020_param[i])->motor))/M_PI*180.0f);
c->motorfeedback.rotor_rpm6020[i] = MOTOR_GetRotorSpeed(&(MOTOR_RM_GetMotor(&c->param->motor_6020_param[i])->motor));
c->motorfeedback.rotor_rpm3508[i] = MOTOR_GetRotorSpeed(&(MOTOR_RM_GetMotor(&c->param->motor_6020_param[i])->motor));
// c->motorfeedback.rotor_angle6020[i] = MOTOR_GetRotorAbsAngle(&(MOTOR_RM_GetMotor(&c->param->motor_6020_param[i])->motor));
c->motorfeedback.rotor_angle6020[i] = fmod(c->motorfeedback.rotor_angle6020[i] - c->motoroffset.MOTOR_OFFSET[i], 360.0);
if (c == NULL)
{
return CHASSIS_ERR_NULL; // 参数错误
}
/*更新所有电机数据*/
MOTOR_RM_UpdateAll();
}
/*更新电机反馈*/
for (int i = 0; i < 4; i++)
{
// c->motorfeedback.rotor_angle6020[i] = (MOTOR_GetRotorAbsAngle(&(MOTOR_RM_GetMotor(&c->param->motor_6020_param[i])->motor)) );
c->motorfeedback.rotor_angle6020[i] = (MOTOR_GetRotorAbsAngle(&(MOTOR_RM_GetMotor(&c->param->motor_6020_param[i])->motor)) / M_PI * 180.0f);
c->motorfeedback.rotor_rpm6020[i] = (MOTOR_GetRotorSpeed(&(MOTOR_RM_GetMotor(&c->param->motor_6020_param[i])->motor)) / 320);
c->motorfeedback.rotor_rpm3508[i] = MOTOR_GetRotorSpeed(&(MOTOR_RM_GetMotor(&c->param->motor_3508_param[i])->motor)) /10000;
// c->motorfeedback.rotor_angle6020[i] = MOTOR_GetRotorAbsAngle(&(MOTOR_RM_GetMotor(&c->param->motor_6020_param[i])->motor));
c->motorfeedback.rotor_angle6020[i] = fmod(c->motorfeedback.rotor_angle6020[i] - c->motoroffset.MOTOR_OFFSET[i], 360.0);
if (c->motorfeedback.rotor_angle6020[i] < 0)
{
c->motorfeedback.rotor_angle6020[i] += 360;
}
}
return CHASSIS_OK;
}
@ -250,10 +267,10 @@ int8_t Chassis_update(Chassis_t *c)
void ChassisrolPrevent(Chassis_t *c)
{
}
int8_t Chassis_Control(Chassis_t *c, Chassis_CMD_t *c_cmd)
float chassis6020_detangle[4]; // 6020解算出的角度
int8_t Chassis_Control(Chassis_t *c, Chassis_CMD_t *c_cmd)
{
float chassis6020_detangle[4]; // 6020解算出的角度
if (c == NULL || c_cmd == NULL)
{
return CHASSIS_ERR_NULL; // 参数错误
@ -268,6 +285,7 @@ int8_t Chassis_Control(Chassis_t *c, Chassis_CMD_t *c_cmd)
return CHASSIS_ERR_MODE; /* 设置模式失败 */
}
/*根据底盘模式进行不同的控制*/
switch (c->mode)
{
@ -282,9 +300,11 @@ int8_t Chassis_Control(Chassis_t *c, Chassis_CMD_t *c_cmd)
// 遥控模式
// c->move_vec.Vx = c_cmd->Vx * c_cmd->throttle * 1000.0f;
// c->move_vec.Vy = c_cmd->Vy * c_cmd->throttle * 1000.0f;
c->move_vec.Vx = c_cmd->Vx * 1000.0f;
c->move_vec.Vy = c_cmd->Vy * 1000.0f;
c->move_vec.Vw = c_cmd->Vw * 1000.0f;
c->move_vec.Vx = c_cmd->Vx * c_cmd->throttle;
c->move_vec.Vy = c_cmd->Vy * c_cmd->throttle;
c->move_vec.Vw = c_cmd->Vw * c_cmd->throttle;
break;
default:
@ -303,30 +323,31 @@ int8_t Chassis_Control(Chassis_t *c, Chassis_CMD_t *c_cmd)
c->hopemotorout.motor6020_target[i] = c->hopemotorout.rotor6020_jiesuan_2[i];
chassis6020_detangle[i] = PID_Calc(&(c->pid.chassis_6020anglePid[i]), c->hopemotorout.motor6020_target[i],
c->motorfeedback.rotor_angle6020[i], 0.0f, c->dt);
c->final_out.final_6020out[i] = chassis6020_detangle[i] ; //单环控制就用这个
// c->final_out.final_6020out[i] = PID_Calc(&(c->pid.chassis_6020OmegaPid[i]), chassis6020_detangle[i],
// c->motorfeedback.rotor_rpm6020[i], 0.0f, c->dt);
c->out.rotor6020_out[i] = c->final_out.final_6020out[i];
// c->final_out.final_6020out[i] = chassis6020_detangle[i] ; //单环控制就用这个
c->final_out.final_6020out[i] = PID_Calc(&(c->pid.chassis_6020OmegaPid[i]), chassis6020_detangle[i],
c->motorfeedback.rotor_rpm6020[i], 0.0f, c->dt);
c->out.rotor6020_out[i] = LowPassFilter2p_Apply(&c->filled[7+i], c->final_out.final_6020out[i]);
c->hopemotorout.motor3508_target[i] = c->hopemotorout.rotor3508_jiesuan_2[i];
c->final_out.final_3508out[i] = PID_Calc(&(c->pid.chassis_3508VPID[i]), c->hopemotorout.motor3508_target[i],
c->motorfeedback.rotor_rpm3508[i], 0.0f, c->dt);
c->out.rotor3508_out[i] = c->final_out.final_3508out[i];
c->out.rotor3508_out[i] = LowPassFilter2p_Apply(&c->filled[3+i], c->final_out.final_3508out[i]);
}
return CHASSIS_OK;
return CHASSIS_OK;
}
/*电机输出设定和发送*/
void Chassis_Setoutput(Chassis_t * c)
/*电机输出设定和发送*/
void Chassis_Setoutput(Chassis_t *c)
{
for (int i = 0; i < 4; i++)
{
for (int i = 0; i < 4; i++)
{
MOTOR_RM_SetOutput(&(c->param->motor_3508_param[i]), c->out.rotor3508_out[i]);
MOTOR_RM_SetOutput(&(c->param->motor_6020_param[i]), c->out.rotor6020_out[i]);
}
MOTOR_RM_Ctrl(&(c->param->motor_3508_param[0]));
MOTOR_RM_Ctrl(&(c->param->motor_6020_param[0]));
MOTOR_RM_SetOutput(&(c->param->motor_3508_param[i]), c->out.rotor3508_out[i]);
MOTOR_RM_SetOutput(&(c->param->motor_6020_param[i]), c->out.rotor6020_out[i]);
}
// MOTOR_RM_SetOutput(&(c->param->motor_3508_param[0]), 1.0f);
MOTOR_RM_Ctrl(&(c->param->motor_3508_param[0]));
MOTOR_RM_Ctrl(&(c->param->motor_6020_param[0]));
/*最后这里的数据处理发送和PID计算还未处理写，后面更改，现在注释部分是王志城的代码*/
@ -346,5 +367,4 @@ int8_t Chassis_Control(Chassis_t *c, Chassis_CMD_t *c_cmd)
// out->chassis5065.erpm[i] = c->final_out.final_5065out[i];
// }
// #endif
}
}

2
User/module/chassis.h
View File

@ -195,7 +195,7 @@ extern "C"
MotorOffset_t motoroffset; // 5065校准数据
Chassis_Param_t *param; // 一些固定的参数
fp32 vofa_send[8]; // vofa输出数据
LowPassFilter2p_t filled[3]; // 低通滤波器
LowPassFilter2p_t filled[11]; // 低通滤波器
float keep_angle[4]; // 保持的 6020 角度
Chassis_out_t out;

17
User/module/cmd,h Normal file
View File

@ -0,0 +1,17 @@
/*
该文件cmd用于对各种命令的中转处理，以防止在remote遥控器任务中过于冗杂堆叠
*/
#ifndef _CMD_H
#define _CMD_H
#include "struct_typedef.h"
#include "device.h"
#endif

13
User/module/cmd.c Normal file
View File

@ -0,0 +1,13 @@
/*
该文件cmd用于对各种命令的中转处理，以防止在remote遥控器任务中过于冗杂堆叠
*/
/* Includes ----------------------------------------------------------------- */
#include "cmd.h"
#include "string.h"
/* Private function -------------------------------------------------------- */
/*Export function --------------------------------------------------------------*/

32
User/module/config.c
View File

@ -27,34 +27,34 @@ static const Config_Param_t param_chassis ={
.motor_6020_param[3]={BSP_CAN_1,0x208,MOTOR_GM6020,false,false},
.C6020Omega_param={
.k=0.2f,
.p=0.1f,
.i=0.0f,
.k=1.0f,
.p=0.5f,
.i=0.1f,
.d=0.0f,
.i_limit=0.0f,
.out_limit=100.0f,
.i_limit=1.0f,
.out_limit=1.0f,
.d_cutoff_freq= -1.0f,
.range=-1.0f
},
.C6020Angle_param={
.k=0.25f,
.p=0.15f,
.i=0.0f,
.k=0.2f,
.p=0.21f,
.i=0.1f,
.d=0.0f,
.i_limit=0.0f,
.out_limit=10000.0f,
.i_limit=1.0f,
.out_limit=30.0f,
.d_cutoff_freq= -1.0f,
.range=M_2PI
.range=360
},
.M3508v_param={
.k=1.0f,
.p=1.0f,
.i=0.0f,
.k=0.2f,
.p=0.25f,
.i=0.1f,
.d=0.0f,
.i_limit=0.0f,
.out_limit=10000.0f,
.i_limit=1.0f,
.out_limit=1.0f,
.d_cutoff_freq= -1.0f,
.range=-1.0f
},

13
User/task/Task2.c
View File

@ -86,6 +86,19 @@ cmd_chassis.cmd_power_on_safe=4;
cmd_chassis.mode = STOP ;
break;
}
switch (ld.sw[1])
{
case 352:
cmd_chassis.throttle =1.0f;
break;
case 1024:
cmd_chassis.throttle = 1.5f;
break;
case 1695:
cmd_chassis.throttle = 5.0f;
break;
}
cmd_chassis.Vx = ld.ch[3] * 1.0f;
cmd_chassis.Vy = ld.ch[2] * 1.0f;