From fa2dd118f679ddeddb80c4c5c228c7a6e2d2ae9d Mon Sep 17 00:00:00 2001 From: Link Wu <1774241372@qq.com> Date: Thu, 13 May 2021 21:15:56 +0800 Subject: [PATCH 01/10] new branch for gold miner --- mdk/gold-miner.uvoptx | 1084 +++++++++++++++++++++++++++++++++++ mdk/gold-miner.uvprojx | 696 ++++++++++++++++++++++ user/gold-miner/config.h | 28 + user/gold-miner/handle.h | 82 +++ user/gold-miner/interrupt.c | 176 ++++++ user/gold-miner/main.c | 102 ++++ user/gold-miner/protocol.h | 506 ++++++++++++++++ user/gold-miner/tasks.c | 200 +++++++ user/gold-miner/tasks.h | 13 + 9 files changed, 2887 insertions(+) create mode 100644 mdk/gold-miner.uvoptx create mode 100644 mdk/gold-miner.uvprojx create mode 100644 user/gold-miner/config.h create mode 100644 user/gold-miner/handle.h create mode 100644 user/gold-miner/interrupt.c create mode 100644 user/gold-miner/main.c create mode 100644 user/gold-miner/protocol.h create mode 100644 user/gold-miner/tasks.c create mode 100644 user/gold-miner/tasks.h diff --git a/mdk/gold-miner.uvoptx b/mdk/gold-miner.uvoptx new file mode 100644 index 00000000..dede32c3 --- /dev/null +++ b/mdk/gold-miner.uvoptx @@ -0,0 +1,1084 @@ + + + + 1.0 + +
### uVision Project, (C) Keil Software
+ + + *.c + *.s*; *.src; *.a* + *.obj + *.lib + *.txt; *.h; *.inc + *.plm + *.cpp + 0 + + + + 0 + 0 + + + + Target 1 + 0x4 + ARM-ADS + + 12000000 + + 1 + 1 + 0 + 1 + 0 + + + 1 + 65535 + 0 + 0 + 0 + + + 79 + 66 + 8 + .\ + + + 1 + 1 + 1 + 0 + 1 + 1 + 0 + 1 + 0 + 0 + 0 + 0 + + + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 0 + 0 + + + 1 + 0 + 1 + + 18 + + 0 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 0 + 1 + 1 + 1 + 1 + 0 + 0 + 1 + 0 + 0 + 6 + + + + + + + + + + + Segger\JL2CM3.dll + + + + 0 + ARMRTXEVENTFLAGS + -L70 -Z18 -C0 -M0 -T1 + + + 0 + DLGTARM + (1010=-1,-1,-1,-1,0)(1007=-1,-1,-1,-1,0)(1008=-1,-1,-1,-1,0)(1009=-1,-1,-1,-1,0)(1012=-1,-1,-1,-1,0) + + + 0 + ARMDBGFLAGS + + + + 0 + JL2CM3 + -U59400616 -O78 -S2 -ZTIFSpeedSel5000 -A0 -C0 -JU1 -JI127.0.0.1 -JP0 -RST0 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO18 -TC10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -TB1 -TFE0 -FO7 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_2048 -FS08000000 -FL0200000 -FP0($$Device:STM32F427II$CMSIS\Flash\STM32F4xx_2048.FLM) + + + 0 + UL2CM3 + UL2CM3(-S0 -C0 -P0 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_2048 -FS08000000 -FL0200000 -FP0($$Device:STM32F427II$CMSIS\Flash\STM32F4xx_2048.FLM)) + + + + + 0 + 0 + 107 + 1 +
134229784
+ 0 + 0 + 0 + 0 + 0 + 1 + C:\Codes\RV-CornerStone\user\template\interrupt.c + + + + + 1 + 0 + 186 + 1 +
134234324
+ 0 + 0 + 0 + 0 + 0 + 1 + ..\driver\src\Driver_Protocol.c + + + + + 2 + 0 + 184 + 1 +
134234314
+ 0 + 0 + 0 + 0 + 0 + 1 + ..\driver\src\Driver_Protocol.c + + + + + 3 + 0 + 174 + 1 +
134234276
+ 0 + 0 + 0 + 0 + 0 + 1 + ..\driver\src\Driver_Protocol.c + + + + + 4 + 0 + 166 + 1 +
134234228
+ 0 + 0 + 0 + 0 + 0 + 1 + ..\driver\src\Driver_Protocol.c + + + + + 5 + 0 + 162 + 1 +
134234200
+ 0 + 0 + 0 + 0 + 0 + 1 + ..\driver\src\Driver_Protocol.c + + + + + 6 + 0 + 37 + 1 +
134232692
+ 0 + 0 + 0 + 0 + 0 + 1 + ..\driver\src\Driver_Motor.c + + + + + 7 + 0 + 15 + 1 +
134232410
+ 0 + 0 + 0 + 0 + 0 + 1 + ..\driver\src\Driver_Motor.c + + + + + 8 + 0 + 118 + 1 +
134224354
+ 0 + 0 + 0 + 0 + 0 + 1 + ..\driver\src\Driver_Bridge.c + + \\Belief_exp2\../driver/src/Driver_Bridge.c\118 + + + 9 + 0 + 119 + 1 +
134224374
+ 0 + 0 + 0 + 0 + 0 + 1 + ..\driver\src\Driver_Bridge.c + + \\Belief_exp2\../driver/src/Driver_Bridge.c\119 + + + + + 0 + 1 + LastWakeTime + + + 1 + 1 + position + + + 2 + 1 + speed + + + 3 + 1 + temperature + + + 4 + 1 + actualCurrent + + + 5 + 1 + motor + + + 6 + 1 + protocolInfo + + + 7 + 1 + ProtocolInfoList + + + 8 + 1 + taskName + + + 9 + 1 + protocolInfo + + + + 0 + + + 0 + 0 + 1 + 0 + 0 + 0 + 0 + 1 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + + + + 0 + 0 + 0 + + + + + + + + + + 1 + 0 + 0 + 2 + 10000000 + + + + + + bsp + 0 + 0 + 0 + 0 + + + + core + 0 + 0 + 0 + 0 + + 2 + 1 + 2 + 0 + 0 + 0 + ..\core\startup_stm32f427xx.s + startup_stm32f427xx.s + 0 + 0 + + + + + library + 0 + 0 + 0 + 0 + + 3 + 2 + 1 + 0 + 0 + 0 + ..\library\std\src\misc.c + misc.c + 0 + 0 + + + 3 + 3 + 1 + 0 + 0 + 0 + ..\library\std\src\stm32f4xx_adc.c + stm32f4xx_adc.c + 0 + 0 + + + 3 + 4 + 1 + 0 + 0 + 0 + ..\library\std\src\stm32f4xx_can.c + stm32f4xx_can.c + 0 + 0 + + + 3 + 5 + 1 + 0 + 0 + 0 + ..\library\std\src\stm32f4xx_dma.c + stm32f4xx_dma.c + 0 + 0 + + + 3 + 6 + 1 + 0 + 0 + 0 + ..\library\std\src\stm32f4xx_dma2d.c + stm32f4xx_dma2d.c + 0 + 0 + + + 3 + 7 + 1 + 0 + 0 + 0 + ..\library\std\src\stm32f4xx_exti.c + stm32f4xx_exti.c + 0 + 0 + + + 3 + 8 + 1 + 0 + 0 + 0 + ..\library\std\src\stm32f4xx_flash.c + stm32f4xx_flash.c + 0 + 0 + + + 3 + 9 + 1 + 0 + 0 + 0 + ..\library\std\src\stm32f4xx_gpio.c + stm32f4xx_gpio.c + 0 + 0 + + + 3 + 10 + 1 + 0 + 0 + 0 + ..\library\std\src\stm32f4xx_rcc.c + stm32f4xx_rcc.c + 0 + 0 + + + 3 + 11 + 1 + 0 + 0 + 0 + ..\library\std\src\stm32f4xx_spi.c + stm32f4xx_spi.c + 0 + 0 + + + 3 + 12 + 1 + 0 + 0 + 0 + ..\library\std\src\stm32f4xx_syscfg.c + stm32f4xx_syscfg.c + 0 + 0 + + + 3 + 13 + 1 + 0 + 0 + 0 + ..\library\std\src\stm32f4xx_tim.c + stm32f4xx_tim.c + 0 + 0 + + + 3 + 14 + 1 + 0 + 0 + 0 + ..\library\std\src\stm32f4xx_usart.c + stm32f4xx_usart.c + 0 + 0 + + + 3 + 15 + 1 + 0 + 0 + 0 + ..\library\std\src\stm32f4xx_wwdg.c + stm32f4xx_wwdg.c + 0 + 0 + + + 3 + 16 + 1 + 0 + 0 + 0 + ..\library\freertos\croutine.c + croutine.c + 0 + 0 + + + 3 + 17 + 1 + 0 + 0 + 0 + ..\library\freertos\event_groups.c + event_groups.c + 0 + 0 + + + 3 + 18 + 1 + 0 + 0 + 0 + ..\library\freertos\list.c + list.c + 0 + 0 + + + 3 + 19 + 1 + 0 + 0 + 0 + ..\library\freertos\queue.c + queue.c + 0 + 0 + + + 3 + 20 + 1 + 0 + 0 + 0 + ..\library\freertos\tasks.c + tasks.c + 0 + 0 + + + 3 + 21 + 1 + 0 + 0 + 0 + ..\library\freertos\timers.c + timers.c + 0 + 0 + + + 3 + 22 + 1 + 0 + 0 + 0 + ..\library\freertos\portable\MemMang\heap_4.c + heap_4.c + 0 + 0 + + + 3 + 23 + 1 + 0 + 0 + 0 + ..\library\freertos\portable\RVDS\ARM_CM4F\port.c + port.c + 0 + 0 + + + 3 + 24 + 1 + 0 + 0 + 0 + ..\library\atom\delay\delay.c + delay.c + 0 + 0 + + + 3 + 25 + 1 + 0 + 0 + 0 + ..\library\atom\sys\sys.c + sys.c + 0 + 0 + + + 3 + 26 + 1 + 0 + 0 + 0 + ..\library\mpu6500\MadgwickAHRS.c + MadgwickAHRS.c + 0 + 0 + + + 3 + 27 + 1 + 0 + 0 + 0 + ..\library\mpu6500\mpu6500_driver.c + mpu6500_driver.c + 0 + 0 + + + 3 + 28 + 1 + 0 + 0 + 0 + ..\library\mpu6500\mpu6500_i2c.c + mpu6500_i2c.c + 0 + 0 + + + 3 + 29 + 1 + 0 + 0 + 0 + ..\library\atom\debug\beep.c + beep.c + 0 + 0 + + + 3 + 30 + 1 + 0 + 0 + 0 + ..\library\atom\debug\key.c + key.c + 0 + 0 + + + 3 + 31 + 1 + 0 + 0 + 0 + ..\library\atom\debug\led.c + led.c + 0 + 0 + + + 3 + 32 + 1 + 0 + 0 + 0 + ..\library\vegmath\vegmath.c + vegmath.c + 0 + 0 + + + 3 + 33 + 1 + 0 + 0 + 0 + ..\library\oled\oled.c + oled.c + 0 + 0 + + + 3 + 34 + 1 + 0 + 0 + 0 + ..\library\atom\usart\usart.c + usart.c + 0 + 0 + + + + + driver + 1 + 0 + 0 + 0 + + 4 + 35 + 1 + 0 + 0 + 0 + ..\driver\src\Driver_CAN.c + Driver_CAN.c + 0 + 0 + + + 4 + 36 + 1 + 0 + 0 + 0 + ..\driver\src\Driver_DBUS.c + Driver_DBUS.c + 0 + 0 + + + 4 + 37 + 1 + 0 + 0 + 0 + ..\driver\src\Driver_PID.c + Driver_PID.c + 0 + 0 + + + 4 + 38 + 1 + 0 + 0 + 0 + ..\driver\src\Driver_Magic.c + Driver_Magic.c + 0 + 0 + + + 4 + 39 + 1 + 0 + 0 + 0 + ..\driver\src\Driver_Chassis.c + Driver_Chassis.c + 0 + 0 + + + 4 + 40 + 1 + 0 + 0 + 0 + ..\driver\src\Driver_Motor.c + Driver_Motor.c + 0 + 0 + + + 4 + 41 + 1 + 0 + 0 + 0 + ..\driver\src\Driver_Filter.c + Driver_Filter.c + 0 + 0 + + + 4 + 42 + 1 + 0 + 0 + 0 + ..\driver\src\Driver_Gyroscope.c + Driver_Gyroscope.c + 0 + 0 + + + 4 + 43 + 1 + 0 + 0 + 0 + ..\driver\src\Driver_BSP.c + Driver_BSP.c + 0 + 0 + + + 4 + 44 + 1 + 0 + 0 + 0 + ..\driver\src\Driver_Protocol.c + Driver_Protocol.c + 0 + 0 + + + 4 + 45 + 1 + 0 + 0 + 0 + ..\driver\src\Driver_Fsm.c + Driver_Fsm.c + 0 + 0 + + + 4 + 46 + 1 + 0 + 0 + 0 + ..\driver\src\Driver_Bridge.c + Driver_Bridge.c + 0 + 0 + + + + + user_common + 0 + 0 + 0 + 0 + + 5 + 47 + 5 + 0 + 0 + 0 + ..\user\common\macro.h + macro.h + 0 + 0 + + + 5 + 48 + 1 + 0 + 0 + 0 + ..\user\common\system_stm32f4xx.c + system_stm32f4xx.c + 0 + 0 + + + 5 + 49 + 5 + 0 + 0 + 0 + ..\user\common\interrupt.h + interrupt.h + 0 + 0 + + + + + user_gold_miner + 1 + 0 + 0 + 0 + + 6 + 50 + 1 + 0 + 0 + 0 + ..\user\gold-miner\interrupt.c + interrupt.c + 0 + 0 + + + 6 + 51 + 1 + 0 + 0 + 0 + ..\user\gold-miner\main.c + main.c + 0 + 0 + + + 6 + 52 + 1 + 0 + 0 + 0 + ..\user\gold-miner\tasks.c + tasks.c + 0 + 0 + + + + diff --git a/mdk/gold-miner.uvprojx b/mdk/gold-miner.uvprojx new file mode 100644 index 00000000..928238dd --- /dev/null +++ b/mdk/gold-miner.uvprojx @@ -0,0 +1,696 @@ + + + + 2.1 + +
### uVision Project, (C) Keil Software
+ + + + Target 1 + 0x4 + ARM-ADS + 5060750::V5.06 update 6 (build 750)::ARMCC + 0 + + + STM32F427II + STMicroelectronics + Keil.STM32F4xx_DFP.2.14.0 + http://www.keil.com/pack/ + IRAM(0x20000000,0x30000) IRAM2(0x10000000,0x10000) IROM(0x08000000,0x200000) CPUTYPE("Cortex-M4") FPU2 CLOCK(12000000) ELITTLE + + + UL2CM3(-S0 -C0 -P0 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_2048 -FS08000000 -FL0200000 -FP0($$Device:STM32F427II$Flash\STM32F4xx_2048.FLM)) + 0 + $$Device:STM32F427II$Device\Include\stm32f4xx.h + + + + + + + + + + $$Device:STM32F427II$SVD\STM32F427x.svd + 0 + 0 + + + + + + + 0 + 0 + 0 + 0 + 1 + + .\Objects\ + Belief exp2 + 1 + 0 + 1 + 1 + 1 + .\ + 1 + 0 + 0 + + 0 + 0 + + + 0 + 0 + 0 + 0 + + + 0 + 0 + + + 0 + 0 + 0 + 0 + + + 0 + 0 + + + 0 + 0 + 0 + 0 + + 0 + + + + 0 + 0 + 0 + 0 + 0 + 1 + 0 + 0 + 0 + 0 + 3 + + + 1 + + + SARMCM3.DLL + -REMAP -MPU + DCM.DLL + -pCM4 + SARMCM3.DLL + -MPU + TCM.DLL + -pCM4 + + + + 1 + 0 + 0 + 0 + 16 + + + + + 1 + 0 + 0 + 1 + 1 + 4096 + + 1 + BIN\UL2CM3.DLL + "" () + + + + + 0 + + + + 0 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 0 + 1 + 1 + 0 + 1 + 1 + 0 + 0 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 0 + 0 + "Cortex-M4" + + 0 + 0 + 0 + 1 + 1 + 0 + 0 + 2 + 0 + 0 + 1 + 0 + 8 + 0 + 0 + 0 + 0 + 3 + 3 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 1 + 0 + 0 + 0 + 0 + 1 + 0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x20000000 + 0x30000 + + + 1 + 0x8000000 + 0x200000 + + + 0 + 0x0 + 0x0 + + + 1 + 0x0 + 0x0 + + + 1 + 0x0 + 0x0 + + + 1 + 0x0 + 0x0 + + + 1 + 0x8000000 + 0x200000 + + + 1 + 0x0 + 0x0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x20000000 + 0x30000 + + + 0 + 0x10000000 + 0x10000 + + + + + + 1 + 1 + 0 + 0 + 1 + 0 + 0 + 0 + 0 + 0 + 2 + 0 + 0 + 0 + 0 + 0 + 1 + 1 + 1 + 1 + 0 + 0 + 0 + + --gnu + STM32F427_437xx,USE_STDPERIPH_DRIVER + + ..\core;..\bsp\inc;..\driver\inc;..\library\std\inc;..\user;..\library\freertos\include;..\library\freertos\portable\RVDS\ARM_CM4F;..\user\common;..\library\atom\delay;..\library\atom\sys;..\library\atom\usart;..\library\atom\debug;..\library\mpu6500;..\library\vegmath;..\user\template;..\library\oled + + + + 1 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 1 + + + + + + + + + 1 + 0 + 0 + 0 + 1 + 0 + 0x08000000 + 0x20000000 + + + + + + + + + + + + + bsp + + + core + + + startup_stm32f427xx.s + 2 + ..\core\startup_stm32f427xx.s + + + + + library + + + misc.c + 1 + ..\library\std\src\misc.c + + + stm32f4xx_adc.c + 1 + ..\library\std\src\stm32f4xx_adc.c + + + stm32f4xx_can.c + 1 + ..\library\std\src\stm32f4xx_can.c + + + stm32f4xx_dma.c + 1 + ..\library\std\src\stm32f4xx_dma.c + + + stm32f4xx_dma2d.c + 1 + ..\library\std\src\stm32f4xx_dma2d.c + + + stm32f4xx_exti.c + 1 + ..\library\std\src\stm32f4xx_exti.c + + + stm32f4xx_flash.c + 1 + ..\library\std\src\stm32f4xx_flash.c + + + stm32f4xx_gpio.c + 1 + ..\library\std\src\stm32f4xx_gpio.c + + + stm32f4xx_rcc.c + 1 + ..\library\std\src\stm32f4xx_rcc.c + + + stm32f4xx_spi.c + 1 + ..\library\std\src\stm32f4xx_spi.c + + + stm32f4xx_syscfg.c + 1 + ..\library\std\src\stm32f4xx_syscfg.c + + + stm32f4xx_tim.c + 1 + ..\library\std\src\stm32f4xx_tim.c + + + stm32f4xx_usart.c + 1 + ..\library\std\src\stm32f4xx_usart.c + + + stm32f4xx_wwdg.c + 1 + ..\library\std\src\stm32f4xx_wwdg.c + + + croutine.c + 1 + ..\library\freertos\croutine.c + + + event_groups.c + 1 + ..\library\freertos\event_groups.c + + + list.c + 1 + ..\library\freertos\list.c + + + queue.c + 1 + ..\library\freertos\queue.c + + + tasks.c + 1 + ..\library\freertos\tasks.c + + + timers.c + 1 + ..\library\freertos\timers.c + + + heap_4.c + 1 + ..\library\freertos\portable\MemMang\heap_4.c + + + port.c + 1 + ..\library\freertos\portable\RVDS\ARM_CM4F\port.c + + + delay.c + 1 + ..\library\atom\delay\delay.c + + + sys.c + 1 + ..\library\atom\sys\sys.c + + + MadgwickAHRS.c + 1 + ..\library\mpu6500\MadgwickAHRS.c + + + mpu6500_driver.c + 1 + ..\library\mpu6500\mpu6500_driver.c + + + mpu6500_i2c.c + 1 + ..\library\mpu6500\mpu6500_i2c.c + + + beep.c + 1 + ..\library\atom\debug\beep.c + + + key.c + 1 + ..\library\atom\debug\key.c + + + led.c + 1 + ..\library\atom\debug\led.c + + + vegmath.c + 1 + ..\library\vegmath\vegmath.c + + + oled.c + 1 + ..\library\oled\oled.c + + + usart.c + 1 + ..\library\atom\usart\usart.c + + + + + driver + + + Driver_CAN.c + 1 + ..\driver\src\Driver_CAN.c + + + Driver_DBUS.c + 1 + ..\driver\src\Driver_DBUS.c + + + Driver_PID.c + 1 + ..\driver\src\Driver_PID.c + + + Driver_Magic.c + 1 + ..\driver\src\Driver_Magic.c + + + Driver_Chassis.c + 1 + ..\driver\src\Driver_Chassis.c + + + Driver_Motor.c + 1 + ..\driver\src\Driver_Motor.c + + + Driver_Filter.c + 1 + ..\driver\src\Driver_Filter.c + + + Driver_Gyroscope.c + 1 + ..\driver\src\Driver_Gyroscope.c + + + Driver_BSP.c + 1 + ..\driver\src\Driver_BSP.c + + + Driver_Protocol.c + 1 + ..\driver\src\Driver_Protocol.c + + + Driver_Fsm.c + 1 + ..\driver\src\Driver_Fsm.c + + + Driver_Bridge.c + 1 + ..\driver\src\Driver_Bridge.c + + + + + user_common + + + macro.h + 5 + ..\user\common\macro.h + + + system_stm32f4xx.c + 1 + ..\user\common\system_stm32f4xx.c + + + interrupt.h + 5 + ..\user\common\interrupt.h + + + + + user_gold_miner + + + interrupt.c + 1 + ..\user\gold-miner\interrupt.c + + + main.c + 1 + ..\user\gold-miner\main.c + + + tasks.c + 1 + ..\user\gold-miner\tasks.c + + + + + + + + + + + + + + + + + <Project Info> + + + + + + 0 + 1 + + + + + diff --git a/user/gold-miner/config.h b/user/gold-miner/config.h new file mode 100644 index 00000000..7732c8c9 --- /dev/null +++ b/user/gold-miner/config.h @@ -0,0 +1,28 @@ +/** + * @brief 机器人参数 + * @note 默认参数及参数列表见 default_config.h + */ + +#include "default_config.h" + +// 总线设置 +#define CAN_DEVICE_ID 0x501 // 发送数据时CAN标准帧中的ID 0x501-0x503 + +// 调试 +#define DEBUG_ENABLED 0 // 调试开关 +#define SERIAL_DEBUG_PORT USART6 // 串口调试端口 + +// 陀螺仪 +#define BOARD_FRONT_IS_UP 1 // 板子正面朝上 +#define BOARD_SHORT_SIDE_IS_PARALLEL_TO_PITCH 1 // 板子短边朝下 +#define GYROSCOPE_YAW_FILTER_THRESHOLD 0.003f // 零飘修正阈值 +#define GYROSCOPE_LSB 16.384f // 陀螺仪敏感度 2^16/4000 +#define ACCELERATE_LSB 4096.0f // 加速度计敏感度 2^16/16 + +// 底盘 +#define CHASSIS_MOTOR_REDUCTION_RATE 19.2f // 底盘电机减速比 +#define CHASSIS_SIZE_K 0.385f // 测量值, 机器人中心点到XY边缘的距离之和 +#define CHASSIS_INVERSE_WHEEL_RADIUS 13.16f // 测量值, 麦克纳姆轮半径的倒数 + +// DMA +#define DMA_BUFFER_LENGTH 128 // DMA发送接收长度 \ No newline at end of file diff --git a/user/gold-miner/handle.h b/user/gold-miner/handle.h new file mode 100644 index 00000000..ad091b3b --- /dev/null +++ b/user/gold-miner/handle.h @@ -0,0 +1,82 @@ +#ifndef __HANDLE_H +#define __HANDLE_H + +#include "sys.h" +#include "delay.h" +#include "led.h" +#include "beep.h" +#include "key.h" +#include "FreeRTOS.h" +#include "task.h" +#include "queue.h" +#include "vegmath.h" +#include "config.h" +#include "oled.h" +#include "Driver_BSP.h" +#include "Driver_Filter.h" +#include "Driver_PID.h" +#include "Driver_DBUS.h" +#include "Driver_CAN.h" +#include "Driver_Motor.h" +#include "Driver_Chassis.h" +#include "mpu6500_driver.h" +#include "Driver_Gyroscope.h" +#include "Driver_Protocol.h" +#include "Driver_Bridge.h" +#include "Driver_Magic.h" +#include "Driver_Fsm.h" + +#ifdef __HANDLE_GLOBALS +#define __HANDLE_EXT +#else +#define __HANDLE_EXT extern +#endif + +// Stone ID +__HANDLE_EXT uint8_t Board_Id, Robot_Id; + +// 功能开关 +__HANDLE_EXT uint8_t SafetyMode; + +// TIM +__HANDLE_EXT volatile uint32_t ulHighFrequencyTimerTicks; + +// 电机 +__HANDLE_EXT Motor_Type Motor_Arm; +__HANDLE_EXT PID_Type PID_Arm_Speed, PID_Arm_Angle; + +// 遥控器 +__HANDLE_EXT uint8_t remoteBuffer[DBUS_LENGTH + DBUS_BACK_LENGTH]; +__HANDLE_EXT Remote_Type remoteData; +__HANDLE_EXT Keyboard_Type keyboardData; +__HANDLE_EXT Mouse_Type mouseData; + +// 陀螺仪 +__HANDLE_EXT volatile ImuData_Type ImuData; +__HANDLE_EXT volatile GyroscopeData_Type Gyroscope_EulerData; + +// 调试数据 +__HANDLE_EXT DebugData_Type DebugData; + +// 底盘 +__HANDLE_EXT ChassisData_Type ChassisData; +__HANDLE_EXT Motor_Type Motor_LF, Motor_RF, Motor_RB, Motor_LB; +__HANDLE_EXT PID_Type PID_LFCM, PID_LBCM, PID_RBCM, PID_RFCM, PID_YawAngle, PID_YawSpeed; + +// 通讯协议 +__HANDLE_EXT ProtocolData_Type ProtocolData; +__HANDLE_EXT Node_Type Node_Judge, Node_Host, Node_Board; + +// PWM +__HANDLE_EXT PWM_Type PWM_Test; + +// 总线 +__HANDLE_EXT Bridge_Type BridgeData; + +/** + * @brief 初始化结构体 + * @note 该函数将在所有硬件及任务初始化之前执行 + */ +void Handle_Init(void); + +#endif diff --git a/user/gold-miner/interrupt.c b/user/gold-miner/interrupt.c new file mode 100644 index 00000000..186b4846 --- /dev/null +++ b/user/gold-miner/interrupt.c @@ -0,0 +1,176 @@ +/** + * @brief 中断服务函数根据地 + */ +#include "handle.h" + +// EXTI9_5 陀螺仪中断 +void EXTI9_5_IRQHandler(void) { + uint8_t suc; + if (EXTI_GetITStatus(EXTI_Line8) != RESET) { + EXTI_ClearFlag(EXTI_Line8); + EXTI_ClearITPendingBit(EXTI_Line8); + Gyroscope_Update(&Gyroscope_EulerData); + } +} + +// DBus空闲中断(USART1) +void USART1_IRQHandler(void) { + uint8_t UARTtemp; + + UARTtemp = USART1->DR; + UARTtemp = USART1->SR; + + // disabe DMA + DMA_Disable(USART1_Rx); + + //数据量正确 + if (DMA_Get_Stream(USART1_Rx)->NDTR == DBUS_BACK_LENGTH) { + DBus_Update(&remoteData, &keyboardData, &mouseData, remoteBuffer); //解码 + } + + // enable DMA + DMA_Enable(USART1_Rx, DBUS_LENGTH + DBUS_BACK_LENGTH); +} + +/** + * @brief USART3 串口中断 + */ +void USART3_IRQHandler(void) { + uint8_t tmp; + Bridge_Receive_USART(&BridgeData, USART_BRIDGE, 3); +} + +/** + * @brief USART6 串口中断 + */ +void USART6_IRQHandler(void) { + Bridge_Receive_USART(&BridgeData, USART_BRIDGE, 6); +} + +/** + * @brief UART7 串口中断 + */ +void UART7_IRQHandler(void) { + Bridge_Receive_USART(&BridgeData, USART_BRIDGE, 7); +} + +/** + * @brief UART8 串口中断 + */ +void UART8_IRQHandler(void) { + Bridge_Receive_USART(&BridgeData, USART_BRIDGE, 8); +} + +// CAN1数据接收中断服务函数 +void CAN1_RX0_IRQHandler(void) { + Bridge_Receive_CAN(&BridgeData, CAN1_BRIDGE); +} + +// CAN2数据接收中断服务函数 +void CAN2_RX0_IRQHandler(void) { + Bridge_Receive_CAN(&BridgeData, CAN2_BRIDGE); +} + +// void CAN1_SCE_IRQHandler(void) { +// RED_LIGHT_ON; +// CAN_ClearITPendingBit(CAN1, CAN_IT_EWG | CAN_IT_EPV | CAN_IT_BOF | CAN_IT_LEC | CAN_IT_ERR); +// } + +// TIM2 高频计数器 +extern volatile uint32_t ulHighFrequencyTimerTicks; + +void TIM2_IRQHandler(void) { + if (TIM_GetITStatus(TIM2, TIM_IT_Update) != RESET) { + ulHighFrequencyTimerTicks++; + TIM_ClearITPendingBit(TIM2, TIM_IT_Update); + TIM_ClearFlag(TIM2, TIM_FLAG_Update); + } +} + +/** + * @brief This function handles NMI exception. + * @param None + * @return None + */ + +void NMI_Handler(void) { + while (1) { + } +} + +/** + * @brief This function handles Hard Fault exception. + * @param None + * @return None + */ +void HardFault_Handler(void) { + while (1) { + } +} + +/** + * @brief This function handles Memory Manage exception. + * @param None + * @return None + */ +void MemManage_Handler(void) { + while (1) { + } +} + +/** + * @brief This function handles Bus Fault exception. + * @param None + * @return None + */ +void BusFault_Handler(void) { + while (1) { + } +} + +/** + * @brief This function handles Usage Fault exception. + * @param None + * @return None + */ +void UsageFault_Handler(void) { + while (1) { + } +} + +/** + * @brief This function handles Debug Monitor exception. + * @param None + * @return None + */ +void DebugMon_Handler(void) { + while (1) { + } +} + +// /** +// * @brief This function handles SVCall exception. +// * @param None +// * @return None +// */ +// void SVC_Handler(void) { +// //while(1){} +// } + +// /** +// * @brief This function handles PendSVC exception. +// * @param None +// * @return None +// */ +// void PendSV_Handler(void) { +// //while(1){} +// } + +// /** +// * @brief This function handles SysTick Handler. +// * @param None +// * @return None +// */ +// void SysTick_Handler(void) { +// //while(1){} +// } \ No newline at end of file diff --git a/user/gold-miner/main.c b/user/gold-miner/main.c new file mode 100644 index 00000000..b791672a --- /dev/null +++ b/user/gold-miner/main.c @@ -0,0 +1,102 @@ +#define __HANDLE_GLOBALS + +#include "config.h" +#include "macro.h" +#include "handle.h" +#include "FreeRTOS.h" +#include "task.h" +#include "tasks.h" +// #include "queue.h" + +int main(void) { + + //设置中断优先级位数 + NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4); + + //调试相关 + Delay_Init(180); // 初始化延时 + LED_Init(); // 初始化LED + Beep_Init(); // 初始化蜂鸣器 + + /******************************************************************************* + * 硬件初始化 * + *******************************************************************************/ + + // 获得设备ID + BSP_Stone_Id_Init(&Board_Id, &Robot_Id); + + // 底盘电机 + Motor_Init(&Motor_Arm, CHASSIS_MOTOR_REDUCTION_RATE, ENABLE, ENABLE); + + // 遥控器数据初始化 + DBUS_Init(&remoteData, &keyboardData, &mouseData); + + // 通讯协议初始化 + Protocol_Init(&Node_Judge, &ProtocolData); + Protocol_Init(&Node_Host, &ProtocolData); + Protocol_Init(&Node_Board, &ProtocolData); + + // 硬件配置 + BSP_CAN_Init(); + BSP_DBUS_Init(remoteBuffer); + BSP_TIM2_Init(); + BSP_IMU_Init(); + BSP_Laser_Init(); + BSP_Beep_Init(); + BSP_LED_Init(); + BSP_User_Power_Init(); + // BSP_OLED_Init(); + + // USART + // BSP_USART3_Init(115200, USART_IT_IDLE); // Rx故障 + // BSP_USART6_Init(115200, USART_IT_IDLE); // Tx故障 + BSP_UART7_Init(115200, USART_IT_IDLE); + BSP_UART8_Init(115200, USART_IT_IDLE); + + // PWM + // BSP_PWM_Set_Port(&PWM_Test, PWM_PD12); + // BSP_PWM_Init(&PWM_Test, 9000, 200, TIM_OCPolarity_Low); + + // ADC + BSP_ADC1_Init(1, ADC_Channel6, 0); + + // 总线设置 + Bridge_Bind(&BridgeData, CAN1_BRIDGE, 0x205, &Motor_Arm); + Bridge_Bind(&BridgeData, USART_BRIDGE, 7, &Node_Host); + Bridge_Bind(&BridgeData, USART_BRIDGE, 8, &Node_Judge); + + // 陀螺仪 + Gyroscope_Set_Bias(&ImuData, 0, 0, 0); // 设置静态误差 + Gyroscope_Init(&Gyroscope_EulerData, 300); // 初始化 + + /******************************************************************************* + * 任务初始化 * + *******************************************************************************/ + + // 低优先级任务 + xTaskCreate(Task_Blink, "Task_Blink", 400, NULL, 3, NULL); // 跑马灯/呼吸灯任务 + // xTaskCreate(Task_OLED, "Task_OLED", 400, NULL, 3, NULL);// OLED 菜单任务 + // xTaskCreate(Task_Startup_Music, "Task_Startup_Music", 400, NULL, 3, NULL);// 开机音乐任务 + + // 等待遥控器开启 + // while (!remoteData.state) { + // } + + // 高优先级任务 + xTaskCreate(Task_Control, "Task_Control", 400, NULL, 9, NULL); //模式切换任务 + // xTaskCreate(Task_Chassis, "Task_Chassis", 400, NULL, 5, NULL); // 底盘运动任务 + xTaskCreate(Task_Manipulator, "Task_Manipulator", 400, NULL, 5, NULL); // 机械臂运动任务 + // xTaskCreate(Task_Communication, "Task_Communication", 500, NULL, 6, NULL); // 通讯测试任务 + xTaskCreate(Task_Can_Send, "Task_Can_Send", 500, NULL, 6, NULL); // Can发送任务 + + // 定义协议发送频率 + Bridge_Send_Protocol(&Node_Host, 0x120, 1); // 心跳包 + // Bridge_Send_Protocol(&Node_Host, 0x501, 50); // 板间通讯 + + //启动调度,开始执行任务 + vTaskStartScheduler(); + + //系统启动失败:定时器任务或者空闲任务的heap空间不足 + while (1) { + } +} diff --git a/user/gold-miner/protocol.h b/user/gold-miner/protocol.h new file mode 100644 index 00000000..8a8f6aee --- /dev/null +++ b/user/gold-miner/protocol.h @@ -0,0 +1,506 @@ +#ifndef __PROTOCOL_H +#define __PROTOCOL_H +/** + * @file protocol.h + * @note 本文件由 Node_Bridge 生成, 请勿直接修改 + * + * @note Node command_id + * 裁判系统 0x0000 - 0x03FF + * 上位机 0x0400 - 0x04FF + * 板间通讯 0x0500 - 0x05FF + * 裁判系统 0xF100 - 0xF1FF + * 车间通讯 0xF200 - 0xF2FF + * @version judge.yml v1.0 + * @version host.yml v0.1 + * @version user.yml v0.1 + */ + +/********************************************************************** + * 结构体 + **********************************************************************/ + +typedef struct { + uint8_t graphic_name[3]; + uint32_t operate_tpye : 3; + uint32_t graphic_tpye : 3; + uint32_t layer : 4; + uint32_t color : 4; + uint32_t start_angle : 9; + uint32_t end_angle : 9; + uint32_t width : 10; + uint32_t start_x : 11; + uint32_t start_y : 11; + uint32_t radius : 10; + uint32_t end_x : 11; + uint32_t end_y : 11; +} GraphicData_Type; + +typedef struct { + int32_t debug1; + int32_t debug2; + int32_t debug3; + int32_t debug4; + int32_t debug5; + int32_t debug6; + int32_t debug7; + int32_t debug8; +} DebugData_Type; + +/********************************************************************** + * 协议列表 + **********************************************************************/ + +typedef union { + struct { + uint8_t game_type : 4; + uint8_t game_progress : 4; + uint16_t stage_remain_time; + uint64_t SyncTimeStamp; + }; + struct { + uint8_t data[11]; + }; +} game_status_t; + +typedef union { + struct { + uint8_t winner; + }; + struct { + uint8_t data[1]; + }; +} game_result_t; + +typedef union { + struct { + uint16_t red_1_robot_HP; + uint16_t red_2_robot_HP; + uint16_t red_3_robot_HP; + uint16_t red_4_robot_HP; + uint16_t red_5_robot_HP; + uint16_t red_7_robot_HP; + uint16_t red_outpost_HP; + uint16_t red_base_HP; + uint16_t blue_1_robot_HP; + uint16_t blue_2_robot_HP; + uint16_t blue_3_robot_HP; + uint16_t blue_4_robot_HP; + uint16_t blue_5_robot_HP; + uint16_t blue_7_robot_HP; + uint16_t blue_outpost_HP; + uint16_t blue_base_HP; + }; + struct { + uint8_t data[32]; + }; +} game_robot_HP_t; + +typedef union { + struct { + uint8_t dart_belong; + uint16_t stage_remaining_time; + }; + struct { + uint8_t data[3]; + }; +} dart_status_t; + +typedef union { + struct { + uint32_t event_type; + }; + struct { + uint8_t data[4]; + }; +} event_data_t; + +typedef union { + struct { + uint8_t supply_projectile_id; + uint8_t supply_robot_id; + uint8_t supply_projectile_step; + uint8_t supply_projectile_num; + }; + struct { + uint8_t data[4]; + }; +} supply_projectile_action_t; + +typedef union { + struct { + uint8_t level; + uint8_t foul_robot_id; + }; + struct { + uint8_t data[2]; + }; +} referee_warning_t; + +typedef union { + struct { + uint8_t dart_remaining_time; + }; + struct { + uint8_t data[1]; + }; +} dart_remaining_time_t; + +typedef union { + struct { + uint8_t robot_id; + uint8_t robot_level; + uint16_t remain_HP; + uint16_t max_HP; + uint16_t shooter_id1_17mm_cooling_rate; + uint16_t shooter_id1_17mm_cooling_limit; + uint16_t shooter_id1_17mm_speed_limit; + uint16_t shooter_id2_17mm_cooling_rate; + uint16_t shooter_id2_17mm_cooling_limit; + uint16_t shooter_id2_17mm_speed_limit; + uint16_t shooter_id1_42mm_cooling_rate; + uint16_t shooter_id1_42mm_cooling_limit; + uint16_t shooter_id1_42mm_speed_limit; + uint16_t chassis_power_limit; + uint8_t mains_power_gimbal_output : 1; + uint8_t mains_power_chassis_output : 1; + uint8_t mains_power_shooter_output : 1; + }; + struct { + uint8_t data[27]; + }; +} game_robot_status_t; + +typedef union { + struct { + uint16_t chassis_volt; + uint16_t chassis_current; + float chassis_power; + uint16_t chassis_power_buffer; + uint16_t shooter_id1_17mm_cooling_heat; + uint16_t shooter_id2_17mm_cooling_heat; + uint16_t shooter_id1_42mm_cooling_heat; + }; + struct { + uint8_t data[16]; + }; +} power_heat_data_t; + +typedef union { + struct { + float x; + float y; + float z; + float yaw; + }; + struct { + uint8_t data[16]; + }; +} game_robot_pos_t; + +typedef union { + struct { + uint8_t power_rune_buff; + }; + struct { + uint8_t data[1]; + }; +} buff_info_t; + +typedef union { + struct { + uint8_t attack_time; + }; + struct { + uint8_t data[1]; + }; +} aerial_robot_energy_t; + +typedef union { + struct { + uint8_t armor_id : 4; + uint8_t hurt_type : 4; + }; + struct { + uint8_t data[1]; + }; +} robot_hurt_t; + +typedef union { + struct { + uint8_t bullet_type; + uint8_t shooter_id; + uint8_t bullet_freq; + float bullet_speed; + }; + struct { + uint8_t data[7]; + }; +} shoot_data_t; + +typedef union { + struct { + uint16_t bullet_remaining_num_17mm; + uint16_t bullet_remaining_num_42mm; + uint16_t coin_remaining_num; + }; + struct { + uint8_t data[6]; + }; +} bullet_remaining_t; + +typedef union { + struct { + uint32_t rfid_status; + }; + struct { + uint8_t data[4]; + }; +} rfid_status_t; + +typedef union { + struct { + uint8_t dart_launch_opening_status; + uint8_t dart_attack_target; + uint16_t target_change_time; + uint8_t first_dart_speed; + uint8_t second_dart_speed; + uint8_t third_dart_speed; + uint8_t fourth_dart_speed; + uint16_t last_dart_launch_time; + uint16_t operate_launch_cmd_time; + }; + struct { + uint8_t data[12]; + }; +} dart_client_cmd_t; + +typedef union { + struct { + uint16_t data_cmd_id; + uint16_t send_id; + uint16_t receiver_id; + uint8_t operate_tpye; + uint8_t layer; + }; + struct { + uint8_t data[8]; + }; +} client_custom_graphic_delete_t; + +typedef union { + struct { + uint16_t data_cmd_id; + uint16_t send_id; + uint16_t receiver_id; + GraphicData_Type grapic_data_struct; + }; + struct { + uint8_t data[21]; + }; +} client_custom_graphic_single_t; + +typedef union { + struct { + uint16_t data_cmd_id; + uint16_t send_id; + uint16_t receiver_id; + GraphicData_Type grapic_data_struct[2]; + }; + struct { + uint8_t data[36]; + }; +} client_custom_graphic_double_t; + +typedef union { + struct { + uint16_t data_cmd_id; + uint16_t send_id; + uint16_t receiver_id; + GraphicData_Type grapic_data_struct[5]; + }; + struct { + uint8_t data[81]; + }; +} client_custom_graphic_five_t; + +typedef union { + struct { + uint16_t data_cmd_id; + uint16_t send_id; + uint16_t receiver_id; + GraphicData_Type grapic_data_struct; + uint8_t data[30]; + }; + struct { + uint8_t data[51]; + }; +} client_custom_character_t; + +typedef union { + struct { + uint16_t data_cmd_id; + uint16_t send_id; + uint16_t receiver_id; + GraphicData_Type grapic_data_struct[7]; + }; + struct { + uint8_t data[111]; + }; +} client_custom_graphic_seven_t; + +typedef union { + struct { + DebugData_Type debugData; + }; + struct { + uint8_t data[32]; + }; +} debug_info_t; + +typedef union { + struct { + uint16_t code; + char text[22]; + }; + struct { + uint8_t data[24]; + }; +} error_info_t; + +typedef union { + struct {}; + struct { + uint8_t data[0]; + }; +} heartbeat_t; + +typedef union { + struct { + float yaw_angle_diff; + float pitch_angle_diff; + uint8_t fire; + }; + struct { + uint8_t data[9]; + }; +} autoaim_data_t; + +typedef union { + struct { + float vx; + float vy; + float vw; + }; + struct { + uint8_t data[12]; + }; +} chassis_data_t; + +typedef union { + struct { + float yaw; + float pitch; + float roll; + }; + struct { + uint8_t data[12]; + }; +} gyroscope_data_t; + +typedef union { + struct { + uint8_t dbusBuffer[18]; + }; + struct { + uint8_t data[18]; + }; +} dbus_data_t; + +typedef union { + struct { + uint16_t data_cmd_id; + uint16_t send_id; + uint16_t receiver_id; + int32_t data1; + int32_t data2; + int32_t data3; + int32_t data4; + }; + struct { + uint8_t data[22]; + }; +} robot_interactive_data_t; + +typedef union { + struct { + int32_t data1; + int32_t data2; + int32_t data3; + int32_t data4; + }; + struct { + uint8_t data[16]; + }; +} board_interactive_data_t; + +/********************************************************************** + * 协议信息 + **********************************************************************/ + +#define PROTOCOL_INFO_LIST \ + { \ + {0X0001, 11, 1}, {0X0002, 1, 1}, {0X0003, 32, 1}, {0X0004, 3, 1}, {0X0101, 4, 1}, {0X0102, 4, 1}, {0X0104, 2, 1}, {0X0105, 1, 1}, {0X0201, 27, 1}, \ + {0X0202, 16, 1}, {0X0203, 16, 1}, {0X0204, 1, 1}, {0X0205, 1, 1}, {0X0206, 1, 1}, {0X0207, 7, 1}, {0X0208, 6, 1}, {0X0209, 4, 1}, {0X020A, 12, 1}, \ + {0XF100, 8, 0}, {0XF101, 21, 0}, {0XF102, 36, 0}, {0XF103, 81, 0}, {0XF110, 51, 0}, {0XF104, 111, 0}, {0X1024, 32, 0}, {0X6666, 24, 0}, \ + {0X0120, 0, 1}, {0X0401, 9, 1}, {0X0402, 12, 1}, {0X0403, 12, 0}, {0X0404, 18, 0}, {0XF301, 22, 1}, {0X0501, 16, 1}, { \ + 0X0502, 16, 1 \ + } \ + } + +/********************************************************************** + * 协议接口 + **********************************************************************/ + +typedef union { + struct { + game_status_t gameStatus; // 0X0001 比赛状态数据 + game_result_t gameResult; // 0X0002 比赛结果的数据 + game_robot_HP_t healthPoint; // 0X0003 机器人血量数据 + dart_status_t dartStatus; // 0X0004 飞镖发射状态 + event_data_t eventData; // 0X0101 场地事件数据 + supply_projectile_action_t supplyProjectileaction; // 0X0102 补给站动作标识 + referee_warning_t refereeWarning; // 0X0104 裁判警告信息 + dart_remaining_time_t dartRemainingtime; // 0X0105 飞镖发射口倒计 + game_robot_status_t gameRobotstatus; // 0X0201 比赛机器人状态 + power_heat_data_t powerHeatData; // 0X0202 实时功率热量数据 + game_robot_pos_t gameRobotpos; // 0X0203 机器人位置 + buff_info_t buffInfo; // 0X0204 机器人增益 + aerial_robot_energy_t aerialRobotenergy; // 0X0205 空中机器人能量状态 + robot_hurt_t robotHurt; // 0X0206 伤害状态 + shoot_data_t shootData; // 0X0207 实时射击信息 + bullet_remaining_t bulletRemaining; // 0X0208 子弹剩余发射数 + rfid_status_t rfidStatus; // 0X0209 机器人RFID状态 + dart_client_cmd_t dartClientCmds; // 0X020A 飞镖机器人客户端指令数据 + client_custom_graphic_delete_t clientCustomGraphicDelete; // 0XF100 客户端删除图形 + client_custom_graphic_single_t client_custom_graphicSingle; // 0XF101 客户端绘制一个图形 + client_custom_graphic_double_t clientCustomGraphicDouble; // 0XF102 客户端绘制两个图形 + client_custom_graphic_five_t clientCustomGraphicFive; // 0XF103 客户端绘制五个图形 + client_custom_character_t clientCustomCharacter; // 0XF110 客户端绘制字符 + client_custom_graphic_seven_t clientCustomGraphicSeven; // 0XF104 客户端绘制七个图形 + debug_info_t debugInfo; // 0X1024 调试信息 + error_info_t errorInfo; // 0X6666 报错信息 + heartbeat_t heartbeat; // 0X0120 心跳包 + autoaim_data_t autoaimData; // 0X0401 云台控制 + chassis_data_t chassisData; // 0X0402 底盘控制 + gyroscope_data_t gyroscopeData; // 0X0403 陀螺仪数据 + dbus_data_t dbusData; // 0X0404 遥控器及键鼠数据 + robot_interactive_data_t robotCommunication; // 0XF301 学生机器人间通信 + board_interactive_data_t boardAlpha; // 0X0501 主控板间通信 + board_interactive_data_t boardBeta; // 0X0502 主控板间通信 + }; + struct { + uint8_t data[618]; + }; +} ProtocolData_Type; + +#endif \ No newline at end of file diff --git a/user/gold-miner/tasks.c b/user/gold-miner/tasks.c new file mode 100644 index 00000000..c63b9d18 --- /dev/null +++ b/user/gold-miner/tasks.c @@ -0,0 +1,200 @@ +/** + * @brief 任务 + */ +#include "tasks.h" +#include "config.h" +#include "macro.h" +#include "handle.h" + +void Task_Control(void *Parameters) { + TickType_t LastWakeTime = xTaskGetTickCount(); + float interval = 0.01; // 任务运行间隔 s + int intervalms = interval * 1000; // 任务运行间隔 ms + while (1) { + // SafetyMode = (RIGHT_SWITCH_BOTTOM && LEFT_SWITCH_BOTTOM); + vTaskDelayUntil(&LastWakeTime, intervalms); + } + vTaskDelete(NULL); +} + +void Task_Chassis(void *Parameters) { + // 任务 + TickType_t LastWakeTime = xTaskGetTickCount(); // 时钟 + float interval = 0.005; // 任务运行间隔 s + int intervalms = interval * 1000; // 任务运行间隔 ms + + // 运动模式 + int mode = 1; // 底盘运动模式,1直线,2转弯 + int lastMode = 2; // 上一次的运动模式 + float yawAngleTarget = 0; // 目标值 + float yawAngle, yawSpeed; // 反馈值 + + // 初始化麦轮角速度PID + PID_Init(&PID_LFCM, 15, 0.3, 0, 4000, 2000); + PID_Init(&PID_LBCM, 15, 0.3, 0, 4000, 2000); + PID_Init(&PID_RBCM, 15, 0.3, 0, 4000, 2000); + PID_Init(&PID_RFCM, 15, 0.3, 0, 4000, 2000); + + // 初始化航向角角度PID和角速度PID + PID_Init(&PID_YawAngle, 10, 0, 0, 1000, 1000); + PID_Init(&PID_YawSpeed, 2, 0, 0, 4000, 1000); + + // 初始化底盘 + Chassis_Init(&ChassisData); + + while (1) { + + // 更新运动模式 + mode = ABS(remoteData.rx) < 5 ? 1 : 2; + + // 设置反馈值 + yawAngle = Gyroscope_EulerData.yaw; // 航向角角度反馈 + yawSpeed = -1 * ImuData.gz / GYROSCOPE_LSB; // 逆时针为正 + + // 切换运动模式 + if (mode != lastMode) { + PID_YawAngle.output_I = 0; // 清空角度PID积分 + PID_YawSpeed.output_I = 0; // 清空角速度PID积分 + yawAngleTarget = yawAngle; // 更新角度PID目标值 + lastMode = mode; // 更新lastMode + } + + // 根据运动模式计算PID + if (mode == 1) { + PID_Calculate(&PID_YawAngle, yawAngleTarget, yawAngle); // 计算航向角角度PID + PID_Calculate(&PID_YawSpeed, PID_YawAngle.output, yawSpeed); // 计算航向角角速度PID + } else { + PID_Calculate(&PID_YawSpeed, -remoteData.rx, yawSpeed); // 计算航向角角速度PID + } + + // 设置底盘总体移动速度 + Chassis_Update(&ChassisData, (float) -remoteData.lx / 660.0f, (float) remoteData.ly / 660.0f, (float) PID_YawSpeed.output / 660.0f); + + // 麦轮解算 + Chassis_Calculate_Rotor_Speed(&ChassisData); + + // 设置转子速度上限 (rad/s) + Chassis_Limit_Rotor_Speed(&ChassisData, 300); + + // 计算输出电流PID + PID_Calculate(&PID_LFCM, ChassisData.rotorSpeed[0], Motor_LF.speed * RPM2RPS); + PID_Calculate(&PID_LBCM, ChassisData.rotorSpeed[1], Motor_LB.speed * RPM2RPS); + PID_Calculate(&PID_RBCM, ChassisData.rotorSpeed[2], Motor_RB.speed * RPM2RPS); + PID_Calculate(&PID_RFCM, ChassisData.rotorSpeed[3], Motor_RF.speed * RPM2RPS); + + // 输出电流值到电调(安全起见默认注释此行) + Motor_LF.input = PID_LFCM.output; + Motor_LB.input = PID_LBCM.output; + Motor_RB.input = PID_RBCM.output; + Motor_RF.input = PID_RFCM.output; + + // 底盘运动更新频率 + vTaskDelayUntil(&LastWakeTime, intervalms); + } + + vTaskDelete(NULL); +} + +void Task_Communication(void *Parameters) { + TickType_t LastWakeTime = xTaskGetTickCount(); // 时钟 + float interval = 0.001; // 任务运行间隔 s + int intervalms = interval * 1000; // 任务运行间隔 ms + ProtocolInfo_Type *protocolInfo = Protocol_Get_Info_Handle(0x501); + extern DMA_Type DMA_Table[10]; + while (1) { + + // 修改数据 + ProtocolData.boardAlpha.data1 = 1; + ProtocolData.boardAlpha.data2 = 2; + ProtocolData.boardAlpha.data3 = 3; + ProtocolData.boardAlpha.data4 = 4; + + // 建议在main.c中使用Bridge_Send_Protocol + // Bridge_Send_Protocol_Once(&Node_Host, 0x501); + + // 发送频率 + vTaskDelayUntil(&LastWakeTime, intervalms); + + // 调试信息 + // DebugData.debug1 = ProtocolData.boardAlpha.data1 * 1000; + // DebugData.debug2 = ProtocolData.boardAlpha.data2 * 1000; + // DebugData.debug3 = Node_Host.sendSeq; + // DebugData.debug3 = Node_Host.sendLock; + // DebugData.debug4 = protocolInfo->receiveCount; + } + vTaskDelete(NULL); +} + +void Task_Manipulator(void *Parameters) { + TickType_t LastWakeTime = xTaskGetTickCount(); // 时钟 + float interval = 0.01; // 任务运行间隔 s + int intervalms = interval * 1000; // 任务运行间隔 ms + + PID_Init(&PID_Arm_Speed, 40, 0, 0, 16384, 16384); + PID_Init(&PID_Arm_Angle, 0, 0, 0, 16384, 16384); + + int armAngle = 0; + while (1) { + + PID_Arm_Speed.i = CHOOSEL(1, 3, 10); + PID_Calculate(&PID_Arm_Speed, (remoteData.ly > 0 ? 1 : -1) * (ABS(remoteData.ly) > 330 ? 200 : 0), Motor_Arm.speed * RPM2RPS); + + // armAngle += remoteData.ly / 660.0f * 50; + // PID_Calculate(&PID_Arm_Angle, armAngle, Motor_Arm.angle * RPM2RPS); + // PID_Calculate(&PID_Arm_Speed, PID_Arm_Angle.output, Motor_Arm.speed * RPM2RPS); + + Motor_Arm.input = PID_Arm_Speed.output; + vTaskDelayUntil(&LastWakeTime, intervalms); // 发送频率 + + DebugData.debug1 = PID_Arm_Speed.target; + DebugData.debug2 = PID_Arm_Speed.feedback; + DebugData.debug3 = PID_Arm_Speed.output; + } + vTaskDelete(NULL); +} + +void Task_Can_Send(void *Parameters) { + TickType_t LastWakeTime = xTaskGetTickCount(); // 时钟 + float interval = 0.01; // 任务运行间隔 s + int intervalms = interval * 1000; // 任务运行间隔 ms + + while (1) { + Bridge_Send_Motor(&BridgeData, SafetyMode); + vTaskDelayUntil(&LastWakeTime, intervalms); // 发送频率 + } + vTaskDelete(NULL); +} + +void Task_Blink(void *Parameters) { + TickType_t LastWakeTime = xTaskGetTickCount(); + while (1) { + LED_Run_Horse_XP(); // XP开机动画,建议延时200ms + // LED_Run_Horse(); // 跑马灯,建议延时20ms + vTaskDelayUntil(&LastWakeTime, 200); + } + + vTaskDelete(NULL); +} + +void Task_Startup_Music(void *Parameters) { + TickType_t LastWakeTime = xTaskGetTickCount(); + while (1) { + if (KTV_Play(Music_Soul)) break; + vTaskDelayUntil(&LastWakeTime, 60); + } + vTaskDelete(NULL); +} + +void Task_OLED(void *Parameters) { + uint16_t JoystickValue = -1; + TickType_t LastWakeTime = xTaskGetTickCount(); + oled_init(); + while (1) { + JoystickValue = ADC_GetConversionValue(ADC1); + oled_clear(Pen_Clear); + oled_menu(JoystickValue); + oled_refresh_gram(); + vTaskDelayUntil(&LastWakeTime, 125); + } + vTaskDelete(NULL); +} diff --git a/user/gold-miner/tasks.h b/user/gold-miner/tasks.h new file mode 100644 index 00000000..dc63063b --- /dev/null +++ b/user/gold-miner/tasks.h @@ -0,0 +1,13 @@ +#ifndef __TASKS_H +#define __TASKS_H + +void Task_Control(void *Parameters); +void Task_Manipulator(void *Parameters); +void Task_Blink(void *Parameters); +void Task_Chassis(void *Parameters); +void Task_Host(void *Parameters); +void Task_Can_Send(void *Parameters); +void Task_Startup_Music(void *Parameters); +void Task_OLED(void *Parameters); + +#endif From 3fb83dd3de488d4395a1528fc66d6f3dfe5f1926 Mon Sep 17 00:00:00 2001 From: Link Wu <1774241372@qq.com> Date: Sat, 22 May 2021 18:52:21 +0800 Subject: [PATCH 02/10] =?UTF-8?q?=E6=9B=B4=E6=96=B0=E7=89=88=E6=9C=AC?= =?UTF-8?q?=E5=8F=B7?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- 2.0.ver | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 2.0.ver diff --git a/2.0.ver b/2.0.ver new file mode 100644 index 00000000..e69de29b From 44cc3a69a50979f3ae479b1fb569e4c48c60b419 Mon Sep 17 00:00:00 2001 From: Link Wu <1774241372@qq.com> Date: Sat, 22 May 2021 19:58:45 +0800 Subject: [PATCH 03/10] =?UTF-8?q?=E6=B3=A8=E9=87=8A=E6=9C=BA=E6=A2=B0?= =?UTF-8?q?=E8=87=82=E6=B5=8B=E8=AF=95=E4=BB=A3=E7=A0=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- user/gold-miner/main.c | 10 +++++----- user/gold-miner/tasks.c | 4 +--- 2 files changed, 6 insertions(+), 8 deletions(-) diff --git a/user/gold-miner/main.c b/user/gold-miner/main.c index b791672a..338098e1 100644 --- a/user/gold-miner/main.c +++ b/user/gold-miner/main.c @@ -25,8 +25,8 @@ int main(void) { // 获得设备ID BSP_Stone_Id_Init(&Board_Id, &Robot_Id); - // 底盘电机 - Motor_Init(&Motor_Arm, CHASSIS_MOTOR_REDUCTION_RATE, ENABLE, ENABLE); + // 电机 + Motor_Init(&Motor_Arm, CHASSIS_MOTOR_REDUCTION_RATE, ENABLE, DISABLE); // 遥控器数据初始化 DBUS_Init(&remoteData, &keyboardData, &mouseData); @@ -85,9 +85,9 @@ int main(void) { // 高优先级任务 xTaskCreate(Task_Control, "Task_Control", 400, NULL, 9, NULL); //模式切换任务 // xTaskCreate(Task_Chassis, "Task_Chassis", 400, NULL, 5, NULL); // 底盘运动任务 - xTaskCreate(Task_Manipulator, "Task_Manipulator", 400, NULL, 5, NULL); // 机械臂运动任务 - // xTaskCreate(Task_Communication, "Task_Communication", 500, NULL, 6, NULL); // 通讯测试任务 - xTaskCreate(Task_Can_Send, "Task_Can_Send", 500, NULL, 6, NULL); // Can发送任务 + xTaskCreate(Task_Manipulator, "Task_Manipulator", 400, NULL, 5, NULL); // 机械臂运动任务 + xTaskCreate(Task_Communication, "Task_Communication", 500, NULL, 6, NULL); // 通讯测试任务 + xTaskCreate(Task_Can_Send, "Task_Can_Send", 500, NULL, 6, NULL); // Can发送任务 // 定义协议发送频率 Bridge_Send_Protocol(&Node_Host, 0x120, 1); // 心跳包 diff --git a/user/gold-miner/tasks.c b/user/gold-miner/tasks.c index c63b9d18..fb56fc4b 100644 --- a/user/gold-miner/tasks.c +++ b/user/gold-miner/tasks.c @@ -130,13 +130,11 @@ void Task_Manipulator(void *Parameters) { float interval = 0.01; // 任务运行间隔 s int intervalms = interval * 1000; // 任务运行间隔 ms - PID_Init(&PID_Arm_Speed, 40, 0, 0, 16384, 16384); + PID_Init(&PID_Arm_Speed, 40, 3, 0, 16384, 16384); PID_Init(&PID_Arm_Angle, 0, 0, 0, 16384, 16384); int armAngle = 0; while (1) { - - PID_Arm_Speed.i = CHOOSEL(1, 3, 10); PID_Calculate(&PID_Arm_Speed, (remoteData.ly > 0 ? 1 : -1) * (ABS(remoteData.ly) > 330 ? 200 : 0), Motor_Arm.speed * RPM2RPS); // armAngle += remoteData.ly / 660.0f * 50; From e311a5f5d8a329be7df36c79dfe2c6ff9dbe8722 Mon Sep 17 00:00:00 2001 From: Link Wu <1774241372@qq.com> Date: Mon, 24 May 2021 21:28:55 +0800 Subject: [PATCH 04/10] =?UTF-8?q?=E6=9B=B4=E6=96=B0=E7=A1=AC=E4=BB=B6?= =?UTF-8?q?=E9=85=8D=E7=BD=AE?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- user/common/macro.h | 5 + user/gold-miner/handle.h | 7 +- user/gold-miner/main.c | 32 ++- user/gold-miner/protocol.h | 29 ++- user/gold-miner/tasks.c | 24 +- user/gold-miner/tasks.h | 1 + user/protocol.h | 506 ------------------------------------- 7 files changed, 76 insertions(+), 528 deletions(-) delete mode 100644 user/protocol.h diff --git a/user/common/macro.h b/user/common/macro.h index 7bcad2f7..febba4bf 100644 --- a/user/common/macro.h +++ b/user/common/macro.h @@ -8,6 +8,11 @@ // clang-format off +// 电机减速比 +#define MOTOR_REDUCTION_RATE_3508 19.2f +#define MOTOR_REDUCTION_RATE_6020 1 +#define MOTOR_REDUCTION_RATE_2006 36.0f + // 单位换算 #define RPM2RPS 0.10471975f // 2 * 3.1415926f / 60.0f, round/min->rad/s #define DPS2RPS 0.01745329f // 3.1415926f / 180.0f, degree/s ->rad/s diff --git a/user/gold-miner/handle.h b/user/gold-miner/handle.h index ad091b3b..f58c6add 100644 --- a/user/gold-miner/handle.h +++ b/user/gold-miner/handle.h @@ -41,8 +41,11 @@ __HANDLE_EXT uint8_t SafetyMode; // TIM __HANDLE_EXT volatile uint32_t ulHighFrequencyTimerTicks; -// 电机 -__HANDLE_EXT Motor_Type Motor_Arm; +// 底盘电机 +__HANDLE_EXT Motor_Type Motor_LF, Motor_RF, Motor_RB, Motor_LB; + +// 机械臂 +__HANDLE_EXT Motor_Type Motor_Base_Joint, Motor_Shoulder_Joint, Motor_Elbow_Joint, Motor_Wrist_Joint_1, Motor_Wrist_Joint_2; __HANDLE_EXT PID_Type PID_Arm_Speed, PID_Arm_Angle; // 遥控器 diff --git a/user/gold-miner/main.c b/user/gold-miner/main.c index 338098e1..8f7c93b0 100644 --- a/user/gold-miner/main.c +++ b/user/gold-miner/main.c @@ -25,8 +25,18 @@ int main(void) { // 获得设备ID BSP_Stone_Id_Init(&Board_Id, &Robot_Id); - // 电机 - Motor_Init(&Motor_Arm, CHASSIS_MOTOR_REDUCTION_RATE, ENABLE, DISABLE); + // 底盘电机 + Motor_Init(&Motor_LF, CHASSIS_MOTOR_REDUCTION_RATE, DISABLE, DISABLE); + Motor_Init(&Motor_LB, CHASSIS_MOTOR_REDUCTION_RATE, DISABLE, DISABLE); + Motor_Init(&Motor_RB, CHASSIS_MOTOR_REDUCTION_RATE, DISABLE, DISABLE); + Motor_Init(&Motor_RF, CHASSIS_MOTOR_REDUCTION_RATE, DISABLE, DISABLE); + + // 机械臂电机 + Motor_Init(&Motor_Base_Joint, MOTOR_REDUCTION_RATE_6020, ENABLE, ENABLE); + Motor_Init(&Motor_Shoulder_Joint, MOTOR_REDUCTION_RATE_3508 * 50, ENABLE, ENABLE); + Motor_Init(&Motor_Elbow_Joint, MOTOR_REDUCTION_RATE_3508 * 50, ENABLE, ENABLE); + Motor_Init(&Motor_Wrist_Joint_1, MOTOR_REDUCTION_RATE_6020, ENABLE, ENABLE); + Motor_Init(&Motor_Wrist_Joint_2, MOTOR_REDUCTION_RATE_2006, ENABLE, ENABLE); // 遥控器数据初始化 DBUS_Init(&remoteData, &keyboardData, &mouseData); @@ -61,7 +71,15 @@ int main(void) { BSP_ADC1_Init(1, ADC_Channel6, 0); // 总线设置 - Bridge_Bind(&BridgeData, CAN1_BRIDGE, 0x205, &Motor_Arm); + Bridge_Bind(&BridgeData, CAN1_BRIDGE, 0x201, &Motor_LF); + Bridge_Bind(&BridgeData, CAN1_BRIDGE, 0x202, &Motor_LB); + Bridge_Bind(&BridgeData, CAN1_BRIDGE, 0x203, &Motor_RB); + Bridge_Bind(&BridgeData, CAN1_BRIDGE, 0x204, &Motor_RF); + Bridge_Bind(&BridgeData, CAN2_BRIDGE, 0x205, &Motor_Base_Joint); + Bridge_Bind(&BridgeData, CAN2_BRIDGE, 0x201, &Motor_Shoulder_Joint); + Bridge_Bind(&BridgeData, CAN2_BRIDGE, 0x202, &Motor_Elbow_Joint); + Bridge_Bind(&BridgeData, CAN2_BRIDGE, 0x206, &Motor_Wrist_Joint_1); + Bridge_Bind(&BridgeData, CAN2_BRIDGE, 0x203, &Motor_Wrist_Joint_2); Bridge_Bind(&BridgeData, USART_BRIDGE, 7, &Node_Host); Bridge_Bind(&BridgeData, USART_BRIDGE, 8, &Node_Judge); @@ -85,13 +103,13 @@ int main(void) { // 高优先级任务 xTaskCreate(Task_Control, "Task_Control", 400, NULL, 9, NULL); //模式切换任务 // xTaskCreate(Task_Chassis, "Task_Chassis", 400, NULL, 5, NULL); // 底盘运动任务 - xTaskCreate(Task_Manipulator, "Task_Manipulator", 400, NULL, 5, NULL); // 机械臂运动任务 + // xTaskCreate(Task_Manipulator, "Task_Manipulator", 400, NULL, 5, NULL); // 机械臂运动任务 xTaskCreate(Task_Communication, "Task_Communication", 500, NULL, 6, NULL); // 通讯测试任务 - xTaskCreate(Task_Can_Send, "Task_Can_Send", 500, NULL, 6, NULL); // Can发送任务 + // xTaskCreate(Task_Can_Send, "Task_Can_Send", 500, NULL, 6, NULL); // Can发送任务 // 定义协议发送频率 - Bridge_Send_Protocol(&Node_Host, 0x120, 1); // 心跳包 - // Bridge_Send_Protocol(&Node_Host, 0x501, 50); // 板间通讯 + Bridge_Send_Protocol(&Node_Host, 0x120, 1); // 心跳包 + Bridge_Send_Protocol(&Node_Host, 0x405, 250); // 机械臂数据 //启动调度,开始执行任务 vTaskStartScheduler(); diff --git a/user/gold-miner/protocol.h b/user/gold-miner/protocol.h index 8a8f6aee..74258e38 100644 --- a/user/gold-miner/protocol.h +++ b/user/gold-miner/protocol.h @@ -11,8 +11,8 @@ * 裁判系统 0xF100 - 0xF1FF * 车间通讯 0xF200 - 0xF2FF * @version judge.yml v1.0 - * @version host.yml v0.1 - * @version user.yml v0.1 + * @version host.yml v1.0 + * @version user.yml v1.0 */ /********************************************************************** @@ -416,6 +416,24 @@ typedef union { }; } dbus_data_t; +typedef union { + struct { + int16_t base_joint_position; + int16_t base_joint_speed; + int16_t shoulder_joint_position; + int16_t shoulder_joint_speed; + int16_t elbow_joint_position; + int16_t elbow_joint_speed; + int16_t wrist_joint_1_position; + int16_t wrist_joint_1_speed; + int16_t wrist_joint_2_position; + int16_t wrist_joint_2_speed; + }; + struct { + uint8_t data[20]; + }; +} joint_info_t; + typedef union { struct { uint16_t data_cmd_id; @@ -452,7 +470,7 @@ typedef union { {0X0001, 11, 1}, {0X0002, 1, 1}, {0X0003, 32, 1}, {0X0004, 3, 1}, {0X0101, 4, 1}, {0X0102, 4, 1}, {0X0104, 2, 1}, {0X0105, 1, 1}, {0X0201, 27, 1}, \ {0X0202, 16, 1}, {0X0203, 16, 1}, {0X0204, 1, 1}, {0X0205, 1, 1}, {0X0206, 1, 1}, {0X0207, 7, 1}, {0X0208, 6, 1}, {0X0209, 4, 1}, {0X020A, 12, 1}, \ {0XF100, 8, 0}, {0XF101, 21, 0}, {0XF102, 36, 0}, {0XF103, 81, 0}, {0XF110, 51, 0}, {0XF104, 111, 0}, {0X1024, 32, 0}, {0X6666, 24, 0}, \ - {0X0120, 0, 1}, {0X0401, 9, 1}, {0X0402, 12, 1}, {0X0403, 12, 0}, {0X0404, 18, 0}, {0XF301, 22, 1}, {0X0501, 16, 1}, { \ + {0X0120, 0, 1}, {0X0401, 9, 1}, {0X0402, 12, 1}, {0X0403, 12, 0}, {0X0404, 18, 0}, {0X0405, 20, 0}, {0XF301, 22, 1}, {0X0501, 16, 1}, { \ 0X0502, 16, 1 \ } \ } @@ -493,13 +511,14 @@ typedef union { autoaim_data_t autoaimData; // 0X0401 云台控制 chassis_data_t chassisData; // 0X0402 底盘控制 gyroscope_data_t gyroscopeData; // 0X0403 陀螺仪数据 - dbus_data_t dbusData; // 0X0404 遥控器及键鼠数据 + dbus_data_t dbugData; // 0X0404 遥控器及键鼠数据 + joint_info_t jointInfo; // 0X0405 工程机械臂数据 robot_interactive_data_t robotCommunication; // 0XF301 学生机器人间通信 board_interactive_data_t boardAlpha; // 0X0501 主控板间通信 board_interactive_data_t boardBeta; // 0X0502 主控板间通信 }; struct { - uint8_t data[618]; + uint8_t data[638]; }; } ProtocolData_Type; diff --git a/user/gold-miner/tasks.c b/user/gold-miner/tasks.c index fb56fc4b..2de18a2e 100644 --- a/user/gold-miner/tasks.c +++ b/user/gold-miner/tasks.c @@ -97,17 +97,25 @@ void Task_Chassis(void *Parameters) { void Task_Communication(void *Parameters) { TickType_t LastWakeTime = xTaskGetTickCount(); // 时钟 - float interval = 0.001; // 任务运行间隔 s + float interval = 0.008; // 任务运行间隔 s int intervalms = interval * 1000; // 任务运行间隔 ms ProtocolInfo_Type *protocolInfo = Protocol_Get_Info_Handle(0x501); extern DMA_Type DMA_Table[10]; - while (1) { + int8_t i = 0; + while (1) { + i++; // 修改数据 - ProtocolData.boardAlpha.data1 = 1; - ProtocolData.boardAlpha.data2 = 2; - ProtocolData.boardAlpha.data3 = 3; - ProtocolData.boardAlpha.data4 = 4; + ProtocolData.jointInfo.base_joint_position = i; + ProtocolData.jointInfo.base_joint_speed = i + 1; + ProtocolData.jointInfo.shoulder_joint_position = i + 2; + ProtocolData.jointInfo.shoulder_joint_speed = i + 3; + ProtocolData.jointInfo.elbow_joint_position = i + 4; + ProtocolData.jointInfo.elbow_joint_speed = i + 5; + ProtocolData.jointInfo.wrist_joint_1_position = i + 6; + ProtocolData.jointInfo.wrist_joint_1_speed = i + 7; + ProtocolData.jointInfo.wrist_joint_2_position = i + 8; + ProtocolData.jointInfo.wrist_joint_2_speed = i + 9; // 建议在main.c中使用Bridge_Send_Protocol // Bridge_Send_Protocol_Once(&Node_Host, 0x501); @@ -135,13 +143,13 @@ void Task_Manipulator(void *Parameters) { int armAngle = 0; while (1) { - PID_Calculate(&PID_Arm_Speed, (remoteData.ly > 0 ? 1 : -1) * (ABS(remoteData.ly) > 330 ? 200 : 0), Motor_Arm.speed * RPM2RPS); + // PID_Calculate(&PID_Arm_Speed, (remoteData.ly > 0 ? 1 : -1) * (ABS(remoteData.ly) > 330 ? 200 : 0), Motor_Arm.speed * RPM2RPS); // armAngle += remoteData.ly / 660.0f * 50; // PID_Calculate(&PID_Arm_Angle, armAngle, Motor_Arm.angle * RPM2RPS); // PID_Calculate(&PID_Arm_Speed, PID_Arm_Angle.output, Motor_Arm.speed * RPM2RPS); - Motor_Arm.input = PID_Arm_Speed.output; + // Motor_Arm.input = PID_Arm_Speed.output; vTaskDelayUntil(&LastWakeTime, intervalms); // 发送频率 DebugData.debug1 = PID_Arm_Speed.target; diff --git a/user/gold-miner/tasks.h b/user/gold-miner/tasks.h index dc63063b..4c90a0db 100644 --- a/user/gold-miner/tasks.h +++ b/user/gold-miner/tasks.h @@ -3,6 +3,7 @@ void Task_Control(void *Parameters); void Task_Manipulator(void *Parameters); +void Task_Communication(void *Parameters); void Task_Blink(void *Parameters); void Task_Chassis(void *Parameters); void Task_Host(void *Parameters); diff --git a/user/protocol.h b/user/protocol.h deleted file mode 100644 index 8a8f6aee..00000000 --- a/user/protocol.h +++ /dev/null @@ -1,506 +0,0 @@ -#ifndef __PROTOCOL_H -#define __PROTOCOL_H -/** - * @file protocol.h - * @note 本文件由 Node_Bridge 生成, 请勿直接修改 - * - * @note Node command_id - * 裁判系统 0x0000 - 0x03FF - * 上位机 0x0400 - 0x04FF - * 板间通讯 0x0500 - 0x05FF - * 裁判系统 0xF100 - 0xF1FF - * 车间通讯 0xF200 - 0xF2FF - * @version judge.yml v1.0 - * @version host.yml v0.1 - * @version user.yml v0.1 - */ - -/********************************************************************** - * 结构体 - **********************************************************************/ - -typedef struct { - uint8_t graphic_name[3]; - uint32_t operate_tpye : 3; - uint32_t graphic_tpye : 3; - uint32_t layer : 4; - uint32_t color : 4; - uint32_t start_angle : 9; - uint32_t end_angle : 9; - uint32_t width : 10; - uint32_t start_x : 11; - uint32_t start_y : 11; - uint32_t radius : 10; - uint32_t end_x : 11; - uint32_t end_y : 11; -} GraphicData_Type; - -typedef struct { - int32_t debug1; - int32_t debug2; - int32_t debug3; - int32_t debug4; - int32_t debug5; - int32_t debug6; - int32_t debug7; - int32_t debug8; -} DebugData_Type; - -/********************************************************************** - * 协议列表 - **********************************************************************/ - -typedef union { - struct { - uint8_t game_type : 4; - uint8_t game_progress : 4; - uint16_t stage_remain_time; - uint64_t SyncTimeStamp; - }; - struct { - uint8_t data[11]; - }; -} game_status_t; - -typedef union { - struct { - uint8_t winner; - }; - struct { - uint8_t data[1]; - }; -} game_result_t; - -typedef union { - struct { - uint16_t red_1_robot_HP; - uint16_t red_2_robot_HP; - uint16_t red_3_robot_HP; - uint16_t red_4_robot_HP; - uint16_t red_5_robot_HP; - uint16_t red_7_robot_HP; - uint16_t red_outpost_HP; - uint16_t red_base_HP; - uint16_t blue_1_robot_HP; - uint16_t blue_2_robot_HP; - uint16_t blue_3_robot_HP; - uint16_t blue_4_robot_HP; - uint16_t blue_5_robot_HP; - uint16_t blue_7_robot_HP; - uint16_t blue_outpost_HP; - uint16_t blue_base_HP; - }; - struct { - uint8_t data[32]; - }; -} game_robot_HP_t; - -typedef union { - struct { - uint8_t dart_belong; - uint16_t stage_remaining_time; - }; - struct { - uint8_t data[3]; - }; -} dart_status_t; - -typedef union { - struct { - uint32_t event_type; - }; - struct { - uint8_t data[4]; - }; -} event_data_t; - -typedef union { - struct { - uint8_t supply_projectile_id; - uint8_t supply_robot_id; - uint8_t supply_projectile_step; - uint8_t supply_projectile_num; - }; - struct { - uint8_t data[4]; - }; -} supply_projectile_action_t; - -typedef union { - struct { - uint8_t level; - uint8_t foul_robot_id; - }; - struct { - uint8_t data[2]; - }; -} referee_warning_t; - -typedef union { - struct { - uint8_t dart_remaining_time; - }; - struct { - uint8_t data[1]; - }; -} dart_remaining_time_t; - -typedef union { - struct { - uint8_t robot_id; - uint8_t robot_level; - uint16_t remain_HP; - uint16_t max_HP; - uint16_t shooter_id1_17mm_cooling_rate; - uint16_t shooter_id1_17mm_cooling_limit; - uint16_t shooter_id1_17mm_speed_limit; - uint16_t shooter_id2_17mm_cooling_rate; - uint16_t shooter_id2_17mm_cooling_limit; - uint16_t shooter_id2_17mm_speed_limit; - uint16_t shooter_id1_42mm_cooling_rate; - uint16_t shooter_id1_42mm_cooling_limit; - uint16_t shooter_id1_42mm_speed_limit; - uint16_t chassis_power_limit; - uint8_t mains_power_gimbal_output : 1; - uint8_t mains_power_chassis_output : 1; - uint8_t mains_power_shooter_output : 1; - }; - struct { - uint8_t data[27]; - }; -} game_robot_status_t; - -typedef union { - struct { - uint16_t chassis_volt; - uint16_t chassis_current; - float chassis_power; - uint16_t chassis_power_buffer; - uint16_t shooter_id1_17mm_cooling_heat; - uint16_t shooter_id2_17mm_cooling_heat; - uint16_t shooter_id1_42mm_cooling_heat; - }; - struct { - uint8_t data[16]; - }; -} power_heat_data_t; - -typedef union { - struct { - float x; - float y; - float z; - float yaw; - }; - struct { - uint8_t data[16]; - }; -} game_robot_pos_t; - -typedef union { - struct { - uint8_t power_rune_buff; - }; - struct { - uint8_t data[1]; - }; -} buff_info_t; - -typedef union { - struct { - uint8_t attack_time; - }; - struct { - uint8_t data[1]; - }; -} aerial_robot_energy_t; - -typedef union { - struct { - uint8_t armor_id : 4; - uint8_t hurt_type : 4; - }; - struct { - uint8_t data[1]; - }; -} robot_hurt_t; - -typedef union { - struct { - uint8_t bullet_type; - uint8_t shooter_id; - uint8_t bullet_freq; - float bullet_speed; - }; - struct { - uint8_t data[7]; - }; -} shoot_data_t; - -typedef union { - struct { - uint16_t bullet_remaining_num_17mm; - uint16_t bullet_remaining_num_42mm; - uint16_t coin_remaining_num; - }; - struct { - uint8_t data[6]; - }; -} bullet_remaining_t; - -typedef union { - struct { - uint32_t rfid_status; - }; - struct { - uint8_t data[4]; - }; -} rfid_status_t; - -typedef union { - struct { - uint8_t dart_launch_opening_status; - uint8_t dart_attack_target; - uint16_t target_change_time; - uint8_t first_dart_speed; - uint8_t second_dart_speed; - uint8_t third_dart_speed; - uint8_t fourth_dart_speed; - uint16_t last_dart_launch_time; - uint16_t operate_launch_cmd_time; - }; - struct { - uint8_t data[12]; - }; -} dart_client_cmd_t; - -typedef union { - struct { - uint16_t data_cmd_id; - uint16_t send_id; - uint16_t receiver_id; - uint8_t operate_tpye; - uint8_t layer; - }; - struct { - uint8_t data[8]; - }; -} client_custom_graphic_delete_t; - -typedef union { - struct { - uint16_t data_cmd_id; - uint16_t send_id; - uint16_t receiver_id; - GraphicData_Type grapic_data_struct; - }; - struct { - uint8_t data[21]; - }; -} client_custom_graphic_single_t; - -typedef union { - struct { - uint16_t data_cmd_id; - uint16_t send_id; - uint16_t receiver_id; - GraphicData_Type grapic_data_struct[2]; - }; - struct { - uint8_t data[36]; - }; -} client_custom_graphic_double_t; - -typedef union { - struct { - uint16_t data_cmd_id; - uint16_t send_id; - uint16_t receiver_id; - GraphicData_Type grapic_data_struct[5]; - }; - struct { - uint8_t data[81]; - }; -} client_custom_graphic_five_t; - -typedef union { - struct { - uint16_t data_cmd_id; - uint16_t send_id; - uint16_t receiver_id; - GraphicData_Type grapic_data_struct; - uint8_t data[30]; - }; - struct { - uint8_t data[51]; - }; -} client_custom_character_t; - -typedef union { - struct { - uint16_t data_cmd_id; - uint16_t send_id; - uint16_t receiver_id; - GraphicData_Type grapic_data_struct[7]; - }; - struct { - uint8_t data[111]; - }; -} client_custom_graphic_seven_t; - -typedef union { - struct { - DebugData_Type debugData; - }; - struct { - uint8_t data[32]; - }; -} debug_info_t; - -typedef union { - struct { - uint16_t code; - char text[22]; - }; - struct { - uint8_t data[24]; - }; -} error_info_t; - -typedef union { - struct {}; - struct { - uint8_t data[0]; - }; -} heartbeat_t; - -typedef union { - struct { - float yaw_angle_diff; - float pitch_angle_diff; - uint8_t fire; - }; - struct { - uint8_t data[9]; - }; -} autoaim_data_t; - -typedef union { - struct { - float vx; - float vy; - float vw; - }; - struct { - uint8_t data[12]; - }; -} chassis_data_t; - -typedef union { - struct { - float yaw; - float pitch; - float roll; - }; - struct { - uint8_t data[12]; - }; -} gyroscope_data_t; - -typedef union { - struct { - uint8_t dbusBuffer[18]; - }; - struct { - uint8_t data[18]; - }; -} dbus_data_t; - -typedef union { - struct { - uint16_t data_cmd_id; - uint16_t send_id; - uint16_t receiver_id; - int32_t data1; - int32_t data2; - int32_t data3; - int32_t data4; - }; - struct { - uint8_t data[22]; - }; -} robot_interactive_data_t; - -typedef union { - struct { - int32_t data1; - int32_t data2; - int32_t data3; - int32_t data4; - }; - struct { - uint8_t data[16]; - }; -} board_interactive_data_t; - -/********************************************************************** - * 协议信息 - **********************************************************************/ - -#define PROTOCOL_INFO_LIST \ - { \ - {0X0001, 11, 1}, {0X0002, 1, 1}, {0X0003, 32, 1}, {0X0004, 3, 1}, {0X0101, 4, 1}, {0X0102, 4, 1}, {0X0104, 2, 1}, {0X0105, 1, 1}, {0X0201, 27, 1}, \ - {0X0202, 16, 1}, {0X0203, 16, 1}, {0X0204, 1, 1}, {0X0205, 1, 1}, {0X0206, 1, 1}, {0X0207, 7, 1}, {0X0208, 6, 1}, {0X0209, 4, 1}, {0X020A, 12, 1}, \ - {0XF100, 8, 0}, {0XF101, 21, 0}, {0XF102, 36, 0}, {0XF103, 81, 0}, {0XF110, 51, 0}, {0XF104, 111, 0}, {0X1024, 32, 0}, {0X6666, 24, 0}, \ - {0X0120, 0, 1}, {0X0401, 9, 1}, {0X0402, 12, 1}, {0X0403, 12, 0}, {0X0404, 18, 0}, {0XF301, 22, 1}, {0X0501, 16, 1}, { \ - 0X0502, 16, 1 \ - } \ - } - -/********************************************************************** - * 协议接口 - **********************************************************************/ - -typedef union { - struct { - game_status_t gameStatus; // 0X0001 比赛状态数据 - game_result_t gameResult; // 0X0002 比赛结果的数据 - game_robot_HP_t healthPoint; // 0X0003 机器人血量数据 - dart_status_t dartStatus; // 0X0004 飞镖发射状态 - event_data_t eventData; // 0X0101 场地事件数据 - supply_projectile_action_t supplyProjectileaction; // 0X0102 补给站动作标识 - referee_warning_t refereeWarning; // 0X0104 裁判警告信息 - dart_remaining_time_t dartRemainingtime; // 0X0105 飞镖发射口倒计 - game_robot_status_t gameRobotstatus; // 0X0201 比赛机器人状态 - power_heat_data_t powerHeatData; // 0X0202 实时功率热量数据 - game_robot_pos_t gameRobotpos; // 0X0203 机器人位置 - buff_info_t buffInfo; // 0X0204 机器人增益 - aerial_robot_energy_t aerialRobotenergy; // 0X0205 空中机器人能量状态 - robot_hurt_t robotHurt; // 0X0206 伤害状态 - shoot_data_t shootData; // 0X0207 实时射击信息 - bullet_remaining_t bulletRemaining; // 0X0208 子弹剩余发射数 - rfid_status_t rfidStatus; // 0X0209 机器人RFID状态 - dart_client_cmd_t dartClientCmds; // 0X020A 飞镖机器人客户端指令数据 - client_custom_graphic_delete_t clientCustomGraphicDelete; // 0XF100 客户端删除图形 - client_custom_graphic_single_t client_custom_graphicSingle; // 0XF101 客户端绘制一个图形 - client_custom_graphic_double_t clientCustomGraphicDouble; // 0XF102 客户端绘制两个图形 - client_custom_graphic_five_t clientCustomGraphicFive; // 0XF103 客户端绘制五个图形 - client_custom_character_t clientCustomCharacter; // 0XF110 客户端绘制字符 - client_custom_graphic_seven_t clientCustomGraphicSeven; // 0XF104 客户端绘制七个图形 - debug_info_t debugInfo; // 0X1024 调试信息 - error_info_t errorInfo; // 0X6666 报错信息 - heartbeat_t heartbeat; // 0X0120 心跳包 - autoaim_data_t autoaimData; // 0X0401 云台控制 - chassis_data_t chassisData; // 0X0402 底盘控制 - gyroscope_data_t gyroscopeData; // 0X0403 陀螺仪数据 - dbus_data_t dbusData; // 0X0404 遥控器及键鼠数据 - robot_interactive_data_t robotCommunication; // 0XF301 学生机器人间通信 - board_interactive_data_t boardAlpha; // 0X0501 主控板间通信 - board_interactive_data_t boardBeta; // 0X0502 主控板间通信 - }; - struct { - uint8_t data[618]; - }; -} ProtocolData_Type; - -#endif \ No newline at end of file From 5e48ba236f8d29fabfacc2b916ff07100d884ab1 Mon Sep 17 00:00:00 2001 From: Link Wu <1774241372@qq.com> Date: Thu, 13 May 2021 21:15:56 +0800 Subject: [PATCH 05/10] new branch for gold miner --- user/gold-miner/protocol.h | 29 +++++------------------------ user/gold-miner/tasks.c | 28 +++++++++++----------------- user/gold-miner/tasks.h | 1 - 3 files changed, 16 insertions(+), 42 deletions(-) diff --git a/user/gold-miner/protocol.h b/user/gold-miner/protocol.h index 74258e38..8a8f6aee 100644 --- a/user/gold-miner/protocol.h +++ b/user/gold-miner/protocol.h @@ -11,8 +11,8 @@ * 裁判系统 0xF100 - 0xF1FF * 车间通讯 0xF200 - 0xF2FF * @version judge.yml v1.0 - * @version host.yml v1.0 - * @version user.yml v1.0 + * @version host.yml v0.1 + * @version user.yml v0.1 */ /********************************************************************** @@ -416,24 +416,6 @@ typedef union { }; } dbus_data_t; -typedef union { - struct { - int16_t base_joint_position; - int16_t base_joint_speed; - int16_t shoulder_joint_position; - int16_t shoulder_joint_speed; - int16_t elbow_joint_position; - int16_t elbow_joint_speed; - int16_t wrist_joint_1_position; - int16_t wrist_joint_1_speed; - int16_t wrist_joint_2_position; - int16_t wrist_joint_2_speed; - }; - struct { - uint8_t data[20]; - }; -} joint_info_t; - typedef union { struct { uint16_t data_cmd_id; @@ -470,7 +452,7 @@ typedef union { {0X0001, 11, 1}, {0X0002, 1, 1}, {0X0003, 32, 1}, {0X0004, 3, 1}, {0X0101, 4, 1}, {0X0102, 4, 1}, {0X0104, 2, 1}, {0X0105, 1, 1}, {0X0201, 27, 1}, \ {0X0202, 16, 1}, {0X0203, 16, 1}, {0X0204, 1, 1}, {0X0205, 1, 1}, {0X0206, 1, 1}, {0X0207, 7, 1}, {0X0208, 6, 1}, {0X0209, 4, 1}, {0X020A, 12, 1}, \ {0XF100, 8, 0}, {0XF101, 21, 0}, {0XF102, 36, 0}, {0XF103, 81, 0}, {0XF110, 51, 0}, {0XF104, 111, 0}, {0X1024, 32, 0}, {0X6666, 24, 0}, \ - {0X0120, 0, 1}, {0X0401, 9, 1}, {0X0402, 12, 1}, {0X0403, 12, 0}, {0X0404, 18, 0}, {0X0405, 20, 0}, {0XF301, 22, 1}, {0X0501, 16, 1}, { \ + {0X0120, 0, 1}, {0X0401, 9, 1}, {0X0402, 12, 1}, {0X0403, 12, 0}, {0X0404, 18, 0}, {0XF301, 22, 1}, {0X0501, 16, 1}, { \ 0X0502, 16, 1 \ } \ } @@ -511,14 +493,13 @@ typedef union { autoaim_data_t autoaimData; // 0X0401 云台控制 chassis_data_t chassisData; // 0X0402 底盘控制 gyroscope_data_t gyroscopeData; // 0X0403 陀螺仪数据 - dbus_data_t dbugData; // 0X0404 遥控器及键鼠数据 - joint_info_t jointInfo; // 0X0405 工程机械臂数据 + dbus_data_t dbusData; // 0X0404 遥控器及键鼠数据 robot_interactive_data_t robotCommunication; // 0XF301 学生机器人间通信 board_interactive_data_t boardAlpha; // 0X0501 主控板间通信 board_interactive_data_t boardBeta; // 0X0502 主控板间通信 }; struct { - uint8_t data[638]; + uint8_t data[618]; }; } ProtocolData_Type; diff --git a/user/gold-miner/tasks.c b/user/gold-miner/tasks.c index 2de18a2e..c63b9d18 100644 --- a/user/gold-miner/tasks.c +++ b/user/gold-miner/tasks.c @@ -97,25 +97,17 @@ void Task_Chassis(void *Parameters) { void Task_Communication(void *Parameters) { TickType_t LastWakeTime = xTaskGetTickCount(); // 时钟 - float interval = 0.008; // 任务运行间隔 s + float interval = 0.001; // 任务运行间隔 s int intervalms = interval * 1000; // 任务运行间隔 ms ProtocolInfo_Type *protocolInfo = Protocol_Get_Info_Handle(0x501); extern DMA_Type DMA_Table[10]; - - int8_t i = 0; while (1) { - i++; + // 修改数据 - ProtocolData.jointInfo.base_joint_position = i; - ProtocolData.jointInfo.base_joint_speed = i + 1; - ProtocolData.jointInfo.shoulder_joint_position = i + 2; - ProtocolData.jointInfo.shoulder_joint_speed = i + 3; - ProtocolData.jointInfo.elbow_joint_position = i + 4; - ProtocolData.jointInfo.elbow_joint_speed = i + 5; - ProtocolData.jointInfo.wrist_joint_1_position = i + 6; - ProtocolData.jointInfo.wrist_joint_1_speed = i + 7; - ProtocolData.jointInfo.wrist_joint_2_position = i + 8; - ProtocolData.jointInfo.wrist_joint_2_speed = i + 9; + ProtocolData.boardAlpha.data1 = 1; + ProtocolData.boardAlpha.data2 = 2; + ProtocolData.boardAlpha.data3 = 3; + ProtocolData.boardAlpha.data4 = 4; // 建议在main.c中使用Bridge_Send_Protocol // Bridge_Send_Protocol_Once(&Node_Host, 0x501); @@ -138,18 +130,20 @@ void Task_Manipulator(void *Parameters) { float interval = 0.01; // 任务运行间隔 s int intervalms = interval * 1000; // 任务运行间隔 ms - PID_Init(&PID_Arm_Speed, 40, 3, 0, 16384, 16384); + PID_Init(&PID_Arm_Speed, 40, 0, 0, 16384, 16384); PID_Init(&PID_Arm_Angle, 0, 0, 0, 16384, 16384); int armAngle = 0; while (1) { - // PID_Calculate(&PID_Arm_Speed, (remoteData.ly > 0 ? 1 : -1) * (ABS(remoteData.ly) > 330 ? 200 : 0), Motor_Arm.speed * RPM2RPS); + + PID_Arm_Speed.i = CHOOSEL(1, 3, 10); + PID_Calculate(&PID_Arm_Speed, (remoteData.ly > 0 ? 1 : -1) * (ABS(remoteData.ly) > 330 ? 200 : 0), Motor_Arm.speed * RPM2RPS); // armAngle += remoteData.ly / 660.0f * 50; // PID_Calculate(&PID_Arm_Angle, armAngle, Motor_Arm.angle * RPM2RPS); // PID_Calculate(&PID_Arm_Speed, PID_Arm_Angle.output, Motor_Arm.speed * RPM2RPS); - // Motor_Arm.input = PID_Arm_Speed.output; + Motor_Arm.input = PID_Arm_Speed.output; vTaskDelayUntil(&LastWakeTime, intervalms); // 发送频率 DebugData.debug1 = PID_Arm_Speed.target; diff --git a/user/gold-miner/tasks.h b/user/gold-miner/tasks.h index 4c90a0db..dc63063b 100644 --- a/user/gold-miner/tasks.h +++ b/user/gold-miner/tasks.h @@ -3,7 +3,6 @@ void Task_Control(void *Parameters); void Task_Manipulator(void *Parameters); -void Task_Communication(void *Parameters); void Task_Blink(void *Parameters); void Task_Chassis(void *Parameters); void Task_Host(void *Parameters); From 715c1e2ad95cdec22b7bcb4254f279737eb3698d Mon Sep 17 00:00:00 2001 From: odin1777 <947287884@qq.com> Date: Thu, 27 May 2021 00:30:21 +0800 Subject: [PATCH 06/10] =?UTF-8?q?=E8=B0=83=E8=AF=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 对机械臂的调试 --- user/gold-miner/handle.h | 7 +- user/gold-miner/main.c | 40 +-- user/gold-miner/protocol.h | 526 +++++++++++++++++++++++-------------- user/gold-miner/tasks.c | 87 ++++-- 4 files changed, 414 insertions(+), 246 deletions(-) diff --git a/user/gold-miner/handle.h b/user/gold-miner/handle.h index f58c6add..b571dbef 100644 --- a/user/gold-miner/handle.h +++ b/user/gold-miner/handle.h @@ -45,9 +45,10 @@ __HANDLE_EXT volatile uint32_t ulHighFrequencyTimerTicks; __HANDLE_EXT Motor_Type Motor_LF, Motor_RF, Motor_RB, Motor_LB; // 机械臂 -__HANDLE_EXT Motor_Type Motor_Base_Joint, Motor_Shoulder_Joint, Motor_Elbow_Joint, Motor_Wrist_Joint_1, Motor_Wrist_Joint_2; -__HANDLE_EXT PID_Type PID_Arm_Speed, PID_Arm_Angle; - +__HANDLE_EXT Motor_Type Motor_BaseJoint, Motor_ShoulderJoint, Motor_ElbowJoint, Motor_WristJoint1, Motor_WristJoint2; +__HANDLE_EXT PID_Type PID_BaseJointSpeed, PID_BaseJointAngle,PID_ShoulderJointSpeed, PID_ShoulderJointAngle, + PID_ElbowSpeed, PID_ElbowAngle,PID_WristJoint1Speed, PID_WristJoint1Angle, PID_WristJoint2Speed, PID_WristJoint2Angle; + // 遥控器 __HANDLE_EXT uint8_t remoteBuffer[DBUS_LENGTH + DBUS_BACK_LENGTH]; __HANDLE_EXT Remote_Type remoteData; diff --git a/user/gold-miner/main.c b/user/gold-miner/main.c index 8f7c93b0..df0812d4 100644 --- a/user/gold-miner/main.c +++ b/user/gold-miner/main.c @@ -32,11 +32,11 @@ int main(void) { Motor_Init(&Motor_RF, CHASSIS_MOTOR_REDUCTION_RATE, DISABLE, DISABLE); // 机械臂电机 - Motor_Init(&Motor_Base_Joint, MOTOR_REDUCTION_RATE_6020, ENABLE, ENABLE); - Motor_Init(&Motor_Shoulder_Joint, MOTOR_REDUCTION_RATE_3508 * 50, ENABLE, ENABLE); - Motor_Init(&Motor_Elbow_Joint, MOTOR_REDUCTION_RATE_3508 * 50, ENABLE, ENABLE); - Motor_Init(&Motor_Wrist_Joint_1, MOTOR_REDUCTION_RATE_6020, ENABLE, ENABLE); - Motor_Init(&Motor_Wrist_Joint_2, MOTOR_REDUCTION_RATE_2006, ENABLE, ENABLE); + Motor_Init(&Motor_BaseJoint, MOTOR_REDUCTION_RATE_6020, ENABLE, ENABLE); + Motor_Init(&Motor_ShoulderJoint, MOTOR_REDUCTION_RATE_3508 * 50, ENABLE, ENABLE); + Motor_Init(&Motor_ElbowJoint, MOTOR_REDUCTION_RATE_3508 * 50, ENABLE, ENABLE); + Motor_Init(&Motor_WristJoint1, MOTOR_REDUCTION_RATE_6020, ENABLE, ENABLE); + Motor_Init(&Motor_WristJoint2, MOTOR_REDUCTION_RATE_3508 * 30, ENABLE, ENABLE); // 遥控器数据初始化 DBUS_Init(&remoteData, &keyboardData, &mouseData); @@ -71,15 +71,15 @@ int main(void) { BSP_ADC1_Init(1, ADC_Channel6, 0); // 总线设置 - Bridge_Bind(&BridgeData, CAN1_BRIDGE, 0x201, &Motor_LF); - Bridge_Bind(&BridgeData, CAN1_BRIDGE, 0x202, &Motor_LB); - Bridge_Bind(&BridgeData, CAN1_BRIDGE, 0x203, &Motor_RB); - Bridge_Bind(&BridgeData, CAN1_BRIDGE, 0x204, &Motor_RF); - Bridge_Bind(&BridgeData, CAN2_BRIDGE, 0x205, &Motor_Base_Joint); - Bridge_Bind(&BridgeData, CAN2_BRIDGE, 0x201, &Motor_Shoulder_Joint); - Bridge_Bind(&BridgeData, CAN2_BRIDGE, 0x202, &Motor_Elbow_Joint); - Bridge_Bind(&BridgeData, CAN2_BRIDGE, 0x206, &Motor_Wrist_Joint_1); - Bridge_Bind(&BridgeData, CAN2_BRIDGE, 0x203, &Motor_Wrist_Joint_2); + // Bridge_Bind(&BridgeData, CAN1_BRIDGE, 0x201, &Motor_LF); + // Bridge_Bind(&BridgeData, CAN1_BRIDGE, 0x202, &Motor_LB); + // Bridge_Bind(&BridgeData, CAN1_BRIDGE, 0x203, &Motor_RB); + // Bridge_Bind(&BridgeData, CAN1_BRIDGE, 0x204, &Motor_RF); + // Bridge_Bind(&BridgeData, CAN2_BRIDGE, 0x205, &Motor_BaseJoint); + // Bridge_Bind(&BridgeData, CAN2_BRIDGE, 0x201, &Motor_ShoulderJoint); + // Bridge_Bind(&BridgeData, CAN2_BRIDGE, 0x202, &Motor_ElbowJoint); + Bridge_Bind(&BridgeData, CAN2_BRIDGE, 0x206, &Motor_WristJoint1); + Bridge_Bind(&BridgeData, CAN2_BRIDGE, 0x203, &Motor_WristJoint2); Bridge_Bind(&BridgeData, USART_BRIDGE, 7, &Node_Host); Bridge_Bind(&BridgeData, USART_BRIDGE, 8, &Node_Judge); @@ -97,19 +97,19 @@ int main(void) { // xTaskCreate(Task_Startup_Music, "Task_Startup_Music", 400, NULL, 3, NULL);// 开机音乐任务 // 等待遥控器开启 - // while (!remoteData.state) { - // } + while (!remoteData.state) { + } // 高优先级任务 xTaskCreate(Task_Control, "Task_Control", 400, NULL, 9, NULL); //模式切换任务 // xTaskCreate(Task_Chassis, "Task_Chassis", 400, NULL, 5, NULL); // 底盘运动任务 - // xTaskCreate(Task_Manipulator, "Task_Manipulator", 400, NULL, 5, NULL); // 机械臂运动任务 + xTaskCreate(Task_Manipulator, "Task_Manipulator", 400, NULL, 5, NULL); // 机械臂运动任务 xTaskCreate(Task_Communication, "Task_Communication", 500, NULL, 6, NULL); // 通讯测试任务 - // xTaskCreate(Task_Can_Send, "Task_Can_Send", 500, NULL, 6, NULL); // Can发送任务 + xTaskCreate(Task_Can_Send, "Task_Can_Send", 500, NULL, 6, NULL); // Can发送任务 // 定义协议发送频率 - Bridge_Send_Protocol(&Node_Host, 0x120, 1); // 心跳包 - Bridge_Send_Protocol(&Node_Host, 0x405, 250); // 机械臂数据 + Bridge_Send_Protocol(&Node_Host, 0x120, 1); // 心跳包 + Bridge_Send_Protocol(&Node_Host, 0x405, 50); // 机械臂数据 //启动调度,开始执行任务 vTaskStartScheduler(); diff --git a/user/gold-miner/protocol.h b/user/gold-miner/protocol.h index 8a8f6aee..88fad61a 100644 --- a/user/gold-miner/protocol.h +++ b/user/gold-miner/protocol.h @@ -11,15 +11,16 @@ * 裁判系统 0xF100 - 0xF1FF * 车间通讯 0xF200 - 0xF2FF * @version judge.yml v1.0 - * @version host.yml v0.1 - * @version user.yml v0.1 + * @version host.yml v1.2 + * @version user.yml v1.0 */ /********************************************************************** * 结构体 **********************************************************************/ -typedef struct { +typedef struct +{ uint8_t graphic_name[3]; uint32_t operate_tpye : 3; uint32_t graphic_tpye : 3; @@ -35,7 +36,8 @@ typedef struct { uint32_t end_y : 11; } GraphicData_Type; -typedef struct { +typedef struct +{ int32_t debug1; int32_t debug2; int32_t debug3; @@ -50,29 +52,37 @@ typedef struct { * 协议列表 **********************************************************************/ -typedef union { - struct { - uint8_t game_type : 4; - uint8_t game_progress : 4; +typedef union +{ + struct + { + uint8_t game_type : 4; + uint8_t game_progress : 4; uint16_t stage_remain_time; uint64_t SyncTimeStamp; }; - struct { + struct + { uint8_t data[11]; }; } game_status_t; -typedef union { - struct { +typedef union +{ + struct + { uint8_t winner; }; - struct { + struct + { uint8_t data[1]; }; } game_result_t; -typedef union { - struct { +typedef union +{ + struct + { uint16_t red_1_robot_HP; uint16_t red_2_robot_HP; uint16_t red_3_robot_HP; @@ -90,67 +100,83 @@ typedef union { uint16_t blue_outpost_HP; uint16_t blue_base_HP; }; - struct { + struct + { uint8_t data[32]; }; } game_robot_HP_t; -typedef union { - struct { - uint8_t dart_belong; +typedef union +{ + struct + { + uint8_t dart_belong; uint16_t stage_remaining_time; }; - struct { + struct + { uint8_t data[3]; }; } dart_status_t; -typedef union { - struct { +typedef union +{ + struct + { uint32_t event_type; }; - struct { + struct + { uint8_t data[4]; }; } event_data_t; -typedef union { - struct { +typedef union +{ + struct + { uint8_t supply_projectile_id; uint8_t supply_robot_id; uint8_t supply_projectile_step; uint8_t supply_projectile_num; }; - struct { + struct + { uint8_t data[4]; }; } supply_projectile_action_t; -typedef union { - struct { +typedef union +{ + struct + { uint8_t level; uint8_t foul_robot_id; }; - struct { + struct + { uint8_t data[2]; }; } referee_warning_t; -typedef union { - struct { +typedef union +{ + struct + { uint8_t dart_remaining_time; }; - struct { + struct + { uint8_t data[1]; }; } dart_remaining_time_t; -typedef union { - struct { - uint8_t robot_id; - uint8_t robot_level; - uint16_t remain_HP; - uint16_t max_HP; +typedef union +{ + struct + { + uint8_t robot_id; uint8_t robot_level; + uint16_t remain_HP; uint16_t max_HP; uint16_t shooter_id1_17mm_cooling_rate; uint16_t shooter_id1_17mm_cooling_limit; uint16_t shooter_id1_17mm_speed_limit; @@ -161,263 +187,356 @@ typedef union { uint16_t shooter_id1_42mm_cooling_limit; uint16_t shooter_id1_42mm_speed_limit; uint16_t chassis_power_limit; - uint8_t mains_power_gimbal_output : 1; - uint8_t mains_power_chassis_output : 1; - uint8_t mains_power_shooter_output : 1; + uint8_t mains_power_gimbal_output : 1; + uint8_t mains_power_chassis_output : 1; + uint8_t mains_power_shooter_output : 1; }; - struct { + struct + { uint8_t data[27]; }; } game_robot_status_t; -typedef union { - struct { +typedef union +{ + struct + { uint16_t chassis_volt; uint16_t chassis_current; - float chassis_power; + float chassis_power; uint16_t chassis_power_buffer; uint16_t shooter_id1_17mm_cooling_heat; uint16_t shooter_id2_17mm_cooling_heat; uint16_t shooter_id1_42mm_cooling_heat; }; - struct { + struct + { uint8_t data[16]; }; } power_heat_data_t; -typedef union { - struct { +typedef union +{ + struct + { float x; float y; float z; float yaw; }; - struct { + struct + { uint8_t data[16]; }; } game_robot_pos_t; -typedef union { - struct { +typedef union +{ + struct + { uint8_t power_rune_buff; }; - struct { + struct + { uint8_t data[1]; }; } buff_info_t; -typedef union { - struct { +typedef union +{ + struct + { uint8_t attack_time; }; - struct { + struct + { uint8_t data[1]; }; } aerial_robot_energy_t; -typedef union { - struct { +typedef union +{ + struct + { uint8_t armor_id : 4; uint8_t hurt_type : 4; }; - struct { + struct + { uint8_t data[1]; }; } robot_hurt_t; -typedef union { - struct { +typedef union +{ + struct + { uint8_t bullet_type; uint8_t shooter_id; uint8_t bullet_freq; - float bullet_speed; + float bullet_speed; }; - struct { + struct + { uint8_t data[7]; }; } shoot_data_t; -typedef union { - struct { +typedef union +{ + struct + { uint16_t bullet_remaining_num_17mm; uint16_t bullet_remaining_num_42mm; uint16_t coin_remaining_num; }; - struct { + struct + { uint8_t data[6]; }; } bullet_remaining_t; -typedef union { - struct { +typedef union +{ + struct + { uint32_t rfid_status; }; - struct { + struct + { uint8_t data[4]; }; } rfid_status_t; -typedef union { - struct { - uint8_t dart_launch_opening_status; - uint8_t dart_attack_target; +typedef union +{ + struct + { + uint8_t dart_launch_opening_status; + uint8_t dart_attack_target; uint16_t target_change_time; - uint8_t first_dart_speed; - uint8_t second_dart_speed; - uint8_t third_dart_speed; - uint8_t fourth_dart_speed; + uint8_t first_dart_speed; + uint8_t second_dart_speed; + uint8_t third_dart_speed; + uint8_t fourth_dart_speed; uint16_t last_dart_launch_time; uint16_t operate_launch_cmd_time; }; - struct { + struct + { uint8_t data[12]; }; } dart_client_cmd_t; -typedef union { - struct { - uint16_t data_cmd_id; - uint16_t send_id; - uint16_t receiver_id; - uint8_t operate_tpye; - uint8_t layer; - }; - struct { +typedef union +{ + struct + { + uint16_t data_cmd_id; + uint16_t send_id; + uint16_t receiver_id; + uint8_t operate_tpye; + uint8_t layer; + }; + struct + { uint8_t data[8]; }; } client_custom_graphic_delete_t; -typedef union { - struct { - uint16_t data_cmd_id; - uint16_t send_id; - uint16_t receiver_id; - GraphicData_Type grapic_data_struct; - }; - struct { +typedef union +{ + struct + { + uint16_t data_cmd_id; + uint16_t send_id; + uint16_t receiver_id; + GraphicData_Type grapic_data_struct; + }; + struct + { uint8_t data[21]; }; } client_custom_graphic_single_t; -typedef union { - struct { - uint16_t data_cmd_id; - uint16_t send_id; - uint16_t receiver_id; - GraphicData_Type grapic_data_struct[2]; - }; - struct { +typedef union +{ + struct + { + uint16_t data_cmd_id; + uint16_t send_id; + uint16_t receiver_id; + GraphicData_Type grapic_data_struct[2]; + }; + struct + { uint8_t data[36]; }; } client_custom_graphic_double_t; -typedef union { - struct { - uint16_t data_cmd_id; - uint16_t send_id; - uint16_t receiver_id; - GraphicData_Type grapic_data_struct[5]; - }; - struct { +typedef union +{ + struct + { + uint16_t data_cmd_id; + uint16_t send_id; + uint16_t receiver_id; + GraphicData_Type grapic_data_struct[5]; + }; + struct + { uint8_t data[81]; }; } client_custom_graphic_five_t; -typedef union { - struct { - uint16_t data_cmd_id; - uint16_t send_id; - uint16_t receiver_id; - GraphicData_Type grapic_data_struct; - uint8_t data[30]; - }; - struct { +typedef union +{ + struct + { + uint16_t data_cmd_id; + uint16_t send_id; + uint16_t receiver_id; + GraphicData_Type grapic_data_struct; + uint8_t data[30]; + }; + struct + { uint8_t data[51]; }; } client_custom_character_t; -typedef union { - struct { - uint16_t data_cmd_id; - uint16_t send_id; - uint16_t receiver_id; - GraphicData_Type grapic_data_struct[7]; - }; - struct { +typedef union +{ + struct + { + uint16_t data_cmd_id; + uint16_t send_id; + uint16_t receiver_id; + GraphicData_Type grapic_data_struct[7]; + }; + struct + { uint8_t data[111]; }; } client_custom_graphic_seven_t; -typedef union { - struct { +typedef union +{ + struct + { DebugData_Type debugData; }; - struct { + struct + { uint8_t data[32]; }; } debug_info_t; -typedef union { - struct { +typedef union +{ + struct + { uint16_t code; - char text[22]; + char text[22]; }; - struct { + struct + { uint8_t data[24]; }; } error_info_t; -typedef union { - struct {}; - struct { +typedef union +{ + struct + { + + }; + struct + { uint8_t data[0]; }; } heartbeat_t; -typedef union { - struct { - float yaw_angle_diff; - float pitch_angle_diff; +typedef union +{ + struct + { + float yaw_angle_diff; + float pitch_angle_diff; uint8_t fire; }; - struct { + struct + { uint8_t data[9]; }; } autoaim_data_t; -typedef union { - struct { +typedef union +{ + struct + { float vx; float vy; float vw; }; - struct { + struct + { uint8_t data[12]; }; } chassis_data_t; -typedef union { - struct { +typedef union +{ + struct + { float yaw; float pitch; float roll; }; - struct { + struct + { uint8_t data[12]; }; } gyroscope_data_t; -typedef union { - struct { +typedef union +{ + struct + { uint8_t dbusBuffer[18]; }; - struct { + struct + { uint8_t data[18]; }; } dbus_data_t; -typedef union { - struct { +typedef union +{ + struct + { + uint8_t state_code; + int16_t base_joint_position; + int16_t base_joint_speed; + int16_t shoulder_joint_position; + int16_t shoulder_joint_speed; + int16_t elbow_joint_position; + int16_t elbow_joint_speed; + int16_t wrist_joint_1_position; + int16_t wrist_joint_1_speed; + int16_t wrist_joint_2_position; + int16_t wrist_joint_2_speed; + }; + struct + { + uint8_t data[21]; + }; +} joint_info_t; + +typedef union +{ + struct + { uint16_t data_cmd_id; uint16_t send_id; uint16_t receiver_id; @@ -426,19 +545,23 @@ typedef union { int32_t data3; int32_t data4; }; - struct { + struct + { uint8_t data[22]; }; } robot_interactive_data_t; -typedef union { - struct { +typedef union +{ + struct + { int32_t data1; int32_t data2; int32_t data3; int32_t data4; }; - struct { + struct + { uint8_t data[16]; }; } board_interactive_data_t; @@ -447,59 +570,56 @@ typedef union { * 协议信息 **********************************************************************/ -#define PROTOCOL_INFO_LIST \ - { \ - {0X0001, 11, 1}, {0X0002, 1, 1}, {0X0003, 32, 1}, {0X0004, 3, 1}, {0X0101, 4, 1}, {0X0102, 4, 1}, {0X0104, 2, 1}, {0X0105, 1, 1}, {0X0201, 27, 1}, \ - {0X0202, 16, 1}, {0X0203, 16, 1}, {0X0204, 1, 1}, {0X0205, 1, 1}, {0X0206, 1, 1}, {0X0207, 7, 1}, {0X0208, 6, 1}, {0X0209, 4, 1}, {0X020A, 12, 1}, \ - {0XF100, 8, 0}, {0XF101, 21, 0}, {0XF102, 36, 0}, {0XF103, 81, 0}, {0XF110, 51, 0}, {0XF104, 111, 0}, {0X1024, 32, 0}, {0X6666, 24, 0}, \ - {0X0120, 0, 1}, {0X0401, 9, 1}, {0X0402, 12, 1}, {0X0403, 12, 0}, {0X0404, 18, 0}, {0XF301, 22, 1}, {0X0501, 16, 1}, { \ - 0X0502, 16, 1 \ - } \ - } +#define PROTOCOL_INFO_LIST {{0X0001,11,1},{0X0002,1,1},{0X0003,32,1},{0X0004,3,1},{0X0101,4,1},{0X0102,4,1},{0X0104,2,1},{0X0105,1,1},{0X0201,27,1},{0X0202,16,1},{0X0203,16,1},{0X0204,1,1},{0X0205,1,1},{0X0206,1,1},{0X0207,7,1},{0X0208,6,1},{0X0209,4,1},{0X020A,12,1},{0XF100,8,0},{0XF101,21,0},{0XF102,36,0},{0XF103,81,0},{0XF110,51,0},{0XF104,111,0},{0X1024,32,0},{0X6666,24,0},{0X0120,0,1},{0X0401,9,1},{0X0402,12,1},{0X0403,12,0},{0X0404,18,0},{0X0405,21,1},{0X0406,21,1},{0XF301,22,1},{0X0501,16,1},{0X0502,16,1}} /********************************************************************** * 协议接口 **********************************************************************/ -typedef union { - struct { - game_status_t gameStatus; // 0X0001 比赛状态数据 - game_result_t gameResult; // 0X0002 比赛结果的数据 - game_robot_HP_t healthPoint; // 0X0003 机器人血量数据 - dart_status_t dartStatus; // 0X0004 飞镖发射状态 - event_data_t eventData; // 0X0101 场地事件数据 - supply_projectile_action_t supplyProjectileaction; // 0X0102 补给站动作标识 - referee_warning_t refereeWarning; // 0X0104 裁判警告信息 - dart_remaining_time_t dartRemainingtime; // 0X0105 飞镖发射口倒计 - game_robot_status_t gameRobotstatus; // 0X0201 比赛机器人状态 - power_heat_data_t powerHeatData; // 0X0202 实时功率热量数据 - game_robot_pos_t gameRobotpos; // 0X0203 机器人位置 - buff_info_t buffInfo; // 0X0204 机器人增益 - aerial_robot_energy_t aerialRobotenergy; // 0X0205 空中机器人能量状态 - robot_hurt_t robotHurt; // 0X0206 伤害状态 - shoot_data_t shootData; // 0X0207 实时射击信息 - bullet_remaining_t bulletRemaining; // 0X0208 子弹剩余发射数 - rfid_status_t rfidStatus; // 0X0209 机器人RFID状态 - dart_client_cmd_t dartClientCmds; // 0X020A 飞镖机器人客户端指令数据 - client_custom_graphic_delete_t clientCustomGraphicDelete; // 0XF100 客户端删除图形 +typedef union +{ + struct + { + game_status_t gameStatus; // 0X0001 比赛状态数据 + game_result_t gameResult; // 0X0002 比赛结果的数据 + game_robot_HP_t healthPoint; // 0X0003 机器人血量数据 + dart_status_t dartStatus; // 0X0004 飞镖发射状态 + event_data_t eventData; // 0X0101 场地事件数据 + supply_projectile_action_t supplyProjectileaction; // 0X0102 补给站动作标识 + referee_warning_t refereeWarning; // 0X0104 裁判警告信息 + dart_remaining_time_t dartRemainingtime; // 0X0105 飞镖发射口倒计 + game_robot_status_t gameRobotstatus; // 0X0201 比赛机器人状态 + power_heat_data_t powerHeatData; // 0X0202 实时功率热量数据 + game_robot_pos_t gameRobotpos; // 0X0203 机器人位置 + buff_info_t buffInfo; // 0X0204 机器人增益 + aerial_robot_energy_t aerialRobotenergy; // 0X0205 空中机器人能量状态 + robot_hurt_t robotHurt; // 0X0206 伤害状态 + shoot_data_t shootData; // 0X0207 实时射击信息 + bullet_remaining_t bulletRemaining; // 0X0208 子弹剩余发射数 + rfid_status_t rfidStatus; // 0X0209 机器人RFID状态 + dart_client_cmd_t dartClientCmds; // 0X020A 飞镖机器人客户端指令数据 + client_custom_graphic_delete_t clientCustomGraphicDelete; // 0XF100 客户端删除图形 client_custom_graphic_single_t client_custom_graphicSingle; // 0XF101 客户端绘制一个图形 - client_custom_graphic_double_t clientCustomGraphicDouble; // 0XF102 客户端绘制两个图形 - client_custom_graphic_five_t clientCustomGraphicFive; // 0XF103 客户端绘制五个图形 - client_custom_character_t clientCustomCharacter; // 0XF110 客户端绘制字符 - client_custom_graphic_seven_t clientCustomGraphicSeven; // 0XF104 客户端绘制七个图形 - debug_info_t debugInfo; // 0X1024 调试信息 - error_info_t errorInfo; // 0X6666 报错信息 - heartbeat_t heartbeat; // 0X0120 心跳包 - autoaim_data_t autoaimData; // 0X0401 云台控制 - chassis_data_t chassisData; // 0X0402 底盘控制 - gyroscope_data_t gyroscopeData; // 0X0403 陀螺仪数据 - dbus_data_t dbusData; // 0X0404 遥控器及键鼠数据 - robot_interactive_data_t robotCommunication; // 0XF301 学生机器人间通信 - board_interactive_data_t boardAlpha; // 0X0501 主控板间通信 - board_interactive_data_t boardBeta; // 0X0502 主控板间通信 - }; - struct { - uint8_t data[618]; + client_custom_graphic_double_t clientCustomGraphicDouble; // 0XF102 客户端绘制两个图形 + client_custom_graphic_five_t clientCustomGraphicFive; // 0XF103 客户端绘制五个图形 + client_custom_character_t clientCustomCharacter; // 0XF110 客户端绘制字符 + client_custom_graphic_seven_t clientCustomGraphicSeven; // 0XF104 客户端绘制七个图形 + debug_info_t debugInfo; // 0X1024 调试信息 + error_info_t errorInfo; // 0X6666 报错信息 + heartbeat_t heartbeat; // 0X0120 心跳包 + autoaim_data_t autoaimData; // 0X0401 云台控制 + chassis_data_t chassisData; // 0X0402 底盘控制 + gyroscope_data_t gyroscopeData; // 0X0403 陀螺仪数据 + dbus_data_t dbugData; // 0X0404 遥控器及键鼠数据 + joint_info_t jointState; // 0X0405 工程机械臂数据 + joint_info_t jointControl; // 0X0406 工程机械臂数据 + robot_interactive_data_t robotCommunication; // 0XF301 学生机器人间通信 + board_interactive_data_t boardAlpha; // 0X0501 主控板间通信 + board_interactive_data_t boardBeta; // 0X0502 主控板间通信 + }; + struct + { + uint8_t data[660]; }; } ProtocolData_Type; diff --git a/user/gold-miner/tasks.c b/user/gold-miner/tasks.c index c63b9d18..1d96c79d 100644 --- a/user/gold-miner/tasks.c +++ b/user/gold-miner/tasks.c @@ -12,6 +12,7 @@ void Task_Control(void *Parameters) { int intervalms = interval * 1000; // 任务运行间隔 ms while (1) { // SafetyMode = (RIGHT_SWITCH_BOTTOM && LEFT_SWITCH_BOTTOM); + SafetyMode = SafetyMode ? 1 : (ABS(remoteData.rx) > 440); vTaskDelayUntil(&LastWakeTime, intervalms); } vTaskDelete(NULL); @@ -97,17 +98,30 @@ void Task_Chassis(void *Parameters) { void Task_Communication(void *Parameters) { TickType_t LastWakeTime = xTaskGetTickCount(); // 时钟 - float interval = 0.001; // 任务运行间隔 s + float interval = 0.002; // 任务运行间隔 s int intervalms = interval * 1000; // 任务运行间隔 ms ProtocolInfo_Type *protocolInfo = Protocol_Get_Info_Handle(0x501); extern DMA_Type DMA_Table[10]; - while (1) { + int8_t i = 0; + while (1) { + i++; // 修改数据 - ProtocolData.boardAlpha.data1 = 1; - ProtocolData.boardAlpha.data2 = 2; - ProtocolData.boardAlpha.data3 = 3; - ProtocolData.boardAlpha.data4 = 4; + ProtocolData.jointState.base_joint_position = Motor_BaseJoint.angle; + ProtocolData.jointState.base_joint_speed = Motor_BaseJoint.speed; + ProtocolData.jointState.shoulder_joint_position = Motor_ShoulderJoint.angle; + ProtocolData.jointState.shoulder_joint_speed = Motor_ShoulderJoint.speed; + ProtocolData.jointState.elbow_joint_position = Motor_ElbowJoint.angle; + ProtocolData.jointState.elbow_joint_speed = Motor_ElbowJoint.speed; + ProtocolData.jointState.wrist_joint_1_position = Motor_WristJoint1.angle; + ProtocolData.jointState.wrist_joint_1_speed = Motor_WristJoint1.speed; + ProtocolData.jointState.wrist_joint_2_position = Motor_WristJoint2.angle; + ProtocolData.jointState.wrist_joint_2_speed = Motor_WristJoint2.speed; + + // PID_WristJoint1Speed.i = CHOOSEL(1, 2, 3) * CHOOSER(100, 1000, 0); + // PID_WristJoint1Speed.p = CHOOSEL(1, 4, 7) * CHOOSER(0.01, 0.1, 1); + // PID_WristJoint2Angle.p = CHOOSEL(1, 3, 10) * CHOOSER(0.1, 1, 10); + // PID_WristJoint1Speed.p = CHOOSER(4000, 5500, 7000); // 建议在main.c中使用Bridge_Send_Protocol // Bridge_Send_Protocol_Once(&Node_Host, 0x501); @@ -116,7 +130,9 @@ void Task_Communication(void *Parameters) { vTaskDelayUntil(&LastWakeTime, intervalms); // 调试信息 - // DebugData.debug1 = ProtocolData.boardAlpha.data1 * 1000; + DebugData.debug1 = PID_WristJoint1Angle.target * 1000; + DebugData.debug2 = PID_WristJoint1Angle.feedback * 1000; + DebugData.debug3 = PID_WristJoint1Angle.output; // DebugData.debug2 = ProtocolData.boardAlpha.data2 * 1000; // DebugData.debug3 = Node_Host.sendSeq; // DebugData.debug3 = Node_Host.sendLock; @@ -130,25 +146,56 @@ void Task_Manipulator(void *Parameters) { float interval = 0.01; // 任务运行间隔 s int intervalms = interval * 1000; // 任务运行间隔 ms - PID_Init(&PID_Arm_Speed, 40, 0, 0, 16384, 16384); - PID_Init(&PID_Arm_Angle, 0, 0, 0, 16384, 16384); + // 反馈值 + float base_Jointangle, base_Jointspeed, shoulder_Jointangle, shoulder_Jointspeed, elbow_Jointangle, elbow_Jointspeed; + float wrist_Joint_1angle, wrist_Joint_1speed, wrist_Joint_2angle, wrist_Joint_2speed; + + // PID_Init(&PID_BaseJointAngle, 40, 3, 0, 16384, 16384); + // PID_Init(&PID_BaseJointSpeed, 0, 0, 0, 16384, 16384); + // PID_Init(&PID_ShoulderJointAngle, 40, 3, 0, 16384, 16384); + // PID_Init(&PID_ShoulderJointSpeed, 0, 0, 0, 16384, 16384); + // PID_Init(&PID_ElbowAngle, 40, 3, 0, 16384, 16384); + // PID_Init(&PID_ElbowSpeed, 0, 0, 0, 16384, 16384); + PID_Init(&PID_WristJoint1Angle, 0.3, 0, 0, 16384, 16384); + PID_Init(&PID_WristJoint1Speed, 4000, 100, 0, 16384, 4000); + PID_Init(&PID_WristJoint2Angle, 80, 0, 0, 400, 0); + PID_Init(&PID_WristJoint2Speed, 30, 0.75, 0, 15000, 3000); - int armAngle = 0; while (1) { - PID_Arm_Speed.i = CHOOSEL(1, 3, 10); - PID_Calculate(&PID_Arm_Speed, (remoteData.ly > 0 ? 1 : -1) * (ABS(remoteData.ly) > 330 ? 200 : 0), Motor_Arm.speed * RPM2RPS); + // PID_Calculate(&PID_BaseJointSpeed, PID_BaseJointSpeed.target, Motor_BaseJoint.speed * RPM2RPS); + // PID_Calculate(&PID_ShoulderJointSpeed, PID_ShoulderJointSpeed.target, Motor_ShoulderJoint.speed * RPM2RPS); + // PID_Calculate(&PID_ElbowSpeed, PID_ElbowSpeed.target, Motor_ElbowJoint.speed * RPM2RPS); + // PID_Calculate(&PID_WristJoint1Speed, PID_WristJoint1Speed.target, Motor_WristJoint1.speed * RPM2RPS); + // PID_Calculate(&PID_WristJoint2Speed, PID_WristJoint2Speed.target, Motor_WristJoint2.speed * RPM2RPS); + + PID_WristJoint1Angle.target = ABS(remoteData.lx) > 100 ? remoteData.lx / 300 * 45 : 0; + PID_WristJoint2Angle.target = ABS(remoteData.ly) > 100 ? remoteData.ly / 300 * 15 : 0; + + // PID_Calculate(&PID_BaseJointAngle, PID_BaseJointAngle.target, Motor_BaseJoint.angle); + // PID_Calculate(&PID_BaseJointSpeed, PID_BaseJointAngle.output, Motor_BaseJoint.speed * RPM2RPS); + // PID_Calculate(&PID_ShoulderJointAngle, PID_ShoulderJointAngle.target, Motor_ShoulderJoint.angle); + // PID_Calculate(&PID_ShoulderJointSpeed, PID_ShoulderJointAngle.output, Motor_ShoulderJoint.speed * RPM2RPS); + // PID_Calculate(&PID_ElbowAngle, PID_ElbowAngle.target, Motor_ElbowJoint.angle); + // PID_Calculate(&PID_ElbowSpeed, PID_ElbowAngle.output, Motor_ElbowJoint.speed * RPM2RPS); + PID_Calculate(&PID_WristJoint1Angle, PID_WristJoint1Angle.target, Motor_WristJoint1.angle); + PID_Calculate(&PID_WristJoint1Speed, PID_WristJoint1Angle.output, Motor_WristJoint1.speed * RPM2RPS); + PID_Calculate(&PID_WristJoint2Angle, PID_WristJoint2Angle.target, Motor_WristJoint2.angle); + if (ABS(remoteData.ry) > 100) { + PID_WristJoint2Angle.output = remoteData.ry / 330 * 5 * 30; + Motor_WristJoint2.positionBias = Motor_WristJoint2.position; + Motor_WristJoint2.round = 0; + } + PID_Calculate(&PID_WristJoint2Speed, PID_WristJoint2Angle.output, Motor_WristJoint2.speed * RPM2RPS); - // armAngle += remoteData.ly / 660.0f * 50; - // PID_Calculate(&PID_Arm_Angle, armAngle, Motor_Arm.angle * RPM2RPS); - // PID_Calculate(&PID_Arm_Speed, PID_Arm_Angle.output, Motor_Arm.speed * RPM2RPS); + //输出电流到电调 + Motor_BaseJoint.input = PID_BaseJointSpeed.output; + Motor_ShoulderJoint.input = PID_ShoulderJointSpeed.output; + Motor_ElbowJoint.input = PID_ElbowSpeed.output; + Motor_WristJoint1.input = PID_WristJoint1Speed.output; + Motor_WristJoint2.input = PID_WristJoint2Speed.output; - Motor_Arm.input = PID_Arm_Speed.output; vTaskDelayUntil(&LastWakeTime, intervalms); // 发送频率 - - DebugData.debug1 = PID_Arm_Speed.target; - DebugData.debug2 = PID_Arm_Speed.feedback; - DebugData.debug3 = PID_Arm_Speed.output; } vTaskDelete(NULL); } From 2f9f4479aef37e4d6b74c069fce3725d080ada9a Mon Sep 17 00:00:00 2001 From: odin1777 <947287884@qq.com> Date: Thu, 27 May 2021 23:55:05 +0800 Subject: [PATCH 07/10] update mdk files --- mdk/gold-miner.uvoptx | 61 +++++++++++++++++++++++++++++++----------- mdk/gold-miner.uvprojx | 33 +++-------------------- 2 files changed, 49 insertions(+), 45 deletions(-) diff --git a/mdk/gold-miner.uvoptx b/mdk/gold-miner.uvoptx index dede32c3..9a699d8e 100644 --- a/mdk/gold-miner.uvoptx +++ b/mdk/gold-miner.uvoptx @@ -78,6 +78,48 @@ 1 18 + + + 0 + STM32F429I-Discovery Quick Start Guide (STM32F429I-Discovery) + C:\Keil_v5\ARM\PACK\Keil\STM32F4xx_DFP\2.15.0\MDK\Boards\ST\STM32F429I-Discovery\Documentation\32F429IDISCOVERY_QSG.pdf + + + 1 + Getting Started (STM32F429I-Discovery) + C:\Keil_v5\ARM\PACK\Keil\STM32F4xx_DFP\2.15.0\MDK\Boards\ST\STM32F429I-Discovery\Documentation\DM00092920.pdf + + + 2 + User Manual (STM32F429I-Discovery) + C:\Keil_v5\ARM\PACK\Keil\STM32F4xx_DFP\2.15.0\MDK\Boards\ST\STM32F429I-Discovery\Documentation\DM00093903.pdf + + + 3 + STM32F429I-Discovery: Blinky Lab (STM32F429I-Discovery) + C:\Keil_v5\ARM\PACK\Keil\STM32F4xx_DFP\2.15.0\MDK\Boards\ST\STM32F429I-Discovery\Documentation\STMicroelectronics_Blinky_Lab.pdf + + + 4 + Bill of Materials (STM32F429I-Discovery) + C:\Keil_v5\ARM\PACK\Keil\STM32F4xx_DFP\2.15.0\MDK\Boards\ST\STM32F429I-Discovery\Documentation\stm32f429i-disco_bom.zip + + + 5 + Gerber Files (STM32F429I-Discovery) + C:\Keil_v5\ARM\PACK\Keil\STM32F4xx_DFP\2.15.0\MDK\Boards\ST\STM32F429I-Discovery\Documentation\stm32f429i-disco_gerber.zip + + + 6 + Schematics (STM32F429I-Discovery) + C:\Keil_v5\ARM\PACK\Keil\STM32F4xx_DFP\2.15.0\MDK\Boards\ST\STM32F429I-Discovery\Documentation\stm32f429i-disco_sch.zip + + + 7 + STM32F429I-Discovery Web Page (STM32F429I-Discovery) + http://www.st.com/web/catalog/tools/FM116/SC959/SS1532/LN1199/PF259090 + + 0 1 @@ -100,10 +142,7 @@ 1 0 0 - 1 - 0 - 0 - 6 + 3 @@ -389,19 +428,11 @@ 0 - 0 - 0 - - - - - - - - + + + 0 1 - 0 0 2 10000000 diff --git a/mdk/gold-miner.uvprojx b/mdk/gold-miner.uvprojx index 928238dd..c27101a6 100644 --- a/mdk/gold-miner.uvprojx +++ b/mdk/gold-miner.uvprojx @@ -10,13 +10,12 @@ Target 1 0x4 ARM-ADS - 5060750::V5.06 update 6 (build 750)::ARMCC - 0 + 5060183::V5.06 update 2 (build 183)::ARMCC STM32F427II STMicroelectronics - Keil.STM32F4xx_DFP.2.14.0 + Keil.STM32F4xx_DFP.2.15.0 http://www.keil.com/pack/ IRAM(0x20000000,0x30000) IRAM2(0x10000000,0x10000) IROM(0x08000000,0x200000) CPUTYPE("Cortex-M4") FPU2 CLOCK(12000000) ELITTLE @@ -184,8 +183,6 @@ 0 0 2 - 0 - 0 1 0 8 @@ -326,7 +323,6 @@ 0 0 0 - 0 0 1 1 @@ -334,12 +330,11 @@ 1 0 0 - 0 --gnu STM32F427_437xx,USE_STDPERIPH_DRIVER - ..\core;..\bsp\inc;..\driver\inc;..\library\std\inc;..\user;..\library\freertos\include;..\library\freertos\portable\RVDS\ARM_CM4F;..\user\common;..\library\atom\delay;..\library\atom\sys;..\library\atom\usart;..\library\atom\debug;..\library\mpu6500;..\library\vegmath;..\user\template;..\library\oled + ..\core;..\bsp\inc;..\driver\inc;..\library\std\inc;..\user;..\library\freertos\include;..\library\freertos\portable\RVDS\ARM_CM4F;..\user\common;..\library\atom\delay;..\library\atom\sys;..\library\atom\usart;..\library\atom\debug;..\library\mpu6500;..\library\vegmath;..\user\gold-miner;..\library\oled @@ -352,7 +347,6 @@ 0 0 0 - 1 @@ -672,25 +666,4 @@ - - - - - - - - - - <Project Info> - - - - - - 0 - 1 - - - - From 365f9502fe6fc6ee08430584c3367294b25e90a9 Mon Sep 17 00:00:00 2001 From: odin1777 <947287884@qq.com> Date: Sun, 30 May 2021 00:02:48 +0800 Subject: [PATCH 08/10] =?UTF-8?q?=E6=9B=B4=E6=96=B0=E5=B7=A5=E7=A8=8B?= =?UTF-8?q?=E4=BB=A3=E7=A0=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- user/gold-miner/main.c | 6 ++++- user/gold-miner/tasks.c | 51 ++++++++++++++++++++++++++++++----------- 2 files changed, 43 insertions(+), 14 deletions(-) diff --git a/user/gold-miner/main.c b/user/gold-miner/main.c index df0812d4..66abe001 100644 --- a/user/gold-miner/main.c +++ b/user/gold-miner/main.c @@ -70,6 +70,10 @@ int main(void) { // ADC BSP_ADC1_Init(1, ADC_Channel6, 0); + // 电机设置 + Motor_WristJoint1.positionBias = 2110; + Motor_WristJoint1.position = 2110; + // 总线设置 // Bridge_Bind(&BridgeData, CAN1_BRIDGE, 0x201, &Motor_LF); // Bridge_Bind(&BridgeData, CAN1_BRIDGE, 0x202, &Motor_LB); @@ -77,7 +81,7 @@ int main(void) { // Bridge_Bind(&BridgeData, CAN1_BRIDGE, 0x204, &Motor_RF); // Bridge_Bind(&BridgeData, CAN2_BRIDGE, 0x205, &Motor_BaseJoint); // Bridge_Bind(&BridgeData, CAN2_BRIDGE, 0x201, &Motor_ShoulderJoint); - // Bridge_Bind(&BridgeData, CAN2_BRIDGE, 0x202, &Motor_ElbowJoint); + Bridge_Bind(&BridgeData, CAN2_BRIDGE, 0x202, &Motor_ElbowJoint); Bridge_Bind(&BridgeData, CAN2_BRIDGE, 0x206, &Motor_WristJoint1); Bridge_Bind(&BridgeData, CAN2_BRIDGE, 0x203, &Motor_WristJoint2); Bridge_Bind(&BridgeData, USART_BRIDGE, 7, &Node_Host); diff --git a/user/gold-miner/tasks.c b/user/gold-miner/tasks.c index 1d96c79d..c9f8578b 100644 --- a/user/gold-miner/tasks.c +++ b/user/gold-miner/tasks.c @@ -103,21 +103,28 @@ void Task_Communication(void *Parameters) { ProtocolInfo_Type *protocolInfo = Protocol_Get_Info_Handle(0x501); extern DMA_Type DMA_Table[10]; + float positionFactor; + int8_t i = 0; + while (1) { i++; // 修改数据 - ProtocolData.jointState.base_joint_position = Motor_BaseJoint.angle; + positionFactor = 1.0 / 360 * 8192; + ProtocolData.jointState.base_joint_position = Motor_BaseJoint.angle * positionFactor; ProtocolData.jointState.base_joint_speed = Motor_BaseJoint.speed; - ProtocolData.jointState.shoulder_joint_position = Motor_ShoulderJoint.angle; + ProtocolData.jointState.shoulder_joint_position = Motor_ShoulderJoint.angle * positionFactor; ProtocolData.jointState.shoulder_joint_speed = Motor_ShoulderJoint.speed; - ProtocolData.jointState.elbow_joint_position = Motor_ElbowJoint.angle; + ProtocolData.jointState.elbow_joint_position = Motor_ElbowJoint.angle * positionFactor; ProtocolData.jointState.elbow_joint_speed = Motor_ElbowJoint.speed; - ProtocolData.jointState.wrist_joint_1_position = Motor_WristJoint1.angle; + ProtocolData.jointState.wrist_joint_1_position = Motor_WristJoint1.angle * positionFactor; ProtocolData.jointState.wrist_joint_1_speed = Motor_WristJoint1.speed; - ProtocolData.jointState.wrist_joint_2_position = Motor_WristJoint2.angle; + ProtocolData.jointState.wrist_joint_2_position = Motor_WristJoint2.angle * positionFactor; ProtocolData.jointState.wrist_joint_2_speed = Motor_WristJoint2.speed; + // PID_WristJoint1Angle.target = ProtocolData.jointControl.wrist_joint_1_position / 8192.0 * 360; + // PID_WristJoint2Angle.target = ProtocolData.jointControl.wrist_joint_2_position / 8192.0 * 360; + // PID_WristJoint1Speed.i = CHOOSEL(1, 2, 3) * CHOOSER(100, 1000, 0); // PID_WristJoint1Speed.p = CHOOSEL(1, 4, 7) * CHOOSER(0.01, 0.1, 1); // PID_WristJoint2Angle.p = CHOOSEL(1, 3, 10) * CHOOSER(0.1, 1, 10); @@ -130,13 +137,18 @@ void Task_Communication(void *Parameters) { vTaskDelayUntil(&LastWakeTime, intervalms); // 调试信息 - DebugData.debug1 = PID_WristJoint1Angle.target * 1000; - DebugData.debug2 = PID_WristJoint1Angle.feedback * 1000; - DebugData.debug3 = PID_WristJoint1Angle.output; + // DebugData.debug1 = ProtocolData.jointControl.wrist_joint_1_position / 8192.0 * 360000; + // DebugData.debug2 = ProtocolData.jointControl.wrist_joint_2_position / 8192.0 * 360000; + // DebugData.debug1 = PID_ElbowSpeed.target * 1000; + // DebugData.debug2 = PID_ElbowSpeed.feedback * 1000; + // DebugData.debug3 = PID_ElbowSpeed.output; // DebugData.debug2 = ProtocolData.boardAlpha.data2 * 1000; // DebugData.debug3 = Node_Host.sendSeq; // DebugData.debug3 = Node_Host.sendLock; // DebugData.debug4 = protocolInfo->receiveCount; + DebugData.debug3 = ABS(remoteData.lx) > 100 ? (remoteData.lx / 660.0f * 45) : 0; + // DebugData.debug3 = PID_WristJoint1Angle.feedback; + DebugData.debug4 = PID_WristJoint2Angle.feedback; } vTaskDelete(NULL); } @@ -154,8 +166,8 @@ void Task_Manipulator(void *Parameters) { // PID_Init(&PID_BaseJointSpeed, 0, 0, 0, 16384, 16384); // PID_Init(&PID_ShoulderJointAngle, 40, 3, 0, 16384, 16384); // PID_Init(&PID_ShoulderJointSpeed, 0, 0, 0, 16384, 16384); - // PID_Init(&PID_ElbowAngle, 40, 3, 0, 16384, 16384); - // PID_Init(&PID_ElbowSpeed, 0, 0, 0, 16384, 16384); + PID_Init(&PID_ElbowAngle, 40, 3, 0, 16384, 16384); + PID_Init(&PID_ElbowSpeed, 0, 0, 0, 16384, 16384); PID_Init(&PID_WristJoint1Angle, 0.3, 0, 0, 16384, 16384); PID_Init(&PID_WristJoint1Speed, 4000, 100, 0, 16384, 4000); PID_Init(&PID_WristJoint2Angle, 80, 0, 0, 400, 0); @@ -168,20 +180,33 @@ void Task_Manipulator(void *Parameters) { // PID_Calculate(&PID_ElbowSpeed, PID_ElbowSpeed.target, Motor_ElbowJoint.speed * RPM2RPS); // PID_Calculate(&PID_WristJoint1Speed, PID_WristJoint1Speed.target, Motor_WristJoint1.speed * RPM2RPS); // PID_Calculate(&PID_WristJoint2Speed, PID_WristJoint2Speed.target, Motor_WristJoint2.speed * RPM2RPS); + if (remoteData.lx > 100) { + PID_ElbowSpeed.target = remoteData.lx / 300 * 50; + } else if (remoteData.lx < -100) { + PID_ElbowSpeed.target = ((remoteData.lx / 300) - 1) * 50; + } else { + PID_ElbowSpeed.target = 0; + } + + // PID_ElbowSpeed.target = ABS(remoteData.lx) > 100 ? remoteData.lx / 300 * 50 : 0; + PID_ElbowSpeed.p = CHOOSEL(7, 21, 70) * CHOOSER(0.1, 1, 10); - PID_WristJoint1Angle.target = ABS(remoteData.lx) > 100 ? remoteData.lx / 300 * 45 : 0; - PID_WristJoint2Angle.target = ABS(remoteData.ly) > 100 ? remoteData.ly / 300 * 15 : 0; + // PID_WristJoint1Angle.target = ABS(remoteData.lx) > 100 ? remoteData.lx / 300 * 45 : PID_WristJoint1Angle.target; + // PID_WristJoint2Angle.target = ABS(remoteData.ly) > 100 ? remoteData.ly / 300 * 15 : PID_WristJoint2Angle.target; + // PID_WristJoint1Angle.target += ABS(remoteData.lx) > 100 ? (remoteData.lx / 660.0f * 45 * interval) : 0; + // PID_WristJoint2Angle.target += ABS(remoteData.ly) > 100 ? (remoteData.ly / 660.0f * 15 * interval) : 0; // PID_Calculate(&PID_BaseJointAngle, PID_BaseJointAngle.target, Motor_BaseJoint.angle); // PID_Calculate(&PID_BaseJointSpeed, PID_BaseJointAngle.output, Motor_BaseJoint.speed * RPM2RPS); // PID_Calculate(&PID_ShoulderJointAngle, PID_ShoulderJointAngle.target, Motor_ShoulderJoint.angle); // PID_Calculate(&PID_ShoulderJointSpeed, PID_ShoulderJointAngle.output, Motor_ShoulderJoint.speed * RPM2RPS); // PID_Calculate(&PID_ElbowAngle, PID_ElbowAngle.target, Motor_ElbowJoint.angle); - // PID_Calculate(&PID_ElbowSpeed, PID_ElbowAngle.output, Motor_ElbowJoint.speed * RPM2RPS); + PID_Calculate(&PID_ElbowSpeed, PID_ElbowSpeed.target, Motor_ElbowJoint.speed * RPM2RPS); PID_Calculate(&PID_WristJoint1Angle, PID_WristJoint1Angle.target, Motor_WristJoint1.angle); PID_Calculate(&PID_WristJoint1Speed, PID_WristJoint1Angle.output, Motor_WristJoint1.speed * RPM2RPS); PID_Calculate(&PID_WristJoint2Angle, PID_WristJoint2Angle.target, Motor_WristJoint2.angle); if (ABS(remoteData.ry) > 100) { + PID_WristJoint2Angle.target = 0; PID_WristJoint2Angle.output = remoteData.ry / 330 * 5 * 30; Motor_WristJoint2.positionBias = Motor_WristJoint2.position; Motor_WristJoint2.round = 0; From 1af4e7cdd558f2f2aefb3b8c6cac2f7d9e6295ef Mon Sep 17 00:00:00 2001 From: odin1777 <947287884@qq.com> Date: Wed, 2 Jun 2021 16:51:15 +0800 Subject: [PATCH 09/10] =?UTF-8?q?=E6=9B=B4=E6=96=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- driver/inc/Driver_Motor.h | 7 + driver/inc/Driver_Protocol.h | 4 +- driver/src/Driver_Motor.c | 8 +- user/common/default_config.h | 2 +- user/gold-miner/handle.h | 7 +- user/gold-miner/main.c | 6 +- user/gold-miner/protocol.h | 518 ++++++++++++++--------------------- user/gold-miner/tasks.c | 110 ++++---- user/gold-miner/tasks.h | 1 + 9 files changed, 296 insertions(+), 367 deletions(-) diff --git a/driver/inc/Driver_Motor.h b/driver/inc/Driver_Motor.h index 177f0690..f7d22032 100644 --- a/driver/inc/Driver_Motor.h +++ b/driver/inc/Driver_Motor.h @@ -67,4 +67,11 @@ void Motor_Update(Motor_Type *motor, uint8_t data[8]); */ void Motor_Set_Angle_Bias(Motor_Type *motor, float angleBias); +/** + * @brief 校准连续角度为输入量 + * + * @param calibratedAngle 校准角度 + */ +void Motor_Calibrate(Motor_Type *motor, float calibratedAngle); + #endif diff --git a/driver/inc/Driver_Protocol.h b/driver/inc/Driver_Protocol.h index 789d7427..8a2ff9ac 100644 --- a/driver/inc/Driver_Protocol.h +++ b/driver/inc/Driver_Protocol.h @@ -63,8 +63,8 @@ typedef struct { // 接收相关 uint16_t receive; // 是否接收并解包该协议 uint16_t receiveCount; // 接收计数 - uint16_t receiveSeq; // 接收包序号 - uint16_t lastReceiveSeq; // (仅供用户使用)上次接收序号 + int16_t receiveSeq; // 接收包序号 + int16_t lastReceiveSeq; // (仅供用户使用)上次接收序号 TickType_t receiveTime; // 接收时间 // 发送相关 void * node; // 发送节点 diff --git a/driver/src/Driver_Motor.c b/driver/src/Driver_Motor.c index 397878f8..cffad0ad 100644 --- a/driver/src/Driver_Motor.c +++ b/driver/src/Driver_Motor.c @@ -47,7 +47,7 @@ void Motor_Update(Motor_Type *motor, uint8_t data[8]) { if (motor->positionBias != -1) { motor->angleBias = motor->positionBias / 8192.0f * 360; // 向下兼容 } else { - motor->angleBias = angle; // 将当前位置设为 + motor->angleBias = angle; // 将当前位置设为零点 } motor->angleBiasInit = 1; } @@ -63,3 +63,9 @@ void Motor_Set_Angle_Bias(Motor_Type *motor, float angleBias) { motor->angleBiasInit = 1; motor->lastPosition = -1; } + +void Motor_Calibrate(Motor_Type *motor, float calibratedAngle) { + motor->angleBias = motor->angle + motor->angleBias - calibratedAngle; + motor->angle = calibratedAngle; + motor->angleBiasInit = 1; +} diff --git a/user/common/default_config.h b/user/common/default_config.h index 2ae4be0c..c11e323e 100644 --- a/user/common/default_config.h +++ b/user/common/default_config.h @@ -4,7 +4,7 @@ // 总线设置 #define CAN_DEVICE_ID 0x501 // 发送数据时CAN标准帧中的ID 0x501-0x503 -#define CAN_TIMEOUT 100 // 电机超过该时间(ms)无数据将报警 +#define CAN_TIMEOUT 1000 // 电机超过该时间(ms)无数据将报警 // 调试 #define DEBUG_ENABLED 0 // 调试开关 diff --git a/user/gold-miner/handle.h b/user/gold-miner/handle.h index b571dbef..64fbc36d 100644 --- a/user/gold-miner/handle.h +++ b/user/gold-miner/handle.h @@ -37,6 +37,7 @@ __HANDLE_EXT uint8_t Board_Id, Robot_Id; // 功能开关 __HANDLE_EXT uint8_t SafetyMode; +__HANDLE_EXT uint8_t JointCalibrated; // TIM __HANDLE_EXT volatile uint32_t ulHighFrequencyTimerTicks; @@ -46,9 +47,9 @@ __HANDLE_EXT Motor_Type Motor_LF, Motor_RF, Motor_RB, Motor_LB; // 机械臂 __HANDLE_EXT Motor_Type Motor_BaseJoint, Motor_ShoulderJoint, Motor_ElbowJoint, Motor_WristJoint1, Motor_WristJoint2; -__HANDLE_EXT PID_Type PID_BaseJointSpeed, PID_BaseJointAngle,PID_ShoulderJointSpeed, PID_ShoulderJointAngle, - PID_ElbowSpeed, PID_ElbowAngle,PID_WristJoint1Speed, PID_WristJoint1Angle, PID_WristJoint2Speed, PID_WristJoint2Angle; - +__HANDLE_EXT PID_Type PID_BaseJointSpeed, PID_BaseJointAngle, PID_ShoulderJointSpeed, PID_ShoulderJointAngle, PID_ElbowSpeed, PID_ElbowAngle, + PID_WristJoint1Speed, PID_WristJoint1Angle, PID_WristJoint2Speed, PID_WristJoint2Angle; + // 遥控器 __HANDLE_EXT uint8_t remoteBuffer[DBUS_LENGTH + DBUS_BACK_LENGTH]; __HANDLE_EXT Remote_Type remoteData; diff --git a/user/gold-miner/main.c b/user/gold-miner/main.c index 66abe001..df0812d4 100644 --- a/user/gold-miner/main.c +++ b/user/gold-miner/main.c @@ -70,10 +70,6 @@ int main(void) { // ADC BSP_ADC1_Init(1, ADC_Channel6, 0); - // 电机设置 - Motor_WristJoint1.positionBias = 2110; - Motor_WristJoint1.position = 2110; - // 总线设置 // Bridge_Bind(&BridgeData, CAN1_BRIDGE, 0x201, &Motor_LF); // Bridge_Bind(&BridgeData, CAN1_BRIDGE, 0x202, &Motor_LB); @@ -81,7 +77,7 @@ int main(void) { // Bridge_Bind(&BridgeData, CAN1_BRIDGE, 0x204, &Motor_RF); // Bridge_Bind(&BridgeData, CAN2_BRIDGE, 0x205, &Motor_BaseJoint); // Bridge_Bind(&BridgeData, CAN2_BRIDGE, 0x201, &Motor_ShoulderJoint); - Bridge_Bind(&BridgeData, CAN2_BRIDGE, 0x202, &Motor_ElbowJoint); + // Bridge_Bind(&BridgeData, CAN2_BRIDGE, 0x202, &Motor_ElbowJoint); Bridge_Bind(&BridgeData, CAN2_BRIDGE, 0x206, &Motor_WristJoint1); Bridge_Bind(&BridgeData, CAN2_BRIDGE, 0x203, &Motor_WristJoint2); Bridge_Bind(&BridgeData, USART_BRIDGE, 7, &Node_Host); diff --git a/user/gold-miner/protocol.h b/user/gold-miner/protocol.h index 88fad61a..996ac3ec 100644 --- a/user/gold-miner/protocol.h +++ b/user/gold-miner/protocol.h @@ -11,7 +11,7 @@ * 裁判系统 0xF100 - 0xF1FF * 车间通讯 0xF200 - 0xF2FF * @version judge.yml v1.0 - * @version host.yml v1.2 + * @version host.yml v1.3 * @version user.yml v1.0 */ @@ -19,8 +19,7 @@ * 结构体 **********************************************************************/ -typedef struct -{ +typedef struct { uint8_t graphic_name[3]; uint32_t operate_tpye : 3; uint32_t graphic_tpye : 3; @@ -36,8 +35,7 @@ typedef struct uint32_t end_y : 11; } GraphicData_Type; -typedef struct -{ +typedef struct { int32_t debug1; int32_t debug2; int32_t debug3; @@ -52,37 +50,29 @@ typedef struct * 协议列表 **********************************************************************/ -typedef union -{ - struct - { - uint8_t game_type : 4; - uint8_t game_progress : 4; +typedef union { + struct { + uint8_t game_type : 4; + uint8_t game_progress : 4; uint16_t stage_remain_time; uint64_t SyncTimeStamp; }; - struct - { + struct { uint8_t data[11]; }; } game_status_t; -typedef union -{ - struct - { +typedef union { + struct { uint8_t winner; }; - struct - { + struct { uint8_t data[1]; }; } game_result_t; -typedef union -{ - struct - { +typedef union { + struct { uint16_t red_1_robot_HP; uint16_t red_2_robot_HP; uint16_t red_3_robot_HP; @@ -100,83 +90,67 @@ typedef union uint16_t blue_outpost_HP; uint16_t blue_base_HP; }; - struct - { + struct { uint8_t data[32]; }; } game_robot_HP_t; -typedef union -{ - struct - { - uint8_t dart_belong; +typedef union { + struct { + uint8_t dart_belong; uint16_t stage_remaining_time; }; - struct - { + struct { uint8_t data[3]; }; } dart_status_t; -typedef union -{ - struct - { +typedef union { + struct { uint32_t event_type; }; - struct - { + struct { uint8_t data[4]; }; } event_data_t; -typedef union -{ - struct - { +typedef union { + struct { uint8_t supply_projectile_id; uint8_t supply_robot_id; uint8_t supply_projectile_step; uint8_t supply_projectile_num; }; - struct - { + struct { uint8_t data[4]; }; } supply_projectile_action_t; -typedef union -{ - struct - { +typedef union { + struct { uint8_t level; uint8_t foul_robot_id; }; - struct - { + struct { uint8_t data[2]; }; } referee_warning_t; -typedef union -{ - struct - { +typedef union { + struct { uint8_t dart_remaining_time; }; - struct - { + struct { uint8_t data[1]; }; } dart_remaining_time_t; -typedef union -{ - struct - { - uint8_t robot_id; uint8_t robot_level; - uint16_t remain_HP; uint16_t max_HP; +typedef union { + struct { + uint8_t robot_id; + uint8_t robot_level; + uint16_t remain_HP; + uint16_t max_HP; uint16_t shooter_id1_17mm_cooling_rate; uint16_t shooter_id1_17mm_cooling_limit; uint16_t shooter_id1_17mm_speed_limit; @@ -187,334 +161,263 @@ typedef union uint16_t shooter_id1_42mm_cooling_limit; uint16_t shooter_id1_42mm_speed_limit; uint16_t chassis_power_limit; - uint8_t mains_power_gimbal_output : 1; - uint8_t mains_power_chassis_output : 1; - uint8_t mains_power_shooter_output : 1; + uint8_t mains_power_gimbal_output : 1; + uint8_t mains_power_chassis_output : 1; + uint8_t mains_power_shooter_output : 1; }; - struct - { + struct { uint8_t data[27]; }; } game_robot_status_t; -typedef union -{ - struct - { +typedef union { + struct { uint16_t chassis_volt; uint16_t chassis_current; - float chassis_power; + float chassis_power; uint16_t chassis_power_buffer; uint16_t shooter_id1_17mm_cooling_heat; uint16_t shooter_id2_17mm_cooling_heat; uint16_t shooter_id1_42mm_cooling_heat; }; - struct - { + struct { uint8_t data[16]; }; } power_heat_data_t; -typedef union -{ - struct - { +typedef union { + struct { float x; float y; float z; float yaw; }; - struct - { + struct { uint8_t data[16]; }; } game_robot_pos_t; -typedef union -{ - struct - { +typedef union { + struct { uint8_t power_rune_buff; }; - struct - { + struct { uint8_t data[1]; }; } buff_info_t; -typedef union -{ - struct - { +typedef union { + struct { uint8_t attack_time; }; - struct - { + struct { uint8_t data[1]; }; } aerial_robot_energy_t; -typedef union -{ - struct - { +typedef union { + struct { uint8_t armor_id : 4; uint8_t hurt_type : 4; }; - struct - { + struct { uint8_t data[1]; }; } robot_hurt_t; -typedef union -{ - struct - { +typedef union { + struct { uint8_t bullet_type; uint8_t shooter_id; uint8_t bullet_freq; - float bullet_speed; + float bullet_speed; }; - struct - { + struct { uint8_t data[7]; }; } shoot_data_t; -typedef union -{ - struct - { +typedef union { + struct { uint16_t bullet_remaining_num_17mm; uint16_t bullet_remaining_num_42mm; uint16_t coin_remaining_num; }; - struct - { + struct { uint8_t data[6]; }; } bullet_remaining_t; -typedef union -{ - struct - { +typedef union { + struct { uint32_t rfid_status; }; - struct - { + struct { uint8_t data[4]; }; } rfid_status_t; -typedef union -{ - struct - { - uint8_t dart_launch_opening_status; - uint8_t dart_attack_target; +typedef union { + struct { + uint8_t dart_launch_opening_status; + uint8_t dart_attack_target; uint16_t target_change_time; - uint8_t first_dart_speed; - uint8_t second_dart_speed; - uint8_t third_dart_speed; - uint8_t fourth_dart_speed; + uint8_t first_dart_speed; + uint8_t second_dart_speed; + uint8_t third_dart_speed; + uint8_t fourth_dart_speed; uint16_t last_dart_launch_time; uint16_t operate_launch_cmd_time; }; - struct - { + struct { uint8_t data[12]; }; } dart_client_cmd_t; -typedef union -{ - struct - { - uint16_t data_cmd_id; - uint16_t send_id; - uint16_t receiver_id; - uint8_t operate_tpye; - uint8_t layer; - }; - struct - { +typedef union { + struct { + uint16_t data_cmd_id; + uint16_t send_id; + uint16_t receiver_id; + uint8_t operate_tpye; + uint8_t layer; + }; + struct { uint8_t data[8]; }; } client_custom_graphic_delete_t; -typedef union -{ - struct - { - uint16_t data_cmd_id; - uint16_t send_id; - uint16_t receiver_id; - GraphicData_Type grapic_data_struct; - }; - struct - { +typedef union { + struct { + uint16_t data_cmd_id; + uint16_t send_id; + uint16_t receiver_id; + GraphicData_Type grapic_data_struct; + }; + struct { uint8_t data[21]; }; } client_custom_graphic_single_t; -typedef union -{ - struct - { - uint16_t data_cmd_id; - uint16_t send_id; - uint16_t receiver_id; - GraphicData_Type grapic_data_struct[2]; - }; - struct - { +typedef union { + struct { + uint16_t data_cmd_id; + uint16_t send_id; + uint16_t receiver_id; + GraphicData_Type grapic_data_struct[2]; + }; + struct { uint8_t data[36]; }; } client_custom_graphic_double_t; -typedef union -{ - struct - { - uint16_t data_cmd_id; - uint16_t send_id; - uint16_t receiver_id; - GraphicData_Type grapic_data_struct[5]; - }; - struct - { +typedef union { + struct { + uint16_t data_cmd_id; + uint16_t send_id; + uint16_t receiver_id; + GraphicData_Type grapic_data_struct[5]; + }; + struct { uint8_t data[81]; }; } client_custom_graphic_five_t; -typedef union -{ - struct - { - uint16_t data_cmd_id; - uint16_t send_id; - uint16_t receiver_id; - GraphicData_Type grapic_data_struct; - uint8_t data[30]; - }; - struct - { +typedef union { + struct { + uint16_t data_cmd_id; + uint16_t send_id; + uint16_t receiver_id; + GraphicData_Type grapic_data_struct; + uint8_t data[30]; + }; + struct { uint8_t data[51]; }; } client_custom_character_t; -typedef union -{ - struct - { - uint16_t data_cmd_id; - uint16_t send_id; - uint16_t receiver_id; - GraphicData_Type grapic_data_struct[7]; - }; - struct - { +typedef union { + struct { + uint16_t data_cmd_id; + uint16_t send_id; + uint16_t receiver_id; + GraphicData_Type grapic_data_struct[7]; + }; + struct { uint8_t data[111]; }; } client_custom_graphic_seven_t; -typedef union -{ - struct - { +typedef union { + struct { DebugData_Type debugData; }; - struct - { + struct { uint8_t data[32]; }; } debug_info_t; -typedef union -{ - struct - { +typedef union { + struct { uint16_t code; - char text[22]; + char text[22]; }; - struct - { + struct { uint8_t data[24]; }; } error_info_t; -typedef union -{ - struct - { - - }; - struct - { +typedef union { + struct {}; + struct { uint8_t data[0]; }; } heartbeat_t; -typedef union -{ - struct - { - float yaw_angle_diff; - float pitch_angle_diff; +typedef union { + struct { + float yaw_angle_diff; + float pitch_angle_diff; uint8_t fire; }; - struct - { + struct { uint8_t data[9]; }; } autoaim_data_t; -typedef union -{ - struct - { +typedef union { + struct { float vx; float vy; float vw; }; - struct - { + struct { uint8_t data[12]; }; } chassis_data_t; -typedef union -{ - struct - { +typedef union { + struct { float yaw; float pitch; float roll; }; - struct - { + struct { uint8_t data[12]; }; } gyroscope_data_t; -typedef union -{ - struct - { +typedef union { + struct { uint8_t dbusBuffer[18]; }; - struct - { + struct { uint8_t data[18]; }; } dbus_data_t; -typedef union -{ - struct - { +typedef union { + struct { uint8_t state_code; int16_t base_joint_position; int16_t base_joint_speed; @@ -527,16 +430,13 @@ typedef union int16_t wrist_joint_2_position; int16_t wrist_joint_2_speed; }; - struct - { + struct { uint8_t data[21]; }; } joint_info_t; -typedef union -{ - struct - { +typedef union { + struct { uint16_t data_cmd_id; uint16_t send_id; uint16_t receiver_id; @@ -545,23 +445,19 @@ typedef union int32_t data3; int32_t data4; }; - struct - { + struct { uint8_t data[22]; }; } robot_interactive_data_t; -typedef union -{ - struct - { +typedef union { + struct { int32_t data1; int32_t data2; int32_t data3; int32_t data4; }; - struct - { + struct { uint8_t data[16]; }; } board_interactive_data_t; @@ -570,56 +466,66 @@ typedef union * 协议信息 **********************************************************************/ -#define PROTOCOL_INFO_LIST {{0X0001,11,1},{0X0002,1,1},{0X0003,32,1},{0X0004,3,1},{0X0101,4,1},{0X0102,4,1},{0X0104,2,1},{0X0105,1,1},{0X0201,27,1},{0X0202,16,1},{0X0203,16,1},{0X0204,1,1},{0X0205,1,1},{0X0206,1,1},{0X0207,7,1},{0X0208,6,1},{0X0209,4,1},{0X020A,12,1},{0XF100,8,0},{0XF101,21,0},{0XF102,36,0},{0XF103,81,0},{0XF110,51,0},{0XF104,111,0},{0X1024,32,0},{0X6666,24,0},{0X0120,0,1},{0X0401,9,1},{0X0402,12,1},{0X0403,12,0},{0X0404,18,0},{0X0405,21,1},{0X0406,21,1},{0XF301,22,1},{0X0501,16,1},{0X0502,16,1}} +#define PROTOCOL_INFO_LIST \ + { \ + {0X0001, 11, 1, 0, -1, -1}, {0X0002, 1, 1, 0, -1, -1}, {0X0003, 32, 1, 0, -1, -1}, {0X0004, 3, 1, 0, -1, -1}, {0X0101, 4, 1, 0, -1, -1}, \ + {0X0102, 4, 1, 0, -1, -1}, {0X0104, 2, 1, 0, -1, -1}, {0X0105, 1, 1, 0, -1, -1}, {0X0201, 27, 1, 0, -1, -1}, {0X0202, 16, 1, 0, -1, -1}, \ + {0X0203, 16, 1, 0, -1, -1}, {0X0204, 1, 1, 0, -1, -1}, {0X0205, 1, 1, 0, -1, -1}, {0X0206, 1, 1, 0, -1, -1}, {0X0207, 7, 1, 0, -1, -1}, \ + {0X0208, 6, 1, 0, -1, -1}, {0X0209, 4, 1, 0, -1, -1}, {0X020A, 12, 1, 0, -1, -1}, {0XF100, 8, 0, 0, -1, -1}, {0XF101, 21, 0, 0, -1, -1}, \ + {0XF102, 36, 0, 0, -1, -1}, {0XF103, 81, 0, 0, -1, -1}, {0XF110, 51, 0, 0, -1, -1}, {0XF104, 111, 0, 0, -1, -1}, {0X1024, 32, 0, 0, -1, -1}, \ + {0X6666, 24, 0, 0, -1, -1}, {0X0120, 0, 1, 0, -1, -1}, {0X0401, 9, 1, 0, -1, -1}, {0X0402, 12, 1, 0, -1, -1}, {0X0403, 12, 0, 0, -1, -1}, \ + {0X0404, 18, 0, 0, -1, -1}, {0X0405, 21, 1, 0, -1, -1}, {0X0406, 21, 1, 0, -1, -1}, {0X0407, 21, 1, 0, -1, -1}, {0XF301, 22, 1, 0, -1, -1}, \ + {0X0501, 16, 1, 0, -1, -1}, { \ + 0X0502, 16, 1, 0, -1, -1 \ + } \ + } /********************************************************************** * 协议接口 **********************************************************************/ -typedef union -{ - struct - { - game_status_t gameStatus; // 0X0001 比赛状态数据 - game_result_t gameResult; // 0X0002 比赛结果的数据 - game_robot_HP_t healthPoint; // 0X0003 机器人血量数据 - dart_status_t dartStatus; // 0X0004 飞镖发射状态 - event_data_t eventData; // 0X0101 场地事件数据 - supply_projectile_action_t supplyProjectileaction; // 0X0102 补给站动作标识 - referee_warning_t refereeWarning; // 0X0104 裁判警告信息 - dart_remaining_time_t dartRemainingtime; // 0X0105 飞镖发射口倒计 - game_robot_status_t gameRobotstatus; // 0X0201 比赛机器人状态 - power_heat_data_t powerHeatData; // 0X0202 实时功率热量数据 - game_robot_pos_t gameRobotpos; // 0X0203 机器人位置 - buff_info_t buffInfo; // 0X0204 机器人增益 - aerial_robot_energy_t aerialRobotenergy; // 0X0205 空中机器人能量状态 - robot_hurt_t robotHurt; // 0X0206 伤害状态 - shoot_data_t shootData; // 0X0207 实时射击信息 - bullet_remaining_t bulletRemaining; // 0X0208 子弹剩余发射数 - rfid_status_t rfidStatus; // 0X0209 机器人RFID状态 - dart_client_cmd_t dartClientCmds; // 0X020A 飞镖机器人客户端指令数据 - client_custom_graphic_delete_t clientCustomGraphicDelete; // 0XF100 客户端删除图形 +typedef union { + struct { + game_status_t gameStatus; // 0X0001 比赛状态数据 + game_result_t gameResult; // 0X0002 比赛结果的数据 + game_robot_HP_t healthPoint; // 0X0003 机器人血量数据 + dart_status_t dartStatus; // 0X0004 飞镖发射状态 + event_data_t eventData; // 0X0101 场地事件数据 + supply_projectile_action_t supplyProjectileaction; // 0X0102 补给站动作标识 + referee_warning_t refereeWarning; // 0X0104 裁判警告信息 + dart_remaining_time_t dartRemainingtime; // 0X0105 飞镖发射口倒计 + game_robot_status_t gameRobotstatus; // 0X0201 比赛机器人状态 + power_heat_data_t powerHeatData; // 0X0202 实时功率热量数据 + game_robot_pos_t gameRobotpos; // 0X0203 机器人位置 + buff_info_t buffInfo; // 0X0204 机器人增益 + aerial_robot_energy_t aerialRobotenergy; // 0X0205 空中机器人能量状态 + robot_hurt_t robotHurt; // 0X0206 伤害状态 + shoot_data_t shootData; // 0X0207 实时射击信息 + bullet_remaining_t bulletRemaining; // 0X0208 子弹剩余发射数 + rfid_status_t rfidStatus; // 0X0209 机器人RFID状态 + dart_client_cmd_t dartClientCmds; // 0X020A 飞镖机器人客户端指令数据 + client_custom_graphic_delete_t clientCustomGraphicDelete; // 0XF100 客户端删除图形 client_custom_graphic_single_t client_custom_graphicSingle; // 0XF101 客户端绘制一个图形 - client_custom_graphic_double_t clientCustomGraphicDouble; // 0XF102 客户端绘制两个图形 - client_custom_graphic_five_t clientCustomGraphicFive; // 0XF103 客户端绘制五个图形 - client_custom_character_t clientCustomCharacter; // 0XF110 客户端绘制字符 - client_custom_graphic_seven_t clientCustomGraphicSeven; // 0XF104 客户端绘制七个图形 - debug_info_t debugInfo; // 0X1024 调试信息 - error_info_t errorInfo; // 0X6666 报错信息 - heartbeat_t heartbeat; // 0X0120 心跳包 - autoaim_data_t autoaimData; // 0X0401 云台控制 - chassis_data_t chassisData; // 0X0402 底盘控制 - gyroscope_data_t gyroscopeData; // 0X0403 陀螺仪数据 - dbus_data_t dbugData; // 0X0404 遥控器及键鼠数据 - joint_info_t jointState; // 0X0405 工程机械臂数据 - joint_info_t jointControl; // 0X0406 工程机械臂数据 - robot_interactive_data_t robotCommunication; // 0XF301 学生机器人间通信 - board_interactive_data_t boardAlpha; // 0X0501 主控板间通信 - board_interactive_data_t boardBeta; // 0X0502 主控板间通信 - }; - struct - { - uint8_t data[660]; + client_custom_graphic_double_t clientCustomGraphicDouble; // 0XF102 客户端绘制两个图形 + client_custom_graphic_five_t clientCustomGraphicFive; // 0XF103 客户端绘制五个图形 + client_custom_character_t clientCustomCharacter; // 0XF110 客户端绘制字符 + client_custom_graphic_seven_t clientCustomGraphicSeven; // 0XF104 客户端绘制七个图形 + debug_info_t debugInfo; // 0X1024 调试信息 + error_info_t errorInfo; // 0X6666 报错信息 + heartbeat_t heartbeat; // 0X0120 心跳包 + autoaim_data_t autoaimData; // 0X0401 云台控制 + chassis_data_t chassisData; // 0X0402 底盘控制 + gyroscope_data_t gyroscopeData; // 0X0403 陀螺仪数据 + dbus_data_t dbugData; // 0X0404 遥控器及键鼠数据 + joint_info_t jointState; // 0X0405 工程机械臂数据 + joint_info_t jointControl; // 0X0406 工程机械臂数据 + joint_info_t jointCalibration; // 0X0407 工程机械臂数据 + robot_interactive_data_t robotCommunication; // 0XF301 学生机器人间通信 + board_interactive_data_t boardAlpha; // 0X0501 主控板间通信 + board_interactive_data_t boardBeta; // 0X0502 主控板间通信 + }; + struct { + uint8_t data[681]; }; } ProtocolData_Type; diff --git a/user/gold-miner/tasks.c b/user/gold-miner/tasks.c index c9f8578b..7fd790a6 100644 --- a/user/gold-miner/tasks.c +++ b/user/gold-miner/tasks.c @@ -97,20 +97,42 @@ void Task_Chassis(void *Parameters) { } void Task_Communication(void *Parameters) { - TickType_t LastWakeTime = xTaskGetTickCount(); // 时钟 - float interval = 0.002; // 任务运行间隔 s - int intervalms = interval * 1000; // 任务运行间隔 ms - ProtocolInfo_Type *protocolInfo = Protocol_Get_Info_Handle(0x501); - extern DMA_Type DMA_Table[10]; + TickType_t LastWakeTime = xTaskGetTickCount(); // 时钟 + float interval = 0.002; // 任务运行间隔 s + int intervalms = interval * 1000; // 任务运行间隔 ms + extern DMA_Type DMA_Table[10]; - float positionFactor; + float positionFactor = 1.0 / 360 * 8192; - int8_t i = 0; + ProtocolInfo_Type *protocolInfo; while (1) { - i++; + // 校准 + protocolInfo = Protocol_Get_Info_Handle(0x407); + if (!JointCalibrated && protocolInfo->receiveCount > 0) { + JointCalibrated = 1; + // Motor_Calibrate(&Motor_BaseJoint, ProtocolData.jointCalibration.base_joint_position / positionFactor); + // Motor_Calibrate(&Motor_ShoulderJoint, ProtocolData.jointCalibration.shoulder_joint_position / positionFactor); + // Motor_Calibrate(&Motor_ElbowJoint, ProtocolData.jointCalibration.elbow_joint_position / positionFactor); + // PID_ElbowAngle.target = ProtocolData.jointCalibration.elbow_joint_position / positionFactor + PID_ElbowAngle.error; + Motor_Set_Angle_Bias(&Motor_WristJoint1, 92.724); + PID_WristJoint2Angle.target = ProtocolData.jointCalibration.wrist_joint_2_position / positionFactor; + Motor_Calibrate(&Motor_WristJoint2, ProtocolData.jointCalibration.wrist_joint_2_position / positionFactor); + } + DebugData.debug4 = protocolInfo->receiveSeq; + + // 设置目标 + protocolInfo = Protocol_Get_Info_Handle(0x406); + if (protocolInfo->receiveSeq != protocolInfo->lastReceiveSeq) { + // PID_BaseJointAngle.target = ProtocolData.jointControl.base_joint_position / positionFactor; + // PID_ShoulderJointAngle.target = ProtocolData.jointControl.shoulder_joint_position / positionFactor; + // PID_ElbowAngle.target = ProtocolData.jointControl.elbow_joint_position / positionFactor; + PID_WristJoint1Angle.target = ProtocolData.jointControl.wrist_joint_1_position / positionFactor; + // PID_WristJoint2Angle.target = ProtocolData.jointControl.wrist_joint_2_position / positionFactor; + protocolInfo->lastReceiveSeq = protocolInfo->receiveSeq; + } + // 修改数据 - positionFactor = 1.0 / 360 * 8192; ProtocolData.jointState.base_joint_position = Motor_BaseJoint.angle * positionFactor; ProtocolData.jointState.base_joint_speed = Motor_BaseJoint.speed; ProtocolData.jointState.shoulder_joint_position = Motor_ShoulderJoint.angle * positionFactor; @@ -121,34 +143,13 @@ void Task_Communication(void *Parameters) { ProtocolData.jointState.wrist_joint_1_speed = Motor_WristJoint1.speed; ProtocolData.jointState.wrist_joint_2_position = Motor_WristJoint2.angle * positionFactor; ProtocolData.jointState.wrist_joint_2_speed = Motor_WristJoint2.speed; - - // PID_WristJoint1Angle.target = ProtocolData.jointControl.wrist_joint_1_position / 8192.0 * 360; - // PID_WristJoint2Angle.target = ProtocolData.jointControl.wrist_joint_2_position / 8192.0 * 360; - - // PID_WristJoint1Speed.i = CHOOSEL(1, 2, 3) * CHOOSER(100, 1000, 0); - // PID_WristJoint1Speed.p = CHOOSEL(1, 4, 7) * CHOOSER(0.01, 0.1, 1); - // PID_WristJoint2Angle.p = CHOOSEL(1, 3, 10) * CHOOSER(0.1, 1, 10); - // PID_WristJoint1Speed.p = CHOOSER(4000, 5500, 7000); - - // 建议在main.c中使用Bridge_Send_Protocol - // Bridge_Send_Protocol_Once(&Node_Host, 0x501); + // 调试信息 + DebugData.debug1 = Motor_ElbowJoint.angle * 1000; + DebugData.debug2 = PID_WristJoint2Angle.target * 1000; + DebugData.debug3 = PID_WristJoint2Angle.feedback * 1000; // 发送频率 vTaskDelayUntil(&LastWakeTime, intervalms); - - // 调试信息 - // DebugData.debug1 = ProtocolData.jointControl.wrist_joint_1_position / 8192.0 * 360000; - // DebugData.debug2 = ProtocolData.jointControl.wrist_joint_2_position / 8192.0 * 360000; - // DebugData.debug1 = PID_ElbowSpeed.target * 1000; - // DebugData.debug2 = PID_ElbowSpeed.feedback * 1000; - // DebugData.debug3 = PID_ElbowSpeed.output; - // DebugData.debug2 = ProtocolData.boardAlpha.data2 * 1000; - // DebugData.debug3 = Node_Host.sendSeq; - // DebugData.debug3 = Node_Host.sendLock; - // DebugData.debug4 = protocolInfo->receiveCount; - DebugData.debug3 = ABS(remoteData.lx) > 100 ? (remoteData.lx / 660.0f * 45) : 0; - // DebugData.debug3 = PID_WristJoint1Angle.feedback; - DebugData.debug4 = PID_WristJoint2Angle.feedback; } vTaskDelete(NULL); } @@ -171,31 +172,37 @@ void Task_Manipulator(void *Parameters) { PID_Init(&PID_WristJoint1Angle, 0.3, 0, 0, 16384, 16384); PID_Init(&PID_WristJoint1Speed, 4000, 100, 0, 16384, 4000); PID_Init(&PID_WristJoint2Angle, 80, 0, 0, 400, 0); - PID_Init(&PID_WristJoint2Speed, 30, 0.75, 0, 15000, 3000); + PID_Init(&PID_WristJoint2Speed, 30, 0, 0, 15000, 3000); // 30, 0.75 + + JointCalibrated = 0; while (1) { - // PID_Calculate(&PID_BaseJointSpeed, PID_BaseJointSpeed.target, Motor_BaseJoint.speed * RPM2RPS); - // PID_Calculate(&PID_ShoulderJointSpeed, PID_ShoulderJointSpeed.target, Motor_ShoulderJoint.speed * RPM2RPS); - // PID_Calculate(&PID_ElbowSpeed, PID_ElbowSpeed.target, Motor_ElbowJoint.speed * RPM2RPS); - // PID_Calculate(&PID_WristJoint1Speed, PID_WristJoint1Speed.target, Motor_WristJoint1.speed * RPM2RPS); - // PID_Calculate(&PID_WristJoint2Speed, PID_WristJoint2Speed.target, Motor_WristJoint2.speed * RPM2RPS); - if (remoteData.lx > 100) { - PID_ElbowSpeed.target = remoteData.lx / 300 * 50; - } else if (remoteData.lx < -100) { - PID_ElbowSpeed.target = ((remoteData.lx / 300) - 1) * 50; - } else { - PID_ElbowSpeed.target = 0; - } + // 测试输入 + // if (remoteData.lx > 100) { + // PID_ElbowSpeed.target = remoteData.lx / 300 * 50; + // } else if (remoteData.lx < -100) { + // PID_ElbowSpeed.target = ((remoteData.lx / 300) - 1) * 50; + // } else { + // PID_ElbowSpeed.target = 0; + // } // PID_ElbowSpeed.target = ABS(remoteData.lx) > 100 ? remoteData.lx / 300 * 50 : 0; - PID_ElbowSpeed.p = CHOOSEL(7, 21, 70) * CHOOSER(0.1, 1, 10); + // PID_ElbowSpeed.p = CHOOSEL(7, 21, 70) * CHOOSER(0.1, 1, 10); // PID_WristJoint1Angle.target = ABS(remoteData.lx) > 100 ? remoteData.lx / 300 * 45 : PID_WristJoint1Angle.target; // PID_WristJoint2Angle.target = ABS(remoteData.ly) > 100 ? remoteData.ly / 300 * 15 : PID_WristJoint2Angle.target; // PID_WristJoint1Angle.target += ABS(remoteData.lx) > 100 ? (remoteData.lx / 660.0f * 45 * interval) : 0; // PID_WristJoint2Angle.target += ABS(remoteData.ly) > 100 ? (remoteData.ly / 660.0f * 15 * interval) : 0; + // 测试电流环 + // PID_Calculate(&PID_BaseJointSpeed, PID_BaseJointSpeed.target, Motor_BaseJoint.speed * RPM2RPS); + // PID_Calculate(&PID_ShoulderJointSpeed, PID_ShoulderJointSpeed.target, Motor_ShoulderJoint.speed * RPM2RPS); + // PID_Calculate(&PID_ElbowSpeed, PID_ElbowSpeed.target, Motor_ElbowJoint.speed * RPM2RPS); + // PID_Calculate(&PID_WristJoint1Speed, PID_WristJoint1Speed.target, Motor_WristJoint1.speed * RPM2RPS); + // PID_Calculate(&PID_WristJoint2Speed, PID_WristJoint2Speed.target, Motor_WristJoint2.speed * RPM2RPS); + + // 测试角度环 // PID_Calculate(&PID_BaseJointAngle, PID_BaseJointAngle.target, Motor_BaseJoint.angle); // PID_Calculate(&PID_BaseJointSpeed, PID_BaseJointAngle.output, Motor_BaseJoint.speed * RPM2RPS); // PID_Calculate(&PID_ShoulderJointAngle, PID_ShoulderJointAngle.target, Motor_ShoulderJoint.angle); @@ -206,7 +213,7 @@ void Task_Manipulator(void *Parameters) { PID_Calculate(&PID_WristJoint1Speed, PID_WristJoint1Angle.output, Motor_WristJoint1.speed * RPM2RPS); PID_Calculate(&PID_WristJoint2Angle, PID_WristJoint2Angle.target, Motor_WristJoint2.angle); if (ABS(remoteData.ry) > 100) { - PID_WristJoint2Angle.target = 0; + // PID_WristJoint2Angle.target = 0; PID_WristJoint2Angle.output = remoteData.ry / 330 * 5 * 30; Motor_WristJoint2.positionBias = Motor_WristJoint2.position; Motor_WristJoint2.round = 0; @@ -220,6 +227,11 @@ void Task_Manipulator(void *Parameters) { Motor_WristJoint1.input = PID_WristJoint1Speed.output; Motor_WristJoint2.input = PID_WristJoint2Speed.output; + // 调试信息 + // DebugData.debug1 = PID_ElbowSpeed.target * 1000; + // DebugData.debug2 = PID_ElbowSpeed.feedback * 1000; + // DebugData.debug3 = PID_ElbowSpeed.output; + vTaskDelayUntil(&LastWakeTime, intervalms); // 发送频率 } vTaskDelete(NULL); diff --git a/user/gold-miner/tasks.h b/user/gold-miner/tasks.h index dc63063b..07562945 100644 --- a/user/gold-miner/tasks.h +++ b/user/gold-miner/tasks.h @@ -2,6 +2,7 @@ #define __TASKS_H void Task_Control(void *Parameters); +void Task_Communication(void *Parameters); void Task_Manipulator(void *Parameters); void Task_Blink(void *Parameters); void Task_Chassis(void *Parameters); From d5aa9c942ff6c78858e450574997556948f4be5f Mon Sep 17 00:00:00 2001 From: odin1777 <947287884@qq.com> Date: Wed, 2 Jun 2021 17:58:02 +0800 Subject: [PATCH 10/10] offline motor will be disabled for 6 seconds --- driver/inc/Driver_Motor.h | 2 +- driver/src/Driver_Motor.c | 9 +++++++-- 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/driver/inc/Driver_Motor.h b/driver/inc/Driver_Motor.h index f7d22032..53f6192f 100644 --- a/driver/inc/Driver_Motor.h +++ b/driver/inc/Driver_Motor.h @@ -24,7 +24,7 @@ typedef struct { int16_t speed; // 转子转速值, RPM float actualCurrent; //电流 int16_t temperature; //温度 - uint16_t updatedAt; // 更新电机数据时的tickCount + uint32_t updatedAt; // 更新电机数据时的tickCount uint8_t online; // 电机是否离线 int8_t angleEnabled; // 连续角度启用 diff --git a/driver/src/Driver_Motor.c b/driver/src/Driver_Motor.c index cffad0ad..a531b0ec 100644 --- a/driver/src/Driver_Motor.c +++ b/driver/src/Driver_Motor.c @@ -7,6 +7,7 @@ void Motor_Init(Motor_Type *motor, float reductionRate, int8_t angleEnabled, int motor->positionBias = -1; // -1为未赋值状态 motor->angleBias = -1; // -1为未赋值状态 motor->lastPosition = -1; // -1为未赋值状态 + motor->online = 1; motor->reductionRate = reductionRate; motor->angleEnabled = angleEnabled; motor->inputEnabled = inputEnabled; @@ -18,8 +19,12 @@ void Motor_Update(Motor_Type *motor, uint8_t data[8]) { int16_t actualCurrent = data[4] << 8 | data[5]; int16_t temperature = data[6]; float angle = 0; - motor->updatedAt = xTaskGetTickCount(); - motor->online = 1; + if (motor->online) { + motor->updatedAt = xTaskGetTickCount(); + } else if (xTaskGetTickCount() - motor->updatedAt >= 6000) { + motor->updatedAt = xTaskGetTickCount(); + motor->online = 1; + } // 更新转子信息 motor->position = position;