From 0f6cf39492e23e20b74548e3e45297a9b4e4580b Mon Sep 17 00:00:00 2001 From: shentou Date: Mon, 22 Dec 2025 19:46:54 +0800 Subject: [PATCH] =?UTF-8?q?=E5=8A=A0=E4=BA=86=E4=B8=80=E5=A0=86=E4=B8=9C?= =?UTF-8?q?=E8=A5=BF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .gitattributes | 2 +- engineer/.mxproject | 24 +- engineer/CMakeLists.txt | 9 + engineer/Core/Inc/dma.h | 52 ++ engineer/Core/Inc/stm32f4xx_hal_conf.h | 2 +- engineer/Core/Inc/stm32f4xx_it.h | 2 + engineer/Core/Inc/usart.h | 55 ++ engineer/Core/Src/dma.c | 65 ++ engineer/Core/Src/gpio.c | 1 + engineer/Core/Src/main.c | 3 + engineer/Core/Src/stm32f4xx_it.c | 30 + engineer/Core/Src/usart.c | 196 ++++++ engineer/DM.ioc | 36 +- engineer/User/bsp/uart.c | 159 +++++ engineer/User/bsp/uart.h | 69 ++ engineer/User/component/cmd.c | 387 +++++++++++ engineer/User/component/cmd.h | 342 ++++++++++ engineer/User/component/crc16.c | 62 ++ engineer/User/component/crc16.h | 30 + engineer/User/component/crc8.c | 52 ++ engineer/User/component/crc8.h | 30 + engineer/User/component/limiter.c | 107 +++ engineer/User/component/limiter.h | 63 ++ engineer/User/component/matrix.cpp | 24 + engineer/User/component/matrix.hpp | 259 +++++++ engineer/User/component/robotics.cpp | 340 ++++++++++ engineer/User/component/robotics.hpp | 407 +++++++++++ engineer/User/component/ui.c | 301 +++++++++ engineer/User/component/ui.h | 284 ++++++++ engineer/User/component/utils.cpp | 56 ++ engineer/User/component/utils.hpp | 27 + engineer/User/device/referee.c | 788 ++++++++++++++++++++++ engineer/User/device/referee.h | 508 ++++++++++++++ engineer/User/module/arm.c | 26 +- engineer/User/module/arm.h | 12 +- engineer/User/task/control.c | 5 +- engineer/User/task/init.c | 3 +- engineer/cmake/stm32cubemx/CMakeLists.txt | 4 +- 38 files changed, 4785 insertions(+), 37 deletions(-) create mode 100644 engineer/Core/Inc/dma.h create mode 100644 engineer/Core/Inc/usart.h create mode 100644 engineer/Core/Src/dma.c create mode 100644 engineer/Core/Src/usart.c create mode 100644 engineer/User/bsp/uart.c create mode 100644 engineer/User/bsp/uart.h create mode 100644 engineer/User/component/cmd.c create mode 100644 engineer/User/component/cmd.h create mode 100644 engineer/User/component/crc16.c create mode 100644 engineer/User/component/crc16.h create mode 100644 engineer/User/component/crc8.c create mode 100644 engineer/User/component/crc8.h create mode 100644 engineer/User/component/limiter.c create mode 100644 engineer/User/component/limiter.h create mode 100644 engineer/User/component/matrix.cpp create mode 100644 engineer/User/component/matrix.hpp create mode 100644 engineer/User/component/robotics.cpp create mode 100644 engineer/User/component/robotics.hpp create mode 100644 engineer/User/component/ui.c create mode 100644 engineer/User/component/ui.h create mode 100644 engineer/User/component/utils.cpp create mode 100644 engineer/User/component/utils.hpp create mode 100644 engineer/User/device/referee.c create mode 100644 engineer/User/device/referee.h diff --git a/.gitattributes b/.gitattributes index 176a458..6313b56 100644 --- a/.gitattributes +++ b/.gitattributes @@ -1 +1 @@ -* text=auto +* text=auto eol=lf diff --git a/engineer/.mxproject b/engineer/.mxproject index 5de9aa2..3af4ef5 100644 --- a/engineer/.mxproject +++ b/engineer/.mxproject @@ -1,31 +1,33 @@ [PreviousLibFiles] -LibFiles=Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_tim.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_tim_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_can.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_rcc.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_rcc_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_bus.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_rcc.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_system.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_utils.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_flash.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_flash_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_flash_ramfunc.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_gpio.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_gpio_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_gpio.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_dma_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_dma.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_dma.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_dmamux.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_pwr.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_pwr_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_pwr.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_cortex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_cortex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal.h;Drivers/STM32F4xx_HAL_Driver/Inc/Legacy/stm32_hal_legacy.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_def.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_exti.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_exti.h;Middlewares/Third_Party/FreeRTOS/Source/include/croutine.h;Middlewares/Third_Party/FreeRTOS/Source/include/deprecated_definitions.h;Middlewares/Third_Party/FreeRTOS/Source/include/event_groups.h;Middlewares/Third_Party/FreeRTOS/Source/include/FreeRTOS.h;Middlewares/Third_Party/FreeRTOS/Source/include/list.h;Middlewares/Third_Party/FreeRTOS/Source/include/message_buffer.h;Middlewares/Third_Party/FreeRTOS/Source/include/mpu_prototypes.h;Middlewares/Third_Party/FreeRTOS/Source/include/mpu_wrappers.h;Middlewares/Third_Party/FreeRTOS/Source/include/portable.h;Middlewares/Third_Party/FreeRTOS/Source/include/projdefs.h;Middlewares/Third_Party/FreeRTOS/Source/include/queue.h;Middlewares/Third_Party/FreeRTOS/Source/include/semphr.h;Middlewares/Third_Party/FreeRTOS/Source/include/stack_macros.h;Middlewares/Third_Party/FreeRTOS/Source/include/StackMacros.h;Middlewares/Third_Party/FreeRTOS/Source/include/stream_buffer.h;Middlewares/Third_Party/FreeRTOS/Source/include/task.h;Middlewares/Third_Party/FreeRTOS/Source/include/timers.h;Middlewares/Third_Party/FreeRTOS/Source/include/atomic.h;Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/cmsis_os2.h;Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/cmsis_os.h;Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/freertos_mpool.h;Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/freertos_os2.h;Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/portmacro.h;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_tim.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_tim_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_can.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rcc.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rcc_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash_ramfunc.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_gpio.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_dma_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_dma.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pwr.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pwr_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_cortex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_exti.c;Middlewares/Third_Party/FreeRTOS/Source/croutine.c;Middlewares/Third_Party/FreeRTOS/Source/event_groups.c;Middlewares/Third_Party/FreeRTOS/Source/list.c;Middlewares/Third_Party/FreeRTOS/Source/queue.c;Middlewares/Third_Party/FreeRTOS/Source/stream_buffer.c;Middlewares/Third_Party/FreeRTOS/Source/tasks.c;Middlewares/Third_Party/FreeRTOS/Source/timers.c;Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/cmsis_os2.c;Middlewares/Third_Party/FreeRTOS/Source/portable/MemMang/heap_4.c;Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/port.c;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_tim.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_tim_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_can.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_rcc.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_rcc_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_bus.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_rcc.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_system.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_utils.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_flash.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_flash_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_flash_ramfunc.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_gpio.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_gpio_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_gpio.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_dma_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_dma.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_dma.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_dmamux.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_pwr.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_pwr_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_pwr.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_cortex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_cortex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal.h;Drivers/STM32F4xx_HAL_Driver/Inc/Legacy/stm32_hal_legacy.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_def.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_exti.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_exti.h;Middlewares/Third_Party/FreeRTOS/Source/include/croutine.h;Middlewares/Third_Party/FreeRTOS/Source/include/deprecated_definitions.h;Middlewares/Third_Party/FreeRTOS/Source/include/event_groups.h;Middlewares/Third_Party/FreeRTOS/Source/include/FreeRTOS.h;Middlewares/Third_Party/FreeRTOS/Source/include/list.h;Middlewares/Third_Party/FreeRTOS/Source/include/message_buffer.h;Middlewares/Third_Party/FreeRTOS/Source/include/mpu_prototypes.h;Middlewares/Third_Party/FreeRTOS/Source/include/mpu_wrappers.h;Middlewares/Third_Party/FreeRTOS/Source/include/portable.h;Middlewares/Third_Party/FreeRTOS/Source/include/projdefs.h;Middlewares/Third_Party/FreeRTOS/Source/include/queue.h;Middlewares/Third_Party/FreeRTOS/Source/include/semphr.h;Middlewares/Third_Party/FreeRTOS/Source/include/stack_macros.h;Middlewares/Third_Party/FreeRTOS/Source/include/StackMacros.h;Middlewares/Third_Party/FreeRTOS/Source/include/stream_buffer.h;Middlewares/Third_Party/FreeRTOS/Source/include/task.h;Middlewares/Third_Party/FreeRTOS/Source/include/timers.h;Middlewares/Third_Party/FreeRTOS/Source/include/atomic.h;Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/cmsis_os2.h;Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/cmsis_os.h;Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/freertos_mpool.h;Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/freertos_os2.h;Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/portmacro.h;Drivers/CMSIS/Device/ST/STM32F4xx/Include/stm32f407xx.h;Drivers/CMSIS/Device/ST/STM32F4xx/Include/stm32f4xx.h;Drivers/CMSIS/Device/ST/STM32F4xx/Include/system_stm32f4xx.h;Drivers/CMSIS/Device/ST/STM32F4xx/Include/system_stm32f4xx.h;Drivers/CMSIS/Device/ST/STM32F4xx/Source/Templates/system_stm32f4xx.c;Drivers/CMSIS/Include/core_cm4.h;Drivers/CMSIS/Include/core_cm85.h;Drivers/CMSIS/Include/cmsis_version.h;Drivers/CMSIS/Include/core_cm33.h;Drivers/CMSIS/Include/core_cm0.h;Drivers/CMSIS/Include/cmsis_gcc.h;Drivers/CMSIS/Include/core_cm55.h;Drivers/CMSIS/Include/mpu_armv8.h;Drivers/CMSIS/Include/core_cm0plus.h;Drivers/CMSIS/Include/core_starmc1.h;Drivers/CMSIS/Include/cmsis_iccarm.h;Drivers/CMSIS/Include/core_cm3.h;Drivers/CMSIS/Include/cmsis_armclang.h;Drivers/CMSIS/Include/core_sc000.h;Drivers/CMSIS/Include/core_armv81mml.h;Drivers/CMSIS/Include/core_cm1.h;Drivers/CMSIS/Include/cmsis_compiler.h;Drivers/CMSIS/Include/cmsis_armclang_ltm.h;Drivers/CMSIS/Include/core_cm35p.h;Drivers/CMSIS/Include/core_armv8mbl.h;Drivers/CMSIS/Include/mpu_armv7.h;Drivers/CMSIS/Include/cachel1_armv7.h;Drivers/CMSIS/Include/core_cm7.h;Drivers/CMSIS/Include/core_sc300.h;Drivers/CMSIS/Include/core_cm23.h;Drivers/CMSIS/Include/tz_context.h;Drivers/CMSIS/Include/pmu_armv8.h;Drivers/CMSIS/Include/cmsis_armcc.h;Drivers/CMSIS/Include/core_armv8mml.h;Drivers/CMSIS/Include/pac_armv81.h; +LibFiles=Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_tim.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_tim_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_can.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_rcc.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_rcc_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_bus.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_rcc.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_system.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_utils.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_flash.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_flash_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_flash_ramfunc.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_gpio.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_gpio_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_gpio.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_dma_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_dma.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_dma.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_dmamux.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_pwr.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_pwr_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_pwr.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_cortex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_cortex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal.h;Drivers/STM32F4xx_HAL_Driver/Inc/Legacy/stm32_hal_legacy.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_def.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_exti.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_exti.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_uart.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_usart.h;Middlewares/Third_Party/FreeRTOS/Source/include/croutine.h;Middlewares/Third_Party/FreeRTOS/Source/include/deprecated_definitions.h;Middlewares/Third_Party/FreeRTOS/Source/include/event_groups.h;Middlewares/Third_Party/FreeRTOS/Source/include/FreeRTOS.h;Middlewares/Third_Party/FreeRTOS/Source/include/list.h;Middlewares/Third_Party/FreeRTOS/Source/include/message_buffer.h;Middlewares/Third_Party/FreeRTOS/Source/include/mpu_prototypes.h;Middlewares/Third_Party/FreeRTOS/Source/include/mpu_wrappers.h;Middlewares/Third_Party/FreeRTOS/Source/include/portable.h;Middlewares/Third_Party/FreeRTOS/Source/include/projdefs.h;Middlewares/Third_Party/FreeRTOS/Source/include/queue.h;Middlewares/Third_Party/FreeRTOS/Source/include/semphr.h;Middlewares/Third_Party/FreeRTOS/Source/include/stack_macros.h;Middlewares/Third_Party/FreeRTOS/Source/include/StackMacros.h;Middlewares/Third_Party/FreeRTOS/Source/include/stream_buffer.h;Middlewares/Third_Party/FreeRTOS/Source/include/task.h;Middlewares/Third_Party/FreeRTOS/Source/include/timers.h;Middlewares/Third_Party/FreeRTOS/Source/include/atomic.h;Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/cmsis_os2.h;Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/cmsis_os.h;Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/freertos_mpool.h;Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/freertos_os2.h;Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/portmacro.h;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_tim.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_tim_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_can.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rcc.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rcc_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash_ramfunc.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_gpio.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_dma_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_dma.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pwr.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pwr_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_cortex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_exti.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_uart.c;Middlewares/Third_Party/FreeRTOS/Source/croutine.c;Middlewares/Third_Party/FreeRTOS/Source/event_groups.c;Middlewares/Third_Party/FreeRTOS/Source/list.c;Middlewares/Third_Party/FreeRTOS/Source/queue.c;Middlewares/Third_Party/FreeRTOS/Source/stream_buffer.c;Middlewares/Third_Party/FreeRTOS/Source/tasks.c;Middlewares/Third_Party/FreeRTOS/Source/timers.c;Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/cmsis_os2.c;Middlewares/Third_Party/FreeRTOS/Source/portable/MemMang/heap_4.c;Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/port.c;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_tim.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_tim_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_can.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_rcc.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_rcc_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_bus.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_rcc.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_system.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_utils.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_flash.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_flash_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_flash_ramfunc.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_gpio.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_gpio_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_gpio.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_dma_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_dma.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_dma.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_dmamux.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_pwr.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_pwr_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_pwr.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_cortex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_cortex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal.h;Drivers/STM32F4xx_HAL_Driver/Inc/Legacy/stm32_hal_legacy.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_def.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_exti.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_exti.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_uart.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_usart.h;Middlewares/Third_Party/FreeRTOS/Source/include/croutine.h;Middlewares/Third_Party/FreeRTOS/Source/include/deprecated_definitions.h;Middlewares/Third_Party/FreeRTOS/Source/include/event_groups.h;Middlewares/Third_Party/FreeRTOS/Source/include/FreeRTOS.h;Middlewares/Third_Party/FreeRTOS/Source/include/list.h;Middlewares/Third_Party/FreeRTOS/Source/include/message_buffer.h;Middlewares/Third_Party/FreeRTOS/Source/include/mpu_prototypes.h;Middlewares/Third_Party/FreeRTOS/Source/include/mpu_wrappers.h;Middlewares/Third_Party/FreeRTOS/Source/include/portable.h;Middlewares/Third_Party/FreeRTOS/Source/include/projdefs.h;Middlewares/Third_Party/FreeRTOS/Source/include/queue.h;Middlewares/Third_Party/FreeRTOS/Source/include/semphr.h;Middlewares/Third_Party/FreeRTOS/Source/include/stack_macros.h;Middlewares/Third_Party/FreeRTOS/Source/include/StackMacros.h;Middlewares/Third_Party/FreeRTOS/Source/include/stream_buffer.h;Middlewares/Third_Party/FreeRTOS/Source/include/task.h;Middlewares/Third_Party/FreeRTOS/Source/include/timers.h;Middlewares/Third_Party/FreeRTOS/Source/include/atomic.h;Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/cmsis_os2.h;Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/cmsis_os.h;Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/freertos_mpool.h;Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/freertos_os2.h;Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/portmacro.h;Drivers/CMSIS/Device/ST/STM32F4xx/Include/stm32f407xx.h;Drivers/CMSIS/Device/ST/STM32F4xx/Include/stm32f4xx.h;Drivers/CMSIS/Device/ST/STM32F4xx/Include/system_stm32f4xx.h;Drivers/CMSIS/Device/ST/STM32F4xx/Include/system_stm32f4xx.h;Drivers/CMSIS/Device/ST/STM32F4xx/Source/Templates/system_stm32f4xx.c;Drivers/CMSIS/Include/core_cm4.h;Drivers/CMSIS/Include/core_cm85.h;Drivers/CMSIS/Include/cmsis_version.h;Drivers/CMSIS/Include/core_cm33.h;Drivers/CMSIS/Include/core_cm0.h;Drivers/CMSIS/Include/cmsis_gcc.h;Drivers/CMSIS/Include/core_cm55.h;Drivers/CMSIS/Include/mpu_armv8.h;Drivers/CMSIS/Include/core_cm0plus.h;Drivers/CMSIS/Include/core_starmc1.h;Drivers/CMSIS/Include/cmsis_iccarm.h;Drivers/CMSIS/Include/core_cm3.h;Drivers/CMSIS/Include/cmsis_armclang.h;Drivers/CMSIS/Include/core_sc000.h;Drivers/CMSIS/Include/core_armv81mml.h;Drivers/CMSIS/Include/core_cm1.h;Drivers/CMSIS/Include/cmsis_compiler.h;Drivers/CMSIS/Include/cmsis_armclang_ltm.h;Drivers/CMSIS/Include/core_cm35p.h;Drivers/CMSIS/Include/core_armv8mbl.h;Drivers/CMSIS/Include/mpu_armv7.h;Drivers/CMSIS/Include/cachel1_armv7.h;Drivers/CMSIS/Include/core_cm7.h;Drivers/CMSIS/Include/core_sc300.h;Drivers/CMSIS/Include/core_cm23.h;Drivers/CMSIS/Include/tz_context.h;Drivers/CMSIS/Include/pmu_armv8.h;Drivers/CMSIS/Include/cmsis_armcc.h;Drivers/CMSIS/Include/core_armv8mml.h;Drivers/CMSIS/Include/pac_armv81.h; [PreviousUsedCMakes] -SourceFiles=Core/Src/main.c;Core/Src/gpio.c;Core/Src/freertos.c;Core/Src/can.c;Core/Src/stm32f4xx_it.c;Core/Src/stm32f4xx_hal_msp.c;Core/Src/stm32f4xx_hal_timebase_tim.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_tim.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_tim_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_can.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rcc.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rcc_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash_ramfunc.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_gpio.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_dma_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_dma.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pwr.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pwr_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_cortex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_exti.c;Middlewares/Third_Party/FreeRTOS/Source/croutine.c;Middlewares/Third_Party/FreeRTOS/Source/event_groups.c;Middlewares/Third_Party/FreeRTOS/Source/list.c;Middlewares/Third_Party/FreeRTOS/Source/queue.c;Middlewares/Third_Party/FreeRTOS/Source/stream_buffer.c;Middlewares/Third_Party/FreeRTOS/Source/tasks.c;Middlewares/Third_Party/FreeRTOS/Source/timers.c;Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/cmsis_os2.c;Middlewares/Third_Party/FreeRTOS/Source/portable/MemMang/heap_4.c;Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/port.c;Drivers/CMSIS/Device/ST/STM32F4xx/Source/Templates/system_stm32f4xx.c;Core/Src/system_stm32f4xx.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_tim.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_tim_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_can.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rcc.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rcc_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash_ramfunc.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_gpio.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_dma_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_dma.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pwr.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pwr_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_cortex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_exti.c;Middlewares/Third_Party/FreeRTOS/Source/croutine.c;Middlewares/Third_Party/FreeRTOS/Source/event_groups.c;Middlewares/Third_Party/FreeRTOS/Source/list.c;Middlewares/Third_Party/FreeRTOS/Source/queue.c;Middlewares/Third_Party/FreeRTOS/Source/stream_buffer.c;Middlewares/Third_Party/FreeRTOS/Source/tasks.c;Middlewares/Third_Party/FreeRTOS/Source/timers.c;Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/cmsis_os2.c;Middlewares/Third_Party/FreeRTOS/Source/portable/MemMang/heap_4.c;Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/port.c;Drivers/CMSIS/Device/ST/STM32F4xx/Source/Templates/system_stm32f4xx.c;Core/Src/system_stm32f4xx.c;;;Middlewares/Third_Party/FreeRTOS/Source/croutine.c;Middlewares/Third_Party/FreeRTOS/Source/event_groups.c;Middlewares/Third_Party/FreeRTOS/Source/list.c;Middlewares/Third_Party/FreeRTOS/Source/queue.c;Middlewares/Third_Party/FreeRTOS/Source/stream_buffer.c;Middlewares/Third_Party/FreeRTOS/Source/tasks.c;Middlewares/Third_Party/FreeRTOS/Source/timers.c;Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/cmsis_os2.c;Middlewares/Third_Party/FreeRTOS/Source/portable/MemMang/heap_4.c;Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/port.c; +SourceFiles=Core/Src/main.c;Core/Src/gpio.c;Core/Src/freertos.c;Core/Src/can.c;Core/Src/usart.c;Core/Src/stm32f4xx_it.c;Core/Src/stm32f4xx_hal_msp.c;Core/Src/stm32f4xx_hal_timebase_tim.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_tim.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_tim_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_can.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rcc.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rcc_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash_ramfunc.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_gpio.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_dma_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_dma.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pwr.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pwr_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_cortex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_exti.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_uart.c;Middlewares/Third_Party/FreeRTOS/Source/croutine.c;Middlewares/Third_Party/FreeRTOS/Source/event_groups.c;Middlewares/Third_Party/FreeRTOS/Source/list.c;Middlewares/Third_Party/FreeRTOS/Source/queue.c;Middlewares/Third_Party/FreeRTOS/Source/stream_buffer.c;Middlewares/Third_Party/FreeRTOS/Source/tasks.c;Middlewares/Third_Party/FreeRTOS/Source/timers.c;Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/cmsis_os2.c;Middlewares/Third_Party/FreeRTOS/Source/portable/MemMang/heap_4.c;Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/port.c;Drivers/CMSIS/Device/ST/STM32F4xx/Source/Templates/system_stm32f4xx.c;Core/Src/system_stm32f4xx.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_tim.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_tim_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_can.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rcc.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rcc_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash_ramfunc.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_gpio.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_dma_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_dma.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pwr.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pwr_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_cortex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_exti.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_uart.c;Middlewares/Third_Party/FreeRTOS/Source/croutine.c;Middlewares/Third_Party/FreeRTOS/Source/event_groups.c;Middlewares/Third_Party/FreeRTOS/Source/list.c;Middlewares/Third_Party/FreeRTOS/Source/queue.c;Middlewares/Third_Party/FreeRTOS/Source/stream_buffer.c;Middlewares/Third_Party/FreeRTOS/Source/tasks.c;Middlewares/Third_Party/FreeRTOS/Source/timers.c;Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/cmsis_os2.c;Middlewares/Third_Party/FreeRTOS/Source/portable/MemMang/heap_4.c;Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/port.c;Drivers/CMSIS/Device/ST/STM32F4xx/Source/Templates/system_stm32f4xx.c;Core/Src/system_stm32f4xx.c;;;Middlewares/Third_Party/FreeRTOS/Source/croutine.c;Middlewares/Third_Party/FreeRTOS/Source/event_groups.c;Middlewares/Third_Party/FreeRTOS/Source/list.c;Middlewares/Third_Party/FreeRTOS/Source/queue.c;Middlewares/Third_Party/FreeRTOS/Source/stream_buffer.c;Middlewares/Third_Party/FreeRTOS/Source/tasks.c;Middlewares/Third_Party/FreeRTOS/Source/timers.c;Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/cmsis_os2.c;Middlewares/Third_Party/FreeRTOS/Source/portable/MemMang/heap_4.c;Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/port.c; HeaderPath=Drivers/STM32F4xx_HAL_Driver/Inc;Drivers/STM32F4xx_HAL_Driver/Inc/Legacy;Middlewares/Third_Party/FreeRTOS/Source/include;Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2;Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F;Drivers/CMSIS/Device/ST/STM32F4xx/Include;Drivers/CMSIS/Include;Core/Inc; CDefines=USE_HAL_DRIVER;STM32F407xx;USE_HAL_DRIVER;USE_HAL_DRIVER; [PreviousGenFiles] AdvancedFolderStructure=true -HeaderFileListSize=6 +HeaderFileListSize=7 HeaderFiles#0=../Core/Inc/gpio.h HeaderFiles#1=../Core/Inc/FreeRTOSConfig.h HeaderFiles#2=../Core/Inc/can.h -HeaderFiles#3=../Core/Inc/stm32f4xx_it.h -HeaderFiles#4=../Core/Inc/stm32f4xx_hal_conf.h -HeaderFiles#5=../Core/Inc/main.h +HeaderFiles#3=../Core/Inc/usart.h +HeaderFiles#4=../Core/Inc/stm32f4xx_it.h +HeaderFiles#5=../Core/Inc/stm32f4xx_hal_conf.h +HeaderFiles#6=../Core/Inc/main.h HeaderFolderListSize=1 HeaderPath#0=../Core/Inc HeaderFiles=; -SourceFileListSize=7 +SourceFileListSize=8 SourceFiles#0=../Core/Src/gpio.c SourceFiles#1=../Core/Src/freertos.c SourceFiles#2=../Core/Src/can.c -SourceFiles#3=../Core/Src/stm32f4xx_it.c -SourceFiles#4=../Core/Src/stm32f4xx_hal_msp.c -SourceFiles#5=../Core/Src/stm32f4xx_hal_timebase_tim.c -SourceFiles#6=../Core/Src/main.c +SourceFiles#3=../Core/Src/usart.c +SourceFiles#4=../Core/Src/stm32f4xx_it.c +SourceFiles#5=../Core/Src/stm32f4xx_hal_msp.c +SourceFiles#6=../Core/Src/stm32f4xx_hal_timebase_tim.c +SourceFiles#7=../Core/Src/main.c SourceFolderListSize=1 SourcePath#0=../Core/Src SourceFiles=; diff --git a/engineer/CMakeLists.txt b/engineer/CMakeLists.txt index 7b92b00..0c06234 100644 --- a/engineer/CMakeLists.txt +++ b/engineer/CMakeLists.txt @@ -50,6 +50,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE User/bsp/mm.c User/bsp/time.c User/bsp/dwt.c + ./User/bsp/uart.c # User/component sources User/component/ahrs.c @@ -58,6 +59,14 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE User/component/pid.c User/component/user_math.c ./User/component/mixer.c + ./User/component/cmd.c + ./User/component/crc16.c + ./User/component/crc8.c + ./User/component/limiter.c + ./User/component/matrix.cpp + ./User/component/robotics.cpp + ./User/component/utils.cpp + # User/device sources User/device/motor.c diff --git a/engineer/Core/Inc/dma.h b/engineer/Core/Inc/dma.h new file mode 100644 index 0000000..493d98e --- /dev/null +++ b/engineer/Core/Inc/dma.h @@ -0,0 +1,52 @@ +/* USER CODE BEGIN Header */ +/** + ****************************************************************************** + * @file dma.h + * @brief This file contains all the function prototypes for + * the dma.c file + ****************************************************************************** + * @attention + * + * Copyright (c) 2025 STMicroelectronics. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is provided AS-IS. + * + ****************************************************************************** + */ +/* USER CODE END Header */ +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __DMA_H__ +#define __DMA_H__ + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ------------------------------------------------------------------*/ +#include "main.h" + +/* DMA memory to memory transfer handles -------------------------------------*/ + +/* USER CODE BEGIN Includes */ + +/* USER CODE END Includes */ + +/* USER CODE BEGIN Private defines */ + +/* USER CODE END Private defines */ + +void MX_DMA_Init(void); + +/* USER CODE BEGIN Prototypes */ + +/* USER CODE END Prototypes */ + +#ifdef __cplusplus +} +#endif + +#endif /* __DMA_H__ */ + diff --git a/engineer/Core/Inc/stm32f4xx_hal_conf.h b/engineer/Core/Inc/stm32f4xx_hal_conf.h index 4201d81..c6b1561 100644 --- a/engineer/Core/Inc/stm32f4xx_hal_conf.h +++ b/engineer/Core/Inc/stm32f4xx_hal_conf.h @@ -64,7 +64,7 @@ /* #define HAL_MMC_MODULE_ENABLED */ /* #define HAL_SPI_MODULE_ENABLED */ #define HAL_TIM_MODULE_ENABLED -/* #define HAL_UART_MODULE_ENABLED */ +#define HAL_UART_MODULE_ENABLED /* #define HAL_USART_MODULE_ENABLED */ /* #define HAL_IRDA_MODULE_ENABLED */ /* #define HAL_SMARTCARD_MODULE_ENABLED */ diff --git a/engineer/Core/Inc/stm32f4xx_it.h b/engineer/Core/Inc/stm32f4xx_it.h index 10d917e..0a42700 100644 --- a/engineer/Core/Inc/stm32f4xx_it.h +++ b/engineer/Core/Inc/stm32f4xx_it.h @@ -55,6 +55,8 @@ void DebugMon_Handler(void); void CAN1_TX_IRQHandler(void); void CAN1_RX0_IRQHandler(void); void CAN1_RX1_IRQHandler(void); +void USART1_IRQHandler(void); +void USART2_IRQHandler(void); void TIM8_TRG_COM_TIM14_IRQHandler(void); /* USER CODE BEGIN EFP */ diff --git a/engineer/Core/Inc/usart.h b/engineer/Core/Inc/usart.h new file mode 100644 index 0000000..737091e --- /dev/null +++ b/engineer/Core/Inc/usart.h @@ -0,0 +1,55 @@ +/* USER CODE BEGIN Header */ +/** + ****************************************************************************** + * @file usart.h + * @brief This file contains all the function prototypes for + * the usart.c file + ****************************************************************************** + * @attention + * + * Copyright (c) 2025 STMicroelectronics. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is provided AS-IS. + * + ****************************************************************************** + */ +/* USER CODE END Header */ +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __USART_H__ +#define __USART_H__ + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ------------------------------------------------------------------*/ +#include "main.h" + +/* USER CODE BEGIN Includes */ + +/* USER CODE END Includes */ + +extern UART_HandleTypeDef huart1; + +extern UART_HandleTypeDef huart2; + +/* USER CODE BEGIN Private defines */ + +/* USER CODE END Private defines */ + +void MX_USART1_UART_Init(void); +void MX_USART2_UART_Init(void); + +/* USER CODE BEGIN Prototypes */ + +/* USER CODE END Prototypes */ + +#ifdef __cplusplus +} +#endif + +#endif /* __USART_H__ */ + diff --git a/engineer/Core/Src/dma.c b/engineer/Core/Src/dma.c new file mode 100644 index 0000000..da649ed --- /dev/null +++ b/engineer/Core/Src/dma.c @@ -0,0 +1,65 @@ +/* USER CODE BEGIN Header */ +/** + ****************************************************************************** + * @file dma.c + * @brief This file provides code for the configuration + * of all the requested memory to memory DMA transfers. + ****************************************************************************** + * @attention + * + * Copyright (c) 2025 STMicroelectronics. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is provided AS-IS. + * + ****************************************************************************** + */ +/* USER CODE END Header */ + +/* Includes ------------------------------------------------------------------*/ +#include "dma.h" + +/* USER CODE BEGIN 0 */ + +/* USER CODE END 0 */ + +/*----------------------------------------------------------------------------*/ +/* Configure DMA */ +/*----------------------------------------------------------------------------*/ + +/* USER CODE BEGIN 1 */ + +/* USER CODE END 1 */ + +/** + * Enable DMA controller clock + */ +void MX_DMA_Init(void) +{ + + /* DMA controller clock enable */ + __HAL_RCC_DMA1_CLK_ENABLE(); + __HAL_RCC_DMA2_CLK_ENABLE(); + + /* DMA interrupt init */ + /* DMA1_Stream5_IRQn interrupt configuration */ + HAL_NVIC_SetPriority(DMA1_Stream5_IRQn, 5, 0); + HAL_NVIC_EnableIRQ(DMA1_Stream5_IRQn); + /* DMA1_Stream6_IRQn interrupt configuration */ + HAL_NVIC_SetPriority(DMA1_Stream6_IRQn, 5, 0); + HAL_NVIC_EnableIRQ(DMA1_Stream6_IRQn); + /* DMA2_Stream2_IRQn interrupt configuration */ + HAL_NVIC_SetPriority(DMA2_Stream2_IRQn, 5, 0); + HAL_NVIC_EnableIRQ(DMA2_Stream2_IRQn); + /* DMA2_Stream7_IRQn interrupt configuration */ + HAL_NVIC_SetPriority(DMA2_Stream7_IRQn, 5, 0); + HAL_NVIC_EnableIRQ(DMA2_Stream7_IRQn); + +} + +/* USER CODE BEGIN 2 */ + +/* USER CODE END 2 */ + diff --git a/engineer/Core/Src/gpio.c b/engineer/Core/Src/gpio.c index c775b47..4b3264f 100644 --- a/engineer/Core/Src/gpio.c +++ b/engineer/Core/Src/gpio.c @@ -44,6 +44,7 @@ void MX_GPIO_Init(void) /* GPIO Ports Clock Enable */ __HAL_RCC_GPIOA_CLK_ENABLE(); + __HAL_RCC_GPIOB_CLK_ENABLE(); __HAL_RCC_GPIOD_CLK_ENABLE(); __HAL_RCC_GPIOH_CLK_ENABLE(); diff --git a/engineer/Core/Src/main.c b/engineer/Core/Src/main.c index a1305ad..f1fee41 100644 --- a/engineer/Core/Src/main.c +++ b/engineer/Core/Src/main.c @@ -20,6 +20,7 @@ #include "main.h" #include "cmsis_os.h" #include "can.h" +#include "usart.h" #include "gpio.h" /* Private includes ----------------------------------------------------------*/ @@ -90,6 +91,8 @@ int main(void) /* Initialize all configured peripherals */ MX_GPIO_Init(); MX_CAN1_Init(); + MX_USART1_UART_Init(); + MX_USART2_UART_Init(); /* USER CODE BEGIN 2 */ /* USER CODE END 2 */ diff --git a/engineer/Core/Src/stm32f4xx_it.c b/engineer/Core/Src/stm32f4xx_it.c index 1ca151c..3e5ecff 100644 --- a/engineer/Core/Src/stm32f4xx_it.c +++ b/engineer/Core/Src/stm32f4xx_it.c @@ -56,6 +56,8 @@ /* External variables --------------------------------------------------------*/ extern CAN_HandleTypeDef hcan1; +extern UART_HandleTypeDef huart1; +extern UART_HandleTypeDef huart2; extern TIM_HandleTypeDef htim14; /* USER CODE BEGIN EV */ @@ -202,6 +204,34 @@ void CAN1_RX1_IRQHandler(void) /* USER CODE END CAN1_RX1_IRQn 1 */ } +/** + * @brief This function handles USART1 global interrupt. + */ +void USART1_IRQHandler(void) +{ + /* USER CODE BEGIN USART1_IRQn 0 */ + + /* USER CODE END USART1_IRQn 0 */ + HAL_UART_IRQHandler(&huart1); + /* USER CODE BEGIN USART1_IRQn 1 */ + + /* USER CODE END USART1_IRQn 1 */ +} + +/** + * @brief This function handles USART2 global interrupt. + */ +void USART2_IRQHandler(void) +{ + /* USER CODE BEGIN USART2_IRQn 0 */ + + /* USER CODE END USART2_IRQn 0 */ + HAL_UART_IRQHandler(&huart2); + /* USER CODE BEGIN USART2_IRQn 1 */ + + /* USER CODE END USART2_IRQn 1 */ +} + /** * @brief This function handles TIM8 trigger and commutation interrupts and TIM14 global interrupt. */ diff --git a/engineer/Core/Src/usart.c b/engineer/Core/Src/usart.c new file mode 100644 index 0000000..37ad4b8 --- /dev/null +++ b/engineer/Core/Src/usart.c @@ -0,0 +1,196 @@ +/* USER CODE BEGIN Header */ +/** + ****************************************************************************** + * @file usart.c + * @brief This file provides code for the configuration + * of the USART instances. + ****************************************************************************** + * @attention + * + * Copyright (c) 2025 STMicroelectronics. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is provided AS-IS. + * + ****************************************************************************** + */ +/* USER CODE END Header */ +/* Includes ------------------------------------------------------------------*/ +#include "usart.h" + +/* USER CODE BEGIN 0 */ + +/* USER CODE END 0 */ + +UART_HandleTypeDef huart1; +UART_HandleTypeDef huart2; + +/* USART1 init function */ + +void MX_USART1_UART_Init(void) +{ + + /* USER CODE BEGIN USART1_Init 0 */ + + /* USER CODE END USART1_Init 0 */ + + /* USER CODE BEGIN USART1_Init 1 */ + + /* USER CODE END USART1_Init 1 */ + huart1.Instance = USART1; + huart1.Init.BaudRate = 115200; + huart1.Init.WordLength = UART_WORDLENGTH_8B; + huart1.Init.StopBits = UART_STOPBITS_1; + huart1.Init.Parity = UART_PARITY_NONE; + huart1.Init.Mode = UART_MODE_TX_RX; + huart1.Init.HwFlowCtl = UART_HWCONTROL_NONE; + huart1.Init.OverSampling = UART_OVERSAMPLING_16; + if (HAL_UART_Init(&huart1) != HAL_OK) + { + Error_Handler(); + } + /* USER CODE BEGIN USART1_Init 2 */ + + /* USER CODE END USART1_Init 2 */ + +} +/* USART2 init function */ + +void MX_USART2_UART_Init(void) +{ + + /* USER CODE BEGIN USART2_Init 0 */ + + /* USER CODE END USART2_Init 0 */ + + /* USER CODE BEGIN USART2_Init 1 */ + + /* USER CODE END USART2_Init 1 */ + huart2.Instance = USART2; + huart2.Init.BaudRate = 115200; + huart2.Init.WordLength = UART_WORDLENGTH_8B; + huart2.Init.StopBits = UART_STOPBITS_1; + huart2.Init.Parity = UART_PARITY_NONE; + huart2.Init.Mode = UART_MODE_TX_RX; + huart2.Init.HwFlowCtl = UART_HWCONTROL_NONE; + huart2.Init.OverSampling = UART_OVERSAMPLING_16; + if (HAL_UART_Init(&huart2) != HAL_OK) + { + Error_Handler(); + } + /* USER CODE BEGIN USART2_Init 2 */ + + /* USER CODE END USART2_Init 2 */ + +} + +void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle) +{ + + GPIO_InitTypeDef GPIO_InitStruct = {0}; + if(uartHandle->Instance==USART1) + { + /* USER CODE BEGIN USART1_MspInit 0 */ + + /* USER CODE END USART1_MspInit 0 */ + /* USART1 clock enable */ + __HAL_RCC_USART1_CLK_ENABLE(); + + __HAL_RCC_GPIOB_CLK_ENABLE(); + /**USART1 GPIO Configuration + PB7 ------> USART1_RX + PB6 ------> USART1_TX + */ + GPIO_InitStruct.Pin = GPIO_PIN_7|GPIO_PIN_6; + GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH; + GPIO_InitStruct.Alternate = GPIO_AF7_USART1; + HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); + + /* USART1 interrupt Init */ + HAL_NVIC_SetPriority(USART1_IRQn, 5, 0); + HAL_NVIC_EnableIRQ(USART1_IRQn); + /* USER CODE BEGIN USART1_MspInit 1 */ + + /* USER CODE END USART1_MspInit 1 */ + } + else if(uartHandle->Instance==USART2) + { + /* USER CODE BEGIN USART2_MspInit 0 */ + + /* USER CODE END USART2_MspInit 0 */ + /* USART2 clock enable */ + __HAL_RCC_USART2_CLK_ENABLE(); + + __HAL_RCC_GPIOD_CLK_ENABLE(); + /**USART2 GPIO Configuration + PD6 ------> USART2_RX + PD5 ------> USART2_TX + */ + GPIO_InitStruct.Pin = GPIO_PIN_6|GPIO_PIN_5; + GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH; + GPIO_InitStruct.Alternate = GPIO_AF7_USART2; + HAL_GPIO_Init(GPIOD, &GPIO_InitStruct); + + /* USART2 interrupt Init */ + HAL_NVIC_SetPriority(USART2_IRQn, 5, 0); + HAL_NVIC_EnableIRQ(USART2_IRQn); + /* USER CODE BEGIN USART2_MspInit 1 */ + + /* USER CODE END USART2_MspInit 1 */ + } +} + +void HAL_UART_MspDeInit(UART_HandleTypeDef* uartHandle) +{ + + if(uartHandle->Instance==USART1) + { + /* USER CODE BEGIN USART1_MspDeInit 0 */ + + /* USER CODE END USART1_MspDeInit 0 */ + /* Peripheral clock disable */ + __HAL_RCC_USART1_CLK_DISABLE(); + + /**USART1 GPIO Configuration + PB7 ------> USART1_RX + PB6 ------> USART1_TX + */ + HAL_GPIO_DeInit(GPIOB, GPIO_PIN_7|GPIO_PIN_6); + + /* USART1 interrupt Deinit */ + HAL_NVIC_DisableIRQ(USART1_IRQn); + /* USER CODE BEGIN USART1_MspDeInit 1 */ + + /* USER CODE END USART1_MspDeInit 1 */ + } + else if(uartHandle->Instance==USART2) + { + /* USER CODE BEGIN USART2_MspDeInit 0 */ + + /* USER CODE END USART2_MspDeInit 0 */ + /* Peripheral clock disable */ + __HAL_RCC_USART2_CLK_DISABLE(); + + /**USART2 GPIO Configuration + PD6 ------> USART2_RX + PD5 ------> USART2_TX + */ + HAL_GPIO_DeInit(GPIOD, GPIO_PIN_6|GPIO_PIN_5); + + /* USART2 interrupt Deinit */ + HAL_NVIC_DisableIRQ(USART2_IRQn); + /* USER CODE BEGIN USART2_MspDeInit 1 */ + + /* USER CODE END USART2_MspDeInit 1 */ + } +} + +/* USER CODE BEGIN 1 */ + +/* USER CODE END 1 */ diff --git a/engineer/DM.ioc b/engineer/DM.ioc index 1b4b0c7..1c6fd9d 100644 --- a/engineer/DM.ioc +++ b/engineer/DM.ioc @@ -22,18 +22,24 @@ Mcu.IP1=FREERTOS Mcu.IP2=NVIC Mcu.IP3=RCC Mcu.IP4=SYS -Mcu.IPNb=5 +Mcu.IP5=USART1 +Mcu.IP6=USART2 +Mcu.IPNb=7 Mcu.Name=STM32F407I(E-G)Hx Mcu.Package=UFBGA176 Mcu.Pin0=PA14 Mcu.Pin1=PA13 -Mcu.Pin2=PD0 -Mcu.Pin3=PD1 -Mcu.Pin4=PH0-OSC_IN -Mcu.Pin5=PH1-OSC_OUT -Mcu.Pin6=VP_FREERTOS_VS_CMSIS_V2 -Mcu.Pin7=VP_SYS_VS_tim14 -Mcu.PinsNb=8 +Mcu.Pin10=VP_FREERTOS_VS_CMSIS_V2 +Mcu.Pin11=VP_SYS_VS_tim14 +Mcu.Pin2=PB7 +Mcu.Pin3=PB6 +Mcu.Pin4=PD6 +Mcu.Pin5=PD0 +Mcu.Pin6=PD5 +Mcu.Pin7=PD1 +Mcu.Pin8=PH0-OSC_IN +Mcu.Pin9=PH1-OSC_OUT +Mcu.PinsNb=12 Mcu.ThirdPartyNb=0 Mcu.UserConstants= Mcu.UserName=STM32F407IGHx @@ -58,17 +64,27 @@ NVIC.SysTick_IRQn=true\:15\:0\:false\:false\:false\:true\:false\:true\:false NVIC.TIM8_TRG_COM_TIM14_IRQn=true\:15\:0\:false\:false\:true\:false\:false\:true\:true NVIC.TimeBase=TIM8_TRG_COM_TIM14_IRQn NVIC.TimeBaseIP=TIM14 +NVIC.USART1_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true +NVIC.USART2_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false PA13.Mode=Serial_Wire PA13.Signal=SYS_JTMS-SWDIO PA14.Mode=Serial_Wire PA14.Signal=SYS_JTCK-SWCLK +PB6.Mode=Asynchronous +PB6.Signal=USART1_TX +PB7.Mode=Asynchronous +PB7.Signal=USART1_RX PD0.Locked=true PD0.Mode=CAN_Activate PD0.Signal=CAN1_RX PD1.Locked=true PD1.Mode=CAN_Activate PD1.Signal=CAN1_TX +PD5.Mode=Asynchronous +PD5.Signal=USART2_TX +PD6.Mode=Asynchronous +PD6.Signal=USART2_RX PH0-OSC_IN.Mode=HSE-External-Oscillator PH0-OSC_IN.Signal=RCC_OSC_IN PH1-OSC_OUT.Mode=HSE-External-Oscillator @@ -140,6 +156,10 @@ RCC.VCOI2SOutputFreq_Value=384000000 RCC.VCOInputFreq_Value=2000000 RCC.VCOOutputFreq_Value=336000000 RCC.VcooutputI2S=192000000 +USART1.IPParameters=VirtualMode +USART1.VirtualMode=VM_ASYNC +USART2.IPParameters=VirtualMode +USART2.VirtualMode=VM_ASYNC VP_FREERTOS_VS_CMSIS_V2.Mode=CMSIS_V2 VP_FREERTOS_VS_CMSIS_V2.Signal=FREERTOS_VS_CMSIS_V2 VP_SYS_VS_tim14.Mode=TIM14 diff --git a/engineer/User/bsp/uart.c b/engineer/User/bsp/uart.c new file mode 100644 index 0000000..219a196 --- /dev/null +++ b/engineer/User/bsp/uart.c @@ -0,0 +1,159 @@ +/* Includes ----------------------------------------------------------------- */ +#include + +#include "bsp/uart.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Private define ----------------------------------------------------------- */ +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Private variables -------------------------------------------------------- */ +static void (*UART_Callback[BSP_UART_NUM][BSP_UART_CB_NUM])(void); + +/* Private function -------------------------------------------------------- */ +static BSP_UART_t UART_Get(UART_HandleTypeDef *huart) { + if (huart->Instance == USART1) + return BSP_UART_1; + else if (huart->Instance == USART2) + return BSP_UART_2; + else + return BSP_UART_ERR; +} + +void HAL_UART_TxCpltCallback(UART_HandleTypeDef *huart) { + BSP_UART_t bsp_uart = UART_Get(huart); + if (bsp_uart != BSP_UART_ERR) { + if (UART_Callback[bsp_uart][BSP_UART_TX_CPLT_CB]) { + UART_Callback[bsp_uart][BSP_UART_TX_CPLT_CB](); + } + } +} + +void HAL_UART_TxHalfCpltCallback(UART_HandleTypeDef *huart) { + BSP_UART_t bsp_uart = UART_Get(huart); + if (bsp_uart != BSP_UART_ERR) { + if (UART_Callback[bsp_uart][BSP_UART_TX_HALF_CPLT_CB]) { + UART_Callback[bsp_uart][BSP_UART_TX_HALF_CPLT_CB](); + } + } +} + +void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) { + BSP_UART_t bsp_uart = UART_Get(huart); + if (bsp_uart != BSP_UART_ERR) { + if (UART_Callback[bsp_uart][BSP_UART_RX_CPLT_CB]) { + UART_Callback[bsp_uart][BSP_UART_RX_CPLT_CB](); + } + } +} + +void HAL_UART_RxHalfCpltCallback(UART_HandleTypeDef *huart) { + BSP_UART_t bsp_uart = UART_Get(huart); + if (bsp_uart != BSP_UART_ERR) { + if (UART_Callback[bsp_uart][BSP_UART_RX_HALF_CPLT_CB]) { + UART_Callback[bsp_uart][BSP_UART_RX_HALF_CPLT_CB](); + } + } +} + +void HAL_UART_ErrorCallback(UART_HandleTypeDef *huart) { + BSP_UART_t bsp_uart = UART_Get(huart); + if (bsp_uart != BSP_UART_ERR) { + if (UART_Callback[bsp_uart][BSP_UART_ERROR_CB]) { + UART_Callback[bsp_uart][BSP_UART_ERROR_CB](); + } + } +} + +void HAL_UART_AbortCpltCallback(UART_HandleTypeDef *huart) { + BSP_UART_t bsp_uart = UART_Get(huart); + if (bsp_uart != BSP_UART_ERR) { + if (UART_Callback[bsp_uart][BSP_UART_ABORT_CPLT_CB]) { + UART_Callback[bsp_uart][BSP_UART_ABORT_CPLT_CB](); + } + } +} + +void HAL_UART_AbortTransmitCpltCallback(UART_HandleTypeDef *huart) { + BSP_UART_t bsp_uart = UART_Get(huart); + if (bsp_uart != BSP_UART_ERR) { + if (UART_Callback[bsp_uart][BSP_UART_ABORT_TX_CPLT_CB]) { + UART_Callback[bsp_uart][BSP_UART_ABORT_TX_CPLT_CB](); + } + } +} + +void HAL_UART_AbortReceiveCpltCallback(UART_HandleTypeDef *huart) { + BSP_UART_t bsp_uart = UART_Get(huart); + if (bsp_uart != BSP_UART_ERR) { + if (UART_Callback[bsp_uart][BSP_UART_ABORT_RX_CPLT_CB]) { + UART_Callback[bsp_uart][BSP_UART_ABORT_RX_CPLT_CB](); + } + } +} + +/* Exported functions ------------------------------------------------------- */ +void BSP_UART_IRQHandler(UART_HandleTypeDef *huart) { + if (__HAL_UART_GET_FLAG(huart, UART_FLAG_IDLE)) { + __HAL_UART_CLEAR_IDLEFLAG(huart); + if (UART_Callback[UART_Get(huart)][BSP_UART_IDLE_LINE_CB]) { + UART_Callback[UART_Get(huart)][BSP_UART_IDLE_LINE_CB](); + } + } +} + +UART_HandleTypeDef *BSP_UART_GetHandle(BSP_UART_t uart) { + switch (uart) { + case BSP_UART_1: + return &huart1; + case BSP_UART_2: + return &huart2; + default: + return NULL; + } +} + +int8_t BSP_UART_RegisterCallback(BSP_UART_t uart, BSP_UART_Callback_t type, + void (*callback)(void)) { + if (callback == NULL) return BSP_ERR_NULL; + if (uart >= BSP_UART_NUM || type >= BSP_UART_CB_NUM) return BSP_ERR; + UART_Callback[uart][type] = callback; + return BSP_OK; +} + +int8_t BSP_UART_Transmit(BSP_UART_t uart, uint8_t *data, uint16_t size, bool dma) { + if (uart >= BSP_UART_NUM) return BSP_ERR; + if (data == NULL || size == 0) return BSP_ERR_NULL; + + if (dma) { + return HAL_UART_Transmit_DMA(BSP_UART_GetHandle(uart), data, size); + } else { + return HAL_UART_Transmit_IT(BSP_UART_GetHandle(uart), data, size); + } +} + +int8_t BSP_UART_Receive(BSP_UART_t uart, uint8_t *data, uint16_t size, bool dma) { + if (uart >= BSP_UART_NUM) return BSP_ERR; + if (data == NULL || size == 0) return BSP_ERR_NULL; + + if (dma) { + return HAL_UART_Receive_DMA(BSP_UART_GetHandle(uart), data, size); + } else { + return HAL_UART_Receive_IT(BSP_UART_GetHandle(uart), data, size); + } +} + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ \ No newline at end of file diff --git a/engineer/User/bsp/uart.h b/engineer/User/bsp/uart.h new file mode 100644 index 0000000..a50849b --- /dev/null +++ b/engineer/User/bsp/uart.h @@ -0,0 +1,69 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include +#include +#include + +#include "bsp/bsp.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Exported constants ------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Exported types ----------------------------------------------------------- */ + +/* 要添加使用UART的新设备，需要先在此添加对应的枚举值 */ + +/* UART实体枚举，与设备对应 */ +typedef enum { + BSP_UART_1, + BSP_UART_2, + BSP_UART_NUM, + BSP_UART_ERR, +} BSP_UART_t; + +/* UART支持的中断回调函数类型，具体参考HAL中定义 */ +typedef enum { + BSP_UART_TX_HALF_CPLT_CB, + BSP_UART_TX_CPLT_CB, + BSP_UART_RX_HALF_CPLT_CB, + BSP_UART_RX_CPLT_CB, + BSP_UART_ERROR_CB, + BSP_UART_ABORT_CPLT_CB, + BSP_UART_ABORT_TX_CPLT_CB, + BSP_UART_ABORT_RX_CPLT_CB, + + BSP_UART_IDLE_LINE_CB, + BSP_UART_CB_NUM, +} BSP_UART_Callback_t; + +/* Exported functions prototypes -------------------------------------------- */ + +UART_HandleTypeDef *BSP_UART_GetHandle(BSP_UART_t uart); + +void BSP_UART_IRQHandler(UART_HandleTypeDef *huart); + +int8_t BSP_UART_RegisterCallback(BSP_UART_t uart, BSP_UART_Callback_t type, + void (*callback)(void)); + +int8_t BSP_UART_Transmit(BSP_UART_t uart, uint8_t *data, uint16_t size, bool dma); +int8_t BSP_UART_Receive(BSP_UART_t uart, uint8_t *data, uint16_t size, bool dma); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/engineer/User/component/cmd.c b/engineer/User/component/cmd.c new file mode 100644 index 0000000..1effe69 --- /dev/null +++ b/engineer/User/component/cmd.c @@ -0,0 +1,387 @@ +/* + 控制命令 +*/ + +#include "cmd.h" + +#include + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/** + * @brief 行为转换为对应按键 + * + * @param cmd 主结构体 + * @param behavior 行为 + * @return uint16_t 行为对应的按键 + */ +static inline CMD_KeyValue_t CMD_BehaviorToKey(CMD_t *cmd, + CMD_Behavior_t behavior) { + return cmd->param->map.key_map[behavior].key; +} + +static inline CMD_ActiveType_t CMD_BehaviorToActive(CMD_t *cmd, + CMD_Behavior_t behavior) { + return cmd->param->map.key_map[behavior].active; +} + +/** + * @brief 检查按键是否按下 + * + * @param rc 遥控器数据 + * @param key 按键名称 + * @param stateful 是否为状态切换按键 + * @return true 按下 + * @return false 未按下 + */ +static bool CMD_KeyPressedRc(const CMD_RC_t *rc, CMD_KeyValue_t key) { + /* 按下按键为鼠标左、右键 */ + if (key == CMD_L_CLICK) { + return rc->mouse.l_click; + } + if (key == CMD_R_CLICK) { + return rc->mouse.r_click; + } + return rc->key & (1u << key); +} + +static bool CMD_BehaviorOccurredRc(const CMD_RC_t *rc, CMD_t *cmd, + CMD_Behavior_t behavior) { + CMD_KeyValue_t key = CMD_BehaviorToKey(cmd, behavior); + CMD_ActiveType_t active = CMD_BehaviorToActive(cmd, behavior); + + bool now_key_pressed, last_key_pressed; + + /* 按下按键为鼠标左、右键 */ + if (key == CMD_L_CLICK) { + now_key_pressed = rc->mouse.l_click; + last_key_pressed = cmd->mouse_last.l_click; + } else if (key == CMD_R_CLICK) { + now_key_pressed = rc->mouse.r_click; + last_key_pressed = cmd->mouse_last.r_click; + } else { + now_key_pressed = rc->key & (1u << key); + last_key_pressed = cmd->key_last & (1u << key); + } + + switch (active) { + case CMD_ACTIVE_PRESSING: + return now_key_pressed && !last_key_pressed; + case CMD_ACTIVE_RASING: + return !now_key_pressed && last_key_pressed; + case CMD_ACTIVE_PRESSED: + return now_key_pressed; + } +} + +/** + * @brief 解析pc行为逻辑 + * + * @param rc 遥控器数据 + * @param cmd 主结构体 + * @param dt_sec 两次解析的间隔 + */ +static void CMD_PcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) { + cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE; + + /* 云台设置为鼠标控制欧拉角的变化，底盘的控制向量设置为零 */ + cmd->gimbal.delta_eulr.yaw = + (float)rc->mouse.x * dt_sec * cmd->param->sens_mouse; + cmd->gimbal.delta_eulr.pit = + (float)(-rc->mouse.y) * dt_sec * cmd->param->sens_mouse; + cmd->chassis.ctrl_vec.vx = cmd->chassis.ctrl_vec.vy = 0.0f; + cmd->shoot.reverse_trig = false; + + /* 按键行为映射相关逻辑 */ + if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FORE)) { + cmd->chassis.ctrl_vec.vy += cmd->param->move.move_sense; + } + if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_BACK)) { + cmd->chassis.ctrl_vec.vy -= cmd->param->move.move_sense; + } + if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_LEFT)) { + cmd->chassis.ctrl_vec.vx -= cmd->param->move.move_sense; + } + if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_RIGHT)) { + cmd->chassis.ctrl_vec.vx += cmd->param->move.move_sense; + } + if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_ACCELERATE)) { + cmd->chassis.ctrl_vec.vx *= cmd->param->move.move_fast_sense; + cmd->chassis.ctrl_vec.vy *= cmd->param->move.move_fast_sense; + } + if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_DECELEBRATE)) { + cmd->chassis.ctrl_vec.vx *= cmd->param->move.move_slow_sense; + cmd->chassis.ctrl_vec.vy *= cmd->param->move.move_slow_sense; + } + if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FIRE)) { + /* 切换至开火模式，设置相应的射击频率和弹丸初速度 */ + cmd->shoot.mode = SHOOT_MODE_LOADED; + cmd->shoot.fire = true; + } else { + /* 切换至准备模式，停止射击 */ + cmd->shoot.mode = SHOOT_MODE_LOADED; + cmd->shoot.fire = false; + } + if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FIRE_MODE)) { + /* 每按一次依次切换开火下一个模式 */ + cmd->shoot.fire_mode++; + cmd->shoot.fire_mode %= FIRE_MODE_NUM; + } + if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_ROTOR)) { + /* 切换到小陀螺模式 */ + cmd->chassis.mode = CHASSIS_MODE_ROTOR; + cmd->chassis.mode_rotor = ROTOR_MODE_RAND; + } + if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_OPENCOVER)) { + /* 每按一次开、关弹舱盖 */ + cmd->shoot.cover_open = !cmd->shoot.cover_open; + } + if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_BUFF)) { + if (cmd->ai_status == AI_STATUS_HITSWITCH) { + /* 停止ai的打符模式，停用host控制 */ + CMD_RefereeAdd(&(cmd->referee), CMD_UI_HIT_SWITCH_STOP); + cmd->host_overwrite = false; + cmd->ai_status = AI_STATUS_STOP; + } else if (cmd->ai_status == AI_STATUS_AUTOAIM) { + /* 自瞄模式中切换失败提醒 */ + } else { + /* ai切换至打符模式，启用host控制 */ + CMD_RefereeAdd(&(cmd->referee), CMD_UI_HIT_SWITCH_START); + cmd->ai_status = AI_STATUS_HITSWITCH; + cmd->host_overwrite = true; + } + } + if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_AUTOAIM)) { + if (cmd->ai_status == AI_STATUS_AUTOAIM) { + /* 停止ai的自瞄模式，停用host控制 */ + cmd->host_overwrite = false; + cmd->ai_status = AI_STATUS_STOP; + CMD_RefereeAdd(&(cmd->referee), CMD_UI_AUTO_AIM_STOP); + } else { + /* ai切换至自瞄模式，启用host控制 */ + cmd->ai_status = AI_STATUS_AUTOAIM; + cmd->host_overwrite = true; + CMD_RefereeAdd(&(cmd->referee), CMD_UI_AUTO_AIM_START); + } + } else { + cmd->host_overwrite = false; + // TODO: 修复逻辑 + } + if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_REVTRIG)) { + /* 按下拨弹反转 */ + cmd->shoot.reverse_trig = true; + } + if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FOLLOWGIMBAL35)) { + cmd->chassis.mode = CHASSIS_MODE_FOLLOW_GIMBAL_35; + } + /* 保存当前按下的键位状态 */ + cmd->key_last = rc->key; + memcpy(&(cmd->mouse_last), &(rc->mouse), sizeof(cmd->mouse_last)); +} + +/** + * @brief 解析rc行为逻辑 + * + * @param rc 遥控器数据 + * @param cmd 主结构体 + * @param dt_sec 两次解析的间隔 + */ +static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) { + switch (rc->sw_l) { + /* 左拨杆相应行为选择和解析 */ + case CMD_SW_UP: + cmd->chassis.mode = CHASSIS_MODE_BREAK; + break; + + case CMD_SW_MID: + cmd->chassis.mode = CHASSIS_MODE_FOLLOW_GIMBAL; + break; + + case CMD_SW_DOWN: + cmd->chassis.mode = CHASSIS_MODE_ROTOR; + cmd->chassis.mode_rotor = ROTOR_MODE_CW; + break; + + case CMD_SW_ERR: + cmd->chassis.mode = CHASSIS_MODE_RELAX; + break; + } + switch (rc->sw_r) { + /* 右拨杆相应行为选择和解析*/ + case CMD_SW_UP: + cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE; + cmd->shoot.mode = SHOOT_MODE_SAFE; + break; + + case CMD_SW_MID: + cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE; + cmd->shoot.fire = false; + cmd->shoot.mode = SHOOT_MODE_LOADED; + break; + + case CMD_SW_DOWN: + cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE; + cmd->shoot.mode = SHOOT_MODE_LOADED; + cmd->shoot.fire_mode = FIRE_MODE_SINGLE; + cmd->shoot.fire = true; + break; + /* + case CMD_SW_UP: + cmd->gimbal.mode = GIMBAL_MODE_RELAX; + cmd->shoot.mode = SHOOT_MODE_SAFE; + break; + + case CMD_SW_MID: + cmd->gimbal.mode = GIMBAL_MODE_RELAX; + cmd->shoot.fire = false; + cmd->shoot.mode = SHOOT_MODE_LOADED; + break; + + case CMD_SW_DOWN: + cmd->gimbal.mode = GIMBAL_MODE_RELAX; + cmd->shoot.mode = SHOOT_MODE_LOADED; + cmd->shoot.fire_mode = FIRE_MODE_SINGLE; + cmd->shoot.fire = true; + break; + */ + case CMD_SW_ERR: + cmd->gimbal.mode = GIMBAL_MODE_RELAX; + cmd->shoot.mode = SHOOT_MODE_RELAX; + } + /* 将操纵杆的对应值转换为底盘的控制向量和云台变化的欧拉角 */ + cmd->chassis.ctrl_vec.vx = rc->ch_l_x; + cmd->chassis.ctrl_vec.vy = rc->ch_l_y; + cmd->gimbal.delta_eulr.yaw = rc->ch_r_x * dt_sec * cmd->param->sens_rc; + cmd->gimbal.delta_eulr.pit = rc->ch_r_y * dt_sec * cmd->param->sens_rc; +} + +/** + * @brief rc失控时机器人恢复放松模式 + * + * @param cmd 主结构体 + */ +static void CMD_RcLostLogic(CMD_t *cmd) { + /* 机器人底盘、云台、射击运行模式恢复至放松模式 */ + cmd->chassis.mode = CHASSIS_MODE_RELAX; + cmd->gimbal.mode = GIMBAL_MODE_RELAX; + cmd->shoot.mode = SHOOT_MODE_RELAX; +} + +/** + * @brief 初始化命令解析 + * + * @param cmd 主结构体 + * @param param 参数 + * @return int8_t 0对应没有错误 + */ +int8_t CMD_Init(CMD_t *cmd, const CMD_Params_t *param) { + /* 指针检测 */ + if (cmd == NULL) return -1; + if (param == NULL) return -1; + + /* 设置机器人的命令参数，初始化控制方式为rc控制 */ + cmd->pc_ctrl = false; + cmd->param = param; + + return 0; +} + +/** + * @brief 检查是否启用上位机控制指令覆盖 + * + * @param cmd 主结构体 + * @return true 启用 + * @return false 不启用 + */ +inline bool CMD_CheckHostOverwrite(CMD_t *cmd) { return cmd->host_overwrite; } + +/** + * @brief 解析命令 + * + * @param rc 遥控器数据 + * @param cmd 命令 + * @param dt_sec 两次解析的间隔 + * @return int8_t 0对应没有错误 + */ +int8_t CMD_ParseRc(CMD_RC_t *rc, CMD_t *cmd, float dt_sec) { + /* 指针检测 */ + if (rc == NULL) return -1; + if (cmd == NULL) return -1; + + /* 在pc控制和rc控制间切换 */ + if (CMD_KeyPressedRc(rc, CMD_KEY_SHIFT) && + CMD_KeyPressedRc(rc, CMD_KEY_CTRL) && CMD_KeyPressedRc(rc, CMD_KEY_Q)) + cmd->pc_ctrl = true; + + if (CMD_KeyPressedRc(rc, CMD_KEY_SHIFT) && + CMD_KeyPressedRc(rc, CMD_KEY_CTRL) && CMD_KeyPressedRc(rc, CMD_KEY_E)) + cmd->pc_ctrl = false; + /*c当rc丢控时，恢复机器人至默认状态 */ + if ((rc->sw_l == CMD_SW_ERR) || (rc->sw_r == CMD_SW_ERR)) { + CMD_RcLostLogic(cmd); + } else { + if (cmd->pc_ctrl) { + CMD_PcLogic(rc, cmd, dt_sec); + } else { + CMD_RcLogic(rc, cmd, dt_sec); + } + } + return 0; +} + +/** + * @brief 解析上位机命令 + * + * @param host host数据 + * @param cmd 命令 + * @param dt_sec 两次解析的间隔 + * @return int8_t 0对应没有错误 + */ +int8_t CMD_ParseHost(const CMD_Host_t *host, CMD_t *cmd, float dt_sec) { + (void)dt_sec; /* 未使用dt_sec，消除警告 */ + /* 指针检测 */ + if (host == NULL) return -1; + if (cmd == NULL) return -1; + + /* 云台欧拉角设置为host相应的变化的欧拉角 */ + cmd->gimbal.delta_eulr.yaw = host->gimbal_delta.yaw; + cmd->gimbal.delta_eulr.pit = host->gimbal_delta.pit; + + /* host射击命令，设置不同的射击频率和弹丸初速度 */ + if (host->fire) { + cmd->shoot.mode = SHOOT_MODE_LOADED; + cmd->shoot.fire = true; + } else { + cmd->shoot.mode = SHOOT_MODE_SAFE; + } + return 0; +} + +/** + * @brief 添加向Referee发送的命令 + * + * @param ref 命令队列 + * @param cmd 要添加的命令 + * @return int8_t 0对应没有错误 + */ +int8_t CMD_RefereeAdd(CMD_RefereeCmd_t *ref, CMD_UI_t cmd) { + /* 指针检测 */ + if (ref == NULL) return -1; + /* 越界检测 */ + if (ref->counter >= CMD_REFEREE_MAX_NUM || ref->counter < 0) return -1; + + /* 添加机器人当前行为状态到画图的命令队列中 */ + ref->cmd[ref->counter] = cmd; + ref->counter++; + return 0; +} + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ diff --git a/engineer/User/component/cmd.h b/engineer/User/component/cmd.h new file mode 100644 index 0000000..e1071c3 --- /dev/null +++ b/engineer/User/component/cmd.h @@ -0,0 +1,342 @@ +/* + 控制命令 +*/ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +#include "component/ahrs.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +#define CMD_REFEREE_MAX_NUM (3) /* Lines 16 omitted */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* 机器人型号 */ +typedef enum { + ROBOT_MODEL_INFANTRY = 0, /* 步兵机器人 */ + ROBOT_MODEL_HERO, /* 英雄机器人 */ + ROBOT_MODEL_ENGINEER, /* 工程机器人 */ + ROBOT_MODEL_DRONE, /* 空中机器人 */ + ROBOT_MODEL_SENTRY, /* 哨兵机器人 */ + ROBOT_MODEL_NUM, /* 型号数量 */ +} CMD_RobotModel_t; + +/* 底盘运行模式 */ +typedef enum { + CHASSIS_MODE_RELAX, /* 放松模式，电机不输出。一般情况底盘初始化之后的模式 */ + CHASSIS_MODE_BREAK, /* 刹车模式，电机闭环控制保持静止。用于机器人停止状态 */ + CHASSIS_MODE_FOLLOW_GIMBAL, /* 通过闭环控制使车头方向跟随云台 */ + CHASSIS_MODE_FOLLOW_GIMBAL_35, /* 通过闭环控制使车头方向35度跟随云台 */ + CHASSIS_MODE_ROTOR, /* 小陀螺模式，通过闭环控制使底盘不停旋转 */ + CHASSIS_MODE_INDENPENDENT, /* 独立模式。底盘运行不受云台影响 */ + CHASSIS_MODE_OPEN, /* 开环模式。底盘运行不受PID控制，直接输出到电机 */ +} CMD_ChassisMode_t; + +typedef enum{ + SET_ARM_MODE_RELAX, /*放松模式*/ + SET_ARM_MODE_FOLLOW, /*控制器跟踪模式*/ + SET_ARM_MODE_POSITION,/*关节位置指令模式*/ + SET_ARM_MODE_STORAGE, + SET_ARM_MODE_TAKE +}CMD_ArmMode_t; + + + +/* 云台运行模式 */ +typedef enum { + GIMBAL_MODE_RELAX, /* 放松模式，电机不输出。一般情况云台初始化之后的模式 */ + GIMBAL_MODE_ABSOLUTE, /* 绝对坐标系控制，控制在空间内的绝对姿态 */ + GIMBAL_MODE_RELATIVE, /* 相对坐标系控制，控制相对于底盘的姿态 */ +} CMD_GimbalMode_t; + +/* 射击运行模式 */ +typedef enum { + SHOOT_MODE_RELAX, /* 放松模式，电机不输出 */ + SHOOT_MODE_SAFE, /* 保险模式，电机闭环控制保持静止 */ + SHOOT_MODE_LOADED, /* 上膛模式，摩擦轮开启。随时准备开火 */ +} CMD_ShootMode_t; + +typedef enum { + FIRE_MODE_SINGLE, /* 单发开火模式 */ + FIRE_MODE_BURST, /* N连发开火模式 */ + FIRE_MODE_CONT, /* 持续开火模式 */ + FIRE_MODE_NUM, +} CMD_FireMode_t; + +/* 小陀螺转动模式 */ +typedef enum { + ROTOR_MODE_CW, /* 顺时针转动 */ + ROTOR_MODE_CCW, /* 逆时针转动 */ + ROTOR_MODE_RAND, /* 随机转动 */ +} CMD_RotorMode_t; + + +/* 机械臂控制命令*/ +typedef struct { + CMD_ArmMode_t mode; + uint8_t cmd_prepare; + struct{ + float ref_angle[6]; + float set_angle[6]; + }data; +}CMD_Arm_t; + + + + +/* 底盘控制命令 */ +typedef struct { + CMD_ChassisMode_t mode; /* 底盘运行模式 */ + CMD_RotorMode_t mode_rotor; /* 小陀螺转动模式 */ + MoveVector_t ctrl_vec; /* 底盘控制向量 */ +} CMD_ChassisCmd_t; + +/* 云台控制命令 */ +typedef struct { + CMD_GimbalMode_t mode; /* 云台运行模式 */ + AHRS_Eulr_t delta_eulr; /* 欧拉角变化角度 */ +} CMD_GimbalCmd_t; + +/* 射击控制命令 */ +typedef struct { + CMD_ShootMode_t mode; /* 射击运行模式 */ + CMD_FireMode_t fire_mode; /* 开火模式 */ + bool fire; /*开火*/ + bool cover_open; /* 弹舱盖开关 */ + bool reverse_trig; /* 拨弹电机状态 */ +} CMD_ShootCmd_t; + +/* 拨杆位置 */ +typedef enum { + CMD_SW_ERR = 0, + CMD_SW_UP = 1, + CMD_SW_MID = 3, + CMD_SW_DOWN = 2, +} CMD_SwitchPos_t; + +/* 键盘按键值 */ +typedef enum { + CMD_KEY_W = 0, + CMD_KEY_S, + CMD_KEY_A, + CMD_KEY_D, + CMD_KEY_SHIFT, + CMD_KEY_CTRL, + CMD_KEY_Q, + CMD_KEY_E, + CMD_KEY_R, + CMD_KEY_F, + CMD_KEY_G, + CMD_KEY_Z, + CMD_KEY_X, + CMD_KEY_C, + CMD_KEY_V, + CMD_KEY_B, + CMD_L_CLICK, + CMD_R_CLICK, + CMD_KEY_NUM, +} CMD_KeyValue_t; + +/* 行为值序列 */ +typedef enum { + CMD_BEHAVIOR_FORE = 0, /* 向前 */ + CMD_BEHAVIOR_BACK, /* 向后 */ + CMD_BEHAVIOR_LEFT, /* 向左 */ + CMD_BEHAVIOR_RIGHT, /* 向右 */ + CMD_BEHAVIOR_ACCELERATE, /* 加速 */ + CMD_BEHAVIOR_DECELEBRATE, /* 减速 */ + CMD_BEHAVIOR_FIRE, /* 开火 */ + CMD_BEHAVIOR_FIRE_MODE, /* 切换开火模式 */ + CMD_BEHAVIOR_BUFF, /* 打符模式 */ + CMD_BEHAVIOR_AUTOAIM, /* 自瞄模式 */ + CMD_BEHAVIOR_OPENCOVER, /* 弹舱盖开关 */ + CMD_BEHAVIOR_ROTOR, /* 小陀螺模式 */ + CMD_BEHAVIOR_REVTRIG, /* 反转拨弹 */ + CMD_BEHAVIOR_FOLLOWGIMBAL35, /* 跟随云台呈35度 */ + CMD_BEHAVIOR_NUM, +} CMD_Behavior_t; + +typedef enum { + CMD_ACTIVE_PRESSING, /* 按下时触发 */ + CMD_ACTIVE_RASING, /* 抬起时触发 */ + CMD_ACTIVE_PRESSED, /* 按住时触发 */ +} CMD_ActiveType_t; + +typedef struct { + CMD_ActiveType_t active; + CMD_KeyValue_t key; +} CMD_KeyMapItem_t; + +/* 行为映射的对应按键数组 */ +typedef struct { + CMD_KeyMapItem_t key_map[CMD_BEHAVIOR_NUM]; +} CMD_KeyMap_Params_t; + +/* 位移灵敏度参数 */ +typedef struct { + float move_sense; /* 移动灵敏度 */ + float move_fast_sense; /* 加速灵敏度 */ + float move_slow_sense; /* 减速灵敏度 */ +} CMD_Move_Params_t; + +typedef struct { + uint16_t width; + uint16_t height; +} CMD_Screen_t; + +/* 命令参数 */ +typedef struct { + float sens_mouse; /* 鼠标灵敏度 */ + float sens_rc; /* 遥控器摇杆灵敏度 */ + CMD_KeyMap_Params_t map; /* 按键映射行为命令 */ + CMD_Move_Params_t move; /* 位移灵敏度参数 */ + CMD_Screen_t screen; /* 屏幕分辨率参数 */ +} CMD_Params_t; + +/* AI行为状态 */ +typedef enum { + AI_STATUS_STOP, /* 停止状态 */ + AI_STATUS_AUTOAIM, /* 自瞄状态 */ + AI_STATUS_HITSWITCH, /* 打符状态 */ + AI_STATUS_AUTOMATIC /* 自动状态 */ +} CMD_AI_Status_t; + +/* UI所用行为状态 */ +typedef enum { + CMD_UI_NOTHING, /* 当前无状态 */ + CMD_UI_AUTO_AIM_START, /* 自瞄状态开启 */ + CMD_UI_AUTO_AIM_STOP, /* 自瞄状态关闭 */ + CMD_UI_HIT_SWITCH_START, /* 打符状态开启 */ + CMD_UI_HIT_SWITCH_STOP /* 打符状态关闭 */ +} CMD_UI_t; + +/*裁判系统发送的命令*/ +typedef struct { + CMD_UI_t cmd[CMD_REFEREE_MAX_NUM]; /* 命令数组 */ + uint8_t counter; /* 命令计数 */ +} CMD_RefereeCmd_t; + +typedef struct { + bool pc_ctrl; /* 是否使用键鼠控制 */ + bool host_overwrite; /* 是否Host控制 */ + uint16_t key_last; /* 上次按键键值 */ + + struct { + int16_t x; + int16_t y; + int16_t z; + bool l_click; /* 左键 */ + bool r_click; /* 右键 */ + } mouse_last; /* 鼠标值 */ + + CMD_AI_Status_t ai_status; /* AI状态 */ + + const CMD_Params_t *param; /* 命令参数 */ + + CMD_ChassisCmd_t chassis; /* 底盘控制命令 */ + CMD_GimbalCmd_t gimbal; /* 云台控制命令 */ + CMD_ShootCmd_t shoot; /* 射击控制命令 */ + CMD_RefereeCmd_t referee; /* 裁判系统发送命令 */ +} CMD_t; + +typedef struct { + float ch_l_x; /* 遥控器左侧摇杆横轴值，上为正 */ + float ch_l_y; /* 遥控器左侧摇杆纵轴值，右为正 */ + float ch_r_x; /* 遥控器右侧摇杆横轴值，上为正 */ + float ch_r_y; /* 遥控器右侧摇杆纵轴值，右为正 */ + + float ch_res; /* 第五通道值 */ + + CMD_SwitchPos_t sw_r; /* 右侧拨杆位置 */ + CMD_SwitchPos_t sw_l; /* 左侧拨杆位置 */ + + struct { + int16_t x; + int16_t y; + int16_t z; + bool l_click; /* 左键 */ + bool r_click; /* 右键 */ + } mouse; /* 鼠标值 */ + + uint16_t key; /* 按键值 */ + + uint16_t res; /* 保留，未启用 */ +} CMD_RC_t; + +typedef struct { + AHRS_Eulr_t gimbal_delta; /* 欧拉角的变化量 */ + + struct { + float vx; /* x轴移动速度 */ + float vy; /* y轴移动速度 */ + float wz; /* z轴转动速度 */ + } chassis_move_vec; /* 底盘移动向量 */ + + bool fire; /* 开火状态 */ +} CMD_Host_t; + +/** + * @brief 解析行为命令 + * + * @param rc 遥控器数据 + * @param cmd 主结构体 + */ +int8_t CMD_Init(CMD_t *cmd, const CMD_Params_t *param); + +/** + * @brief 检查是否启用上位机控制指令覆盖 + * + * @param cmd 主结构体 + * @return true 启用 + * @return false 不启用 + */ +bool CMD_CheckHostOverwrite(CMD_t *cmd); + +/** + * @brief 解析命令 + * + * @param rc 遥控器数据 + * @param cmd 命令 + * @param dt_sec 两次解析的间隔 + * @return int8_t 0对应没有错误 + */ +int8_t CMD_ParseRc(CMD_RC_t *rc, CMD_t *cmd, float dt_sec); + +/** + * @brief 解析上位机命令 + * + * @param host host数据 + * @param cmd 命令 + * @param dt_sec 两次解析的间隔 + * @return int8_t 0对应没有错误 + */ +int8_t CMD_ParseHost(const CMD_Host_t *host, CMD_t *cmd, float dt_sec); + +/** + * @brief 添加向Referee发送的命令 + * + * @param ref 命令队列 + * @param cmd 要添加的命令 + * @return int8_t 0对应没有错误 + */ +int8_t CMD_RefereeAdd(CMD_RefereeCmd_t *ref, CMD_UI_t cmd); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/engineer/User/component/crc16.c b/engineer/User/component/crc16.c new file mode 100644 index 0000000..0d17eb0 --- /dev/null +++ b/engineer/User/component/crc16.c @@ -0,0 +1,62 @@ +#include "crc16.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +static const uint16_t crc16_tab[256] = { + 0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf, 0x8c48, + 0x9dc1, 0xaf5a, 0xbed3, 0xca6c, 0xdbe5, 0xe97e, 0xf8f7, 0x1081, 0x0108, + 0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e, 0x9cc9, 0x8d40, 0xbfdb, + 0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876, 0x2102, 0x308b, 0x0210, 0x1399, + 0x6726, 0x76af, 0x4434, 0x55bd, 0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e, + 0xfae7, 0xc87c, 0xd9f5, 0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e, + 0x54b5, 0x453c, 0xbdcb, 0xac42, 0x9ed9, 0x8f50, 0xfbef, 0xea66, 0xd8fd, + 0xc974, 0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb, + 0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3, 0x5285, + 0x430c, 0x7197, 0x601e, 0x14a1, 0x0528, 0x37b3, 0x263a, 0xdecd, 0xcf44, + 0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72, 0x6306, 0x728f, 0x4014, + 0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9, 0xef4e, 0xfec7, 0xcc5c, 0xddd5, + 0xa96a, 0xb8e3, 0x8a78, 0x9bf1, 0x7387, 0x620e, 0x5095, 0x411c, 0x35a3, + 0x242a, 0x16b1, 0x0738, 0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862, + 0x9af9, 0x8b70, 0x8408, 0x9581, 0xa71a, 0xb693, 0xc22c, 0xd3a5, 0xe13e, + 0xf0b7, 0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff, + 0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036, 0x18c1, + 0x0948, 0x3bd3, 0x2a5a, 0x5ee5, 0x4f6c, 0x7df7, 0x6c7e, 0xa50a, 0xb483, + 0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5, 0x2942, 0x38cb, 0x0a50, + 0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd, 0xb58b, 0xa402, 0x9699, 0x8710, + 0xf3af, 0xe226, 0xd0bd, 0xc134, 0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7, + 0x6e6e, 0x5cf5, 0x4d7c, 0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1, + 0xa33a, 0xb2b3, 0x4a44, 0x5bcd, 0x6956, 0x78df, 0x0c60, 0x1de9, 0x2f72, + 0x3efb, 0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232, + 0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a, 0xe70e, + 0xf687, 0xc41c, 0xd595, 0xa12a, 0xb0a3, 0x8238, 0x93b1, 0x6b46, 0x7acf, + 0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9, 0xf78f, 0xe606, 0xd49d, + 0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330, 0x7bc7, 0x6a4e, 0x58d5, 0x495c, + 0x3de3, 0x2c6a, 0x1ef1, 0x0f78}; + +static inline uint16_t CRC16_Byte(uint16_t crc, const uint8_t data) { + return (crc >> 8) ^ crc16_tab[(crc ^ data) & 0xff]; +} + +uint16_t CRC16_Calc(const uint8_t *buf, size_t len, uint16_t crc) { + while (len--) crc = CRC16_Byte(crc, *buf++); + return crc; +} + +bool CRC16_Verify(const uint8_t *buf, size_t len) { + if (len < 2) return false; + + uint16_t expected = CRC16_Calc(buf, len - sizeof(uint16_t), CRC16_INIT); + return expected == + ((const uint16_t *)((const uint8_t *)buf + + (len % 2)))[len / sizeof(uint16_t) - 1]; +} + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ diff --git a/engineer/User/component/crc16.h b/engineer/User/component/crc16.h new file mode 100644 index 0000000..68b0a87 --- /dev/null +++ b/engineer/User/component/crc16.h @@ -0,0 +1,30 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include + +#include "user_math.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +#define CRC16_INIT 0XFFFF + +uint16_t CRC16_Calc(const uint8_t *buf, size_t len, uint16_t crc); +bool CRC16_Verify(const uint8_t *buf, size_t len); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/engineer/User/component/crc8.c b/engineer/User/component/crc8.c new file mode 100644 index 0000000..66f4ad2 --- /dev/null +++ b/engineer/User/component/crc8.c @@ -0,0 +1,52 @@ +#include "crc8.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +static const uint8_t crc8_tab[256] = { + 0x00, 0x5e, 0xbc, 0xe2, 0x61, 0x3f, 0xdd, 0x83, 0xc2, 0x9c, 0x7e, 0x20, + 0xa3, 0xfd, 0x1f, 0x41, 0x9d, 0xc3, 0x21, 0x7f, 0xfc, 0xa2, 0x40, 0x1e, + 0x5f, 0x01, 0xe3, 0xbd, 0x3e, 0x60, 0x82, 0xdc, 0x23, 0x7d, 0x9f, 0xc1, + 0x42, 0x1c, 0xfe, 0xa0, 0xe1, 0xbf, 0x5d, 0x03, 0x80, 0xde, 0x3c, 0x62, + 0xbe, 0xe0, 0x02, 0x5c, 0xdf, 0x81, 0x63, 0x3d, 0x7c, 0x22, 0xc0, 0x9e, + 0x1d, 0x43, 0xa1, 0xff, 0x46, 0x18, 0xfa, 0xa4, 0x27, 0x79, 0x9b, 0xc5, + 0x84, 0xda, 0x38, 0x66, 0xe5, 0xbb, 0x59, 0x07, 0xdb, 0x85, 0x67, 0x39, + 0xba, 0xe4, 0x06, 0x58, 0x19, 0x47, 0xa5, 0xfb, 0x78, 0x26, 0xc4, 0x9a, + 0x65, 0x3b, 0xd9, 0x87, 0x04, 0x5a, 0xb8, 0xe6, 0xa7, 0xf9, 0x1b, 0x45, + 0xc6, 0x98, 0x7a, 0x24, 0xf8, 0xa6, 0x44, 0x1a, 0x99, 0xc7, 0x25, 0x7b, + 0x3a, 0x64, 0x86, 0xd8, 0x5b, 0x05, 0xe7, 0xb9, 0x8c, 0xd2, 0x30, 0x6e, + 0xed, 0xb3, 0x51, 0x0f, 0x4e, 0x10, 0xf2, 0xac, 0x2f, 0x71, 0x93, 0xcd, + 0x11, 0x4f, 0xad, 0xf3, 0x70, 0x2e, 0xcc, 0x92, 0xd3, 0x8d, 0x6f, 0x31, + 0xb2, 0xec, 0x0e, 0x50, 0xaf, 0xf1, 0x13, 0x4d, 0xce, 0x90, 0x72, 0x2c, + 0x6d, 0x33, 0xd1, 0x8f, 0x0c, 0x52, 0xb0, 0xee, 0x32, 0x6c, 0x8e, 0xd0, + 0x53, 0x0d, 0xef, 0xb1, 0xf0, 0xae, 0x4c, 0x12, 0x91, 0xcf, 0x2d, 0x73, + 0xca, 0x94, 0x76, 0x28, 0xab, 0xf5, 0x17, 0x49, 0x08, 0x56, 0xb4, 0xea, + 0x69, 0x37, 0xd5, 0x8b, 0x57, 0x09, 0xeb, 0xb5, 0x36, 0x68, 0x8a, 0xd4, + 0x95, 0xcb, 0x29, 0x77, 0xf4, 0xaa, 0x48, 0x16, 0xe9, 0xb7, 0x55, 0x0b, + 0x88, 0xd6, 0x34, 0x6a, 0x2b, 0x75, 0x97, 0xc9, 0x4a, 0x14, 0xf6, 0xa8, + 0x74, 0x2a, 0xc8, 0x96, 0x15, 0x4b, 0xa9, 0xf7, 0xb6, 0xe8, 0x0a, 0x54, + 0xd7, 0x89, 0x6b, 0x35, +}; + +uint8_t CRC8_Calc(const uint8_t *buf, size_t len, uint8_t crc) { + /* loop over the buffer data */ + while (len-- > 0) crc = crc8_tab[(crc ^ *buf++) & 0xff]; + + return crc; +} + +bool CRC8_Verify(const uint8_t *buf, size_t len) { + if (len < 2) return false; + + uint8_t expected = CRC8_Calc(buf, len - sizeof(uint8_t), CRC8_INIT); + return expected == buf[len - sizeof(uint8_t)]; +} + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ diff --git a/engineer/User/component/crc8.h b/engineer/User/component/crc8.h new file mode 100644 index 0000000..a376c71 --- /dev/null +++ b/engineer/User/component/crc8.h @@ -0,0 +1,30 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +#define CRC8_INIT 0xFF + +uint8_t CRC8_Calc(const uint8_t *buf, size_t len, uint8_t crc); +bool CRC8_Verify(const uint8_t *buf, size_t len); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/engineer/User/component/limiter.c b/engineer/User/component/limiter.c new file mode 100644 index 0000000..71e4bf1 --- /dev/null +++ b/engineer/User/component/limiter.c @@ -0,0 +1,107 @@ +/* + 限制器 +*/ + +#include "limiter.h" + +#include +#include + +#define POWER_BUFF_THRESHOLD 20 +#define CHASSIS_POWER_CHECK_FREQ 10 +#define CHASSIS_POWER_FACTOR_PASS 0.9f +#define CHASSIS_POWER_FACTOR_NO_PASS 1.5f + +#define CHASSIS_MOTOR_CIRCUMFERENCE 0.12f + +/** + * @brief 限制底盘功率不超过power_limit + * + * @param power_limit 最大功率 + * @param motor_out 电机输出值 + * @param speed 电机转速 + * @param len 电机数量 + * @return int8_t 0对应没有错误 + */ +int8_t PowerLimit_ChassicOutput(float power_limit, float *motor_out, + float *speed, uint32_t len) { + /* power_limit小于0时不进行限制 */ + if (motor_out == NULL || speed == NULL || power_limit < 0) return -1; + + float sum_motor_out = 0.0f; + for (uint32_t i = 0; i < len; i++) { + /* 总功率计算 P=F(由转矩电流表示)*V(由转速表示) */ + sum_motor_out += + fabsf(motor_out[i]) * fabsf(speed[i]) * CHASSIS_MOTOR_CIRCUMFERENCE; + } + + /* 保持每个电机输出值缩小时比例不变 */ + if (sum_motor_out > power_limit) { + for (uint32_t i = 0; i < len; i++) { + motor_out[i] *= power_limit / sum_motor_out; + } + } + + return 0; +} + +/** + * @brief 电容输入功率计算 + * + * @param power_in 底盘当前功率 + * @param power_limit 裁判系统功率限制值 + * @param power_buffer 缓冲能量 + * @return float 裁判系统输出最大值 + */ +float PowerLimit_CapInput(float power_in, float power_limit, + float power_buffer) { + float target_power = 0.0f; + + /* 计算下一个检测周期的剩余缓冲能量 */ + float heat_buff = power_buffer - (float)(power_in - power_limit) / + (float)CHASSIS_POWER_CHECK_FREQ; + if (heat_buff < POWER_BUFF_THRESHOLD) { /* 功率限制 */ + target_power = power_limit * CHASSIS_POWER_FACTOR_PASS; + } else { + target_power = power_limit * CHASSIS_POWER_FACTOR_NO_PASS; + } + + return target_power; +} + +/** + * @brief 使用缓冲能量计算底盘最大功率 + * + * @param power_limit 裁判系统功率限制值 + * @param power_buffer 缓冲能量 + * @return float 底盘输出最大值 + */ +float PowerLimit_TargetPower(float power_limit, float power_buffer) { + float target_power = 0.0f; + + /* 根据剩余缓冲能量计算输出功率 */ + target_power = power_limit * (power_buffer - 10.0f) / 20.0f; + if (target_power < 0.0f) target_power = 0.0f; + + return target_power; +} + +/** + * @brief 射击频率控制 + * + * @param heat 当前热量 + * @param heat_limit 热量上限 + * @param cooling_rate 冷却速率 + * @param heat_increase 冷却增加 + * @param shoot_freq 经过热量限制后的射击频率 + * @return float 射击频率 + */ +float HeatLimit_ShootFreq(float heat, float heat_limit, float cooling_rate, + float heat_increase, bool is_big) { + float heat_percent = heat / heat_limit; + float stable_freq = cooling_rate / heat_increase; + if (is_big) + return stable_freq; + else + return (heat_percent > 0.7f) ? stable_freq : 3.0f * stable_freq; +} diff --git a/engineer/User/component/limiter.h b/engineer/User/component/limiter.h new file mode 100644 index 0000000..d0aa92a --- /dev/null +++ b/engineer/User/component/limiter.h @@ -0,0 +1,63 @@ +/* + 限制器 +*/ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/** + * @brief 限制底盘功率不超过power_limit + * + * @param power_limit 最大功率 + * @param motor_out 电机输出值 + * @param speed 电机转速 + * @param len 电机数量 + * @return int8_t 0对应没有错误 + */ +int8_t PowerLimit_ChassicOutput(float power_limit, float *motor_out, + float *speed, uint32_t len); +/** + * @brief 电容输入功率计算 + * + * @param power_in 底盘当前功率 + * @param power_limit 裁判系统功率限制值 + * @param power_buffer 缓冲能量 + * @return float 裁判系统输出最大值 + */ +float PowerLimit_CapInput(float power_in, float power_limit, + float power_buffer); +/** + * @brief 使用缓冲能量计算底盘最大功率 + * + * @param power_limit 裁判系统功率限制值 + * @param power_buffer 缓冲能量 + * @return float 底盘输出最大值 + */ +float PowerLimit_TargetPower(float power_limit, float power_buffer); + +/** + * @brief 射击频率控制 + * + * @param heat 当前热量 + * @param heat_limit 热量上限 + * @param cooling_rate 冷却速率 + * @param heat_increase 冷却增加 + * @param shoot_freq 经过热量限制后的射击频率 + * @return float 射击频率 + */ +float HeatLimit_ShootFreq(float heat, float heat_limit, float cooling_rate, + float heat_increase, bool is_big); diff --git a/engineer/User/component/matrix.cpp b/engineer/User/component/matrix.cpp new file mode 100644 index 0000000..91f9b87 --- /dev/null +++ b/engineer/User/component/matrix.cpp @@ -0,0 +1,24 @@ +/** + ****************************************************************************** + * @file matrix.cpp/h + * @brief Matrix/vector calculation. 矩阵/向量运算 + * @author Spoon Guan + ****************************************************************************** + * Copyright (c) 2023 Team JiaoLong-SJTU + * All rights reserved. + ****************************************************************************** + */ + +#include "matrix.hpp" + +// hat of vector +Matrixf<3, 3> vector3f::hat(Matrixf<3, 1> vec) { + float hat[9] = {0, -vec[2][0], vec[1][0], vec[2][0], 0, + -vec[0][0], -vec[1][0], vec[0][0], 0}; + return Matrixf<3, 3>(hat); +} + +// cross product +Matrixf<3, 1> vector3f::cross(Matrixf<3, 1> vec1, Matrixf<3, 1> vec2) { + return vector3f::hat(vec1) * vec2; +} diff --git a/engineer/User/component/matrix.hpp b/engineer/User/component/matrix.hpp new file mode 100644 index 0000000..97155a5 --- /dev/null +++ b/engineer/User/component/matrix.hpp @@ -0,0 +1,259 @@ +/** + ****************************************************************************** + * @file matrix.cpp/h + * @brief Matrix/vector calculation. 矩阵/向量运算 + * @author Spoon Guan + ****************************************************************************** + * Copyright (c) 2023 Team JiaoLong-SJTU + * All rights reserved. + ****************************************************************************** + */ + +#ifndef MATRIX_H +#define MATRIX_H + +#include "arm_math.h" + +// Matrix class +template +class Matrixf { + public: + // Constructor without input data + Matrixf(void) : rows_(_rows), cols_(_cols) { + arm_mat_init_f32(&arm_mat_, _rows, _cols, this->data_); + } + // Constructor with input data + Matrixf(float data[_rows * _cols]) : Matrixf() { + memcpy(this->data_, data, _rows * _cols * sizeof(float)); + } + // Copy constructor + Matrixf(const Matrixf<_rows, _cols>& mat) : Matrixf() { + memcpy(this->data_, mat.data_, _rows * _cols * sizeof(float)); + } + // Destructor + ~Matrixf(void) {} + + // Row size + int rows(void) { return _rows; } + // Column size + int cols(void) { return _cols; } + + // Element + float* operator[](const int& row) { return &this->data_[row * _cols]; } + + // Operators + Matrixf<_rows, _cols>& operator=(const Matrixf<_rows, _cols> mat) { + memcpy(this->data_, mat.data_, _rows * _cols * sizeof(float)); + return *this; + } + Matrixf<_rows, _cols>& operator+=(const Matrixf<_rows, _cols> mat) { + arm_status s; + s = arm_mat_add_f32(&this->arm_mat_, &mat.arm_mat_, &this->arm_mat_); + return *this; + } + Matrixf<_rows, _cols>& operator-=(const Matrixf<_rows, _cols> mat) { + arm_status s; + s = arm_mat_sub_f32(&this->arm_mat_, &mat.arm_mat_, &this->arm_mat_); + return *this; + } + Matrixf<_rows, _cols>& operator*=(const float& val) { + arm_status s; + s = arm_mat_scale_f32(&this->arm_mat_, val, &this->arm_mat_); + return *this; + } + Matrixf<_rows, _cols>& operator/=(const float& val) { + arm_status s; + s = arm_mat_scale_f32(&this->arm_mat_, 1.f / val, &this->arm_mat_); + return *this; + } + Matrixf<_rows, _cols> operator+(const Matrixf<_rows, _cols>& mat) { + arm_status s; + Matrixf<_rows, _cols> res; + s = arm_mat_add_f32(&this->arm_mat_, &mat.arm_mat_, &res.arm_mat_); + return res; + } + Matrixf<_rows, _cols> operator-(const Matrixf<_rows, _cols>& mat) { + arm_status s; + Matrixf<_rows, _cols> res; + s = arm_mat_sub_f32(&this->arm_mat_, &mat.arm_mat_, &res.arm_mat_); + return res; + } + Matrixf<_rows, _cols> operator*(const float& val) { + arm_status s; + Matrixf<_rows, _cols> res; + s = arm_mat_scale_f32(&this->arm_mat_, val, &res.arm_mat_); + return res; + } + friend Matrixf<_rows, _cols> operator*(const float& val, + const Matrixf<_rows, _cols>& mat) { + arm_status s; + Matrixf<_rows, _cols> res; + s = arm_mat_scale_f32(&mat.arm_mat_, val, &res.arm_mat_); + return res; + } + Matrixf<_rows, _cols> operator/(const float& val) { + arm_status s; + Matrixf<_rows, _cols> res; + s = arm_mat_scale_f32(&this->arm_mat_, 1.f / val, &res.arm_mat_); + return res; + } + // Matrix multiplication + template + friend Matrixf<_rows, cols> operator*(const Matrixf<_rows, _cols>& mat1, + const Matrixf<_cols, cols>& mat2) { + arm_status s; + Matrixf<_rows, cols> res; + s = arm_mat_mult_f32(&mat1.arm_mat_, &mat2.arm_mat_, &res.arm_mat_); + return res; + } + + // Submatrix + template + Matrixf block(const int& start_row, const int& start_col) { + Matrixf res; + for (int row = start_row; row < start_row + rows; row++) { + memcpy((float*)res[0] + (row - start_row) * cols, + (float*)this->data_ + row * _cols + start_col, + cols * sizeof(float)); + } + return res; + } + // Specific row + Matrixf<1, _cols> row(const int& row) { return block<1, _cols>(row, 0); } + // Specific column + Matrixf<_rows, 1> col(const int& col) { return block<_rows, 1>(0, col); } + + // Transpose + Matrixf<_cols, _rows> trans(void) { + Matrixf<_cols, _rows> res; + arm_mat_trans_f32(&arm_mat_, &res.arm_mat_); + return res; + } + // Trace + float trace(void) { + float res = 0; + for (int i = 0; i < fmin(_rows, _cols); i++) { + res += (*this)[i][i]; + } + return res; + } + // Norm + float norm(void) { return sqrtf((this->trans() * *this)[0][0]); } + + public: + // arm matrix instance + arm_matrix_instance_f32 arm_mat_; + + protected: + // size + int rows_, cols_; + // data + float data_[_rows * _cols]; +}; + +// Matrix funtions +namespace matrixf { + +// Special Matrices +// Zero matrix +template +Matrixf<_rows, _cols> zeros(void) { + float data[_rows * _cols] = {0}; + return Matrixf<_rows, _cols>(data); +} +// Ones matrix +template +Matrixf<_rows, _cols> ones(void) { + float data[_rows * _cols] = {0}; + for (int i = 0; i < _rows * _cols; i++) { + data[i] = 1; + } + return Matrixf<_rows, _cols>(data); +} +// Identity matrix +template +Matrixf<_rows, _cols> eye(void) { + float data[_rows * _cols] = {0}; + for (int i = 0; i < fmin(_rows, _cols); i++) { + data[i * _cols + i] = 1; + } + return Matrixf<_rows, _cols>(data); +} +// Diagonal matrix +template +Matrixf<_rows, _cols> diag(Matrixf<_rows, 1> vec) { + Matrixf<_rows, _cols> res = matrixf::zeros<_rows, _cols>(); + for (int i = 0; i < fmin(_rows, _cols); i++) { + res[i][i] = vec[i][0]; + } + return res; +} + +// Inverse +template +Matrixf<_dim, _dim> inv(Matrixf<_dim, _dim> mat) { + arm_status s; + // extended matrix [A|I] + Matrixf<_dim, 2 * _dim> ext_mat = matrixf::zeros<_dim, 2 * _dim>(); + for (int i = 0; i < _dim; i++) { + memcpy(ext_mat[i], mat[i], _dim * sizeof(float)); + ext_mat[i][_dim + i] = 1; + } + // elimination + for (int i = 0; i < _dim; i++) { + // find maximum absolute value in the first column in lower right block + float abs_max = fabs(ext_mat[i][i]); + int abs_max_row = i; + for (int row = i; row < _dim; row++) { + if (abs_max < fabs(ext_mat[row][i])) { + abs_max = fabs(ext_mat[row][i]); + abs_max_row = row; + } + } + if (abs_max < 1e-12f) { // singular + return matrixf::zeros<_dim, _dim>(); + s = ARM_MATH_SINGULAR; + } + if (abs_max_row != i) { // row exchange + float tmp; + Matrixf<1, 2 * _dim> row_i = ext_mat.row(i); + Matrixf<1, 2 * _dim> row_abs_max = ext_mat.row(abs_max_row); + memcpy(ext_mat[i], row_abs_max[0], 2 * _dim * sizeof(float)); + memcpy(ext_mat[abs_max_row], row_i[0], 2 * _dim * sizeof(float)); + } + float k = 1.f / ext_mat[i][i]; + for (int col = i; col < 2 * _dim; col++) { + ext_mat[i][col] *= k; + } + for (int row = 0; row < _dim; row++) { + if (row == i) { + continue; + } + k = ext_mat[row][i]; + for (int j = i; j < 2 * _dim; j++) { + ext_mat[row][j] -= k * ext_mat[i][j]; + } + } + } + // inv = ext_mat(:,n+1:2n) + s = ARM_MATH_SUCCESS; + Matrixf<_dim, _dim> res; + for (int i = 0; i < _dim; i++) { + memcpy(res[i], &ext_mat[i][_dim], _dim * sizeof(float)); + } + return res; +} + +} // namespace matrixf + +namespace vector3f { + +// hat of vector +Matrixf<3, 3> hat(Matrixf<3, 1> vec); + +// cross product +Matrixf<3, 1> cross(Matrixf<3, 1> vec1, Matrixf<3, 1> vec2); + +} // namespace vector3f + +#endif // MATRIX_H diff --git a/engineer/User/component/robotics.cpp b/engineer/User/component/robotics.cpp new file mode 100644 index 0000000..eebb0e5 --- /dev/null +++ b/engineer/User/component/robotics.cpp @@ -0,0 +1,340 @@ +/** + ****************************************************************************** + * @file robotics.cpp/h + * @brief Robotic toolbox on STM32. STM32机器人学库 + * @author Spoon Guan + * @ref [1] SJTU ME385-2, Robotics, Y.Ding + * [2] Bruno Siciliano, et al., Robotics: Modelling, Planning and + * Control, Springer, 2010. + * [3] R.Murry, Z.X.Li, and S.Sastry, A Mathematical Introduction + * to Robotic Manipulation, CRC Press, 1994. + ****************************************************************************** + * Copyright (c) 2023 Team JiaoLong-SJTU + * All rights reserved. + ****************************************************************************** + */ + +#include "robotics.hpp" + +Matrixf<3, 1> robotics::r2rpy(Matrixf<3, 3> R) { + float rpy[3] = { + atan2f(R[1][0], R[0][0]), // yaw + atan2f(-R[2][0], sqrtf(R[2][1] * R[2][1] + R[2][2] * R[2][2])), // pitch + atan2f(R[2][1], R[2][2]) // roll + }; + return Matrixf<3, 1>(rpy); +} + +Matrixf<3, 3> robotics::rpy2r(Matrixf<3, 1> rpy) { + float c[3] = {cosf(rpy[0][0]), cosf(rpy[1][0]), cosf(rpy[2][0])}; + float s[3] = {sinf(rpy[0][0]), sinf(rpy[1][0]), sinf(rpy[2][0])}; + float R[9] = { + c[0] * c[1], // R11 + c[0] * s[1] * s[2] - s[0] * c[2], // R12 + c[0] * s[1] * c[2] + s[0] * s[2], // R13 + s[0] * c[1], // R21 + s[0] * s[1] * s[2] + c[0] * c[2], // R22 + s[0] * s[1] * c[2] - c[0] * s[2], // R23 + -s[1], // R31 + c[1] * s[2], // R32 + c[1] * c[2] // R33 + }; + return Matrixf<3, 3>(R); +} + +Matrixf<4, 1> robotics::r2angvec(Matrixf<3, 3> R) { + float theta = acosf(math::limit(0.5f * (R.trace() - 1), -1, 1)); + if (theta == 0 || theta == PI) { + float angvec[4] = { + (1 + R[0][0] - R[1][1] - R[2][2]) / 4.0f, // rx=(1+R11-R22-R33)/4 + (1 - R[0][0] + R[1][1] - R[2][2]) / 4.0f, // ry=(1-R11+R22-R33)/4 + (1 - R[0][0] - R[1][1] + R[2][2]) / 4.0f, // rz=(1-R11-R22+R33)/4 + theta // theta + }; + return Matrixf<4, 1>(angvec); + } + float angvec[4] = { + (R[2][1] - R[1][2]) / (2.0f * sinf(theta)), // rx=(R32-R23)/2sinθ + (R[0][2] - R[2][0]) / (2.0f * sinf(theta)), // ry=(R13-R31)/2sinθ + (R[1][0] - R[0][1]) / (2.0f * sinf(theta)), // rz=(R21-R12)/2sinθ + theta // theta + }; + return Matrixf<4, 1>(angvec); +} + +Matrixf<3, 3> robotics::angvec2r(Matrixf<4, 1> angvec) { + float theta = angvec[3][0]; + Matrixf<3, 1> r = angvec.block<3, 1>(0, 0); + Matrixf<3, 3> R; + // Rodrigues formula: R=I+ω^sinθ+ω^^2(1-cosθ) + R = matrixf::eye<3, 3>() + vector3f::hat(r) * sinf(theta) + + vector3f::hat(r) * vector3f::hat(r) * (1 - cosf(theta)); + return R; +} + +Matrixf<4, 1> robotics::r2quat(Matrixf<3, 3> R) { + float q[4] = { + 0.5f * sqrtf(math::limit(R.trace(), -1, 1) + 1), // q0=cos(θ/2) + math::sign(R[2][1] - R[1][2]) * 0.5f * + sqrtf(math::limit(R[0][0] - R[1][1] - R[2][2], -1, 1) + + 1), // q1=rx*sin(θ/2) + math::sign(R[0][2] - R[2][0]) * 0.5f * + sqrtf(math::limit(-R[0][0] + R[1][1] - R[2][2], -1, 1) + + 1), // q2=ry*sin(θ/2) + math::sign(R[1][0] - R[0][1]) * 0.5f * + sqrtf(math::limit(-R[0][0] - R[1][1] + R[2][2], -1, 1) + + 1), // q3=rz*sin(θ/2) + }; + return (Matrixf<4, 1>(q) / Matrixf<4, 1>(q).norm()); +} + +Matrixf<3, 3> robotics::quat2r(Matrixf<4, 1> q) { + float R[9] = { + 1 - 2.0f * (q[2][0] * q[2][0] + q[3][0] * q[3][0]), // R11 + 2.0f * (q[1][0] * q[2][0] - q[0][0] * q[3][0]), // R12 + 2.0f * (q[1][0] * q[3][0] + q[0][0] * q[2][0]), // R13 + 2.0f * (q[1][0] * q[2][0] + q[0][0] * q[3][0]), // R21 + 1 - 2.0f * (q[1][0] * q[1][0] + q[3][0] * q[3][0]), // R22 + 2.0f * (q[2][0] * q[3][0] - q[0][0] * q[1][0]), // R23 + 2.0f * (q[1][0] * q[3][0] - q[0][0] * q[2][0]), // R31 + 2.0f * (q[2][0] * q[3][0] + q[0][0] * q[1][0]), // R32 + 1 - 2.0f * (q[1][0] * q[1][0] + q[2][0] * q[2][0]) // R33 + }; + return Matrixf<3, 3>(R); +} + +Matrixf<3, 1> robotics::quat2rpy(Matrixf<4, 1> q) { + float rpy[3] = { + atan2f(2.0f * (q[1][0] * q[2][0] + q[0][0] * q[3][0]), + 1 - 2.0f * (q[2][0] * q[2][0] + q[3][0] * q[3][0])), // yaw + asinf(2.0f * (q[0][0] * q[2][0] - q[1][0] * q[3][0])), // pitch + atan2f(2.0f * (q[2][0] * q[3][0] + q[0][0] * q[1][0]), + 1 - 2.0f * (q[1][0] * q[1][0] + q[2][0] * q[2][0])) // rol + }; + return Matrixf<3, 1>(rpy); +} + +Matrixf<4, 1> robotics::rpy2quat(Matrixf<3, 1> rpy) { + float c[3] = {cosf(0.5f * rpy[0][0]), cosf(0.5f * rpy[1][0]), + cosf(0.5f * rpy[2][0])}; // cos(*/2) + float s[3] = {sinf(0.5f * rpy[0][0]), sinf(0.5f * rpy[1][0]), + sinf(0.5f * rpy[2][0])}; // sin(*/2) + float q[4] = { + c[0] * c[1] * c[2] + s[0] * s[1] * s[2], // q0=cos(θ/2) + c[0] * c[1] * s[2] - s[0] * s[1] * c[2], // q1=rx*sin(θ/2) + c[0] * s[1] * c[2] + s[0] * c[1] * s[2], // q2=ry*sin(θ/2) + s[0] * c[1] * c[2] - c[0] * s[1] * s[2] // q3=rz*sin(θ/2) + }; + return (Matrixf<4, 1>(q) / Matrixf<4, 1>(q).norm()); +} + +Matrixf<4, 1> robotics::quat2angvec(Matrixf<4, 1> q) { + float cosq0; + float theta = 2.0f * acosf(math::limit(q[0][0], -1, 1)); + if (theta == 0 || theta == PI) { + float angvec[4] = {0, 0, 0, theta}; // θ=0||PI, return[0;θ] + return Matrixf<4, 1>(angvec); + } + Matrixf<3, 1> vec = q.block<3, 1>(1, 0); + float angvec[4] = { + vec[0][0] / vec.norm(), // rx + vec[1][0] / vec.norm(), // ry + vec[2][0] / vec.norm(), // rz + theta // theta + }; + return Matrixf<4, 1>(angvec); +} + +Matrixf<4, 1> robotics::angvec2quat(Matrixf<4, 1> angvec) { + float c = cosf(0.5f * angvec[3][0]), s = sinf(0.5f * angvec[3][0]); + float q[4] = { + c, // q0=cos(θ/2) + s * angvec[0][0], // q1=rx*sin(θ/2) + s * angvec[1][0], // q2=ry*sin(θ/2) + s * angvec[2][0] // q3=rz*sin(θ/2) + }; + return Matrixf<4, 1>(q) / Matrixf<4, 1>(q).norm(); +} + +Matrixf<3, 3> robotics::t2r(Matrixf<4, 4> T) { + return T.block<3, 3>(0, 0); // R=T(1:3,1:3) +} + +Matrixf<4, 4> robotics::r2t(Matrixf<3, 3> R) { + // T=[R,0;0,1] + float T[16] = {R[0][0], R[0][1], R[0][2], 0, R[1][0], R[1][1], R[1][2], 0, + R[2][0], R[2][1], R[2][2], 0, 0, 0, 0, 1}; + return Matrixf<4, 4>(T); +} + +Matrixf<3, 1> robotics::t2p(Matrixf<4, 4> T) { + return T.block<3, 1>(0, 3); // p=T(1:3,4) +} + +Matrixf<4, 4> robotics::p2t(Matrixf<3, 1> p) { + // T=[I,P;0,1] + float T[16] = {1, 0, 0, p[0][0], 0, 1, 0, p[1][0], + 0, 0, 1, p[2][0], 0, 0, 0, 1}; + return Matrixf<4, 4>(T); +} + +Matrixf<4, 4> robotics::rp2t(Matrixf<3, 3> R, Matrixf<3, 1> p) { + // T=[R,P;0,1] + float T[16] = {R[0][0], R[0][1], R[0][2], p[0][0], R[1][0], R[1][1], + R[1][2], p[1][0], R[2][0], R[2][1], R[2][2], p[2][0], + 0, 0, 0, 1}; + return Matrixf<4, 4>(T); +} + +Matrixf<4, 4> robotics::invT(Matrixf<4, 4> T) { + Matrixf<3, 3> RT = t2r(T).trans(); + Matrixf<3, 1> p_ = -1.0f * RT * t2p(T); + float invT[16] = {RT[0][0], RT[0][1], RT[0][2], p_[0][0], RT[1][0], RT[1][1], + RT[1][2], p_[1][0], RT[2][0], RT[2][1], RT[2][2], p_[2][0], + 0, 0, 0, 1}; + return Matrixf<4, 4>(invT); +} + +Matrixf<3, 1> robotics::t2rpy(Matrixf<4, 4> T) { + return r2rpy(t2r(T)); +} + +Matrixf<4, 4> robotics::rpy2t(Matrixf<3, 1> rpy) { + return r2t(rpy2r(rpy)); +} + +Matrixf<4, 1> robotics::t2angvec(Matrixf<4, 4> T) { + return r2angvec(t2r(T)); +} + +Matrixf<4, 4> robotics::angvec2t(Matrixf<4, 1> angvec) { + return r2t(angvec2r(angvec)); +} + +Matrixf<4, 1> robotics::t2quat(Matrixf<4, 4> T) { + return r2quat(t2r(T)); +} + +Matrixf<4, 4> robotics::quat2t(Matrixf<4, 1> quat) { + return r2t(quat2r(quat)); +} + +Matrixf<6, 1> robotics::t2twist(Matrixf<4, 4> T) { + Matrixf<3, 1> p = t2p(T); + Matrixf<4, 1> angvec = t2angvec(T); + Matrixf<3, 1> phi = angvec.block<3, 1>(0, 0) * angvec[3][0]; + float twist[6] = {p[0][0], p[1][0], p[2][0], phi[0][0], phi[1][0], phi[2][0]}; + return Matrixf<6, 1>(twist); +} + +Matrixf<4, 4> robotics::twist2t(Matrixf<6, 1> twist) { + Matrixf<3, 1> p = twist.block<3, 1>(0, 0); + float theta = twist.block<3, 1>(3, 0).norm(); + float angvec[4] = {0, 0, 0, theta}; + if (theta != 0) { + angvec[0] = twist[3][0] / theta; + angvec[1] = twist[4][0] / theta; + angvec[2] = twist[5][0] / theta; + } + return rp2t(angvec2r(angvec), p); +} + +Matrixf<4, 4> robotics::DH_t::fkine() { + float ct = cosf(theta), st = sinf(theta); // cosθ, sinθ + float ca = cosf(alpha), sa = sinf(alpha); // cosα, sinα + + // T = + // | cθ -sθcα sθsα acθ | + // | sθ cθcα -cθsα asθ | + // | 0 sα cα d | + // | 0 0 0 1 | + T[0][0] = ct; + T[0][1] = -st * ca; + T[0][2] = st * sa; + T[0][3] = a * ct; + + T[1][0] = st; + T[1][1] = ct * ca; + T[1][2] = -ct * sa; + T[1][3] = a * st; + + T[2][0] = 0; + T[2][1] = sa; + T[2][2] = ca; + T[2][3] = d; + + T[3][0] = 0; + T[3][1] = 0; + T[3][2] = 0; + T[3][3] = 1; + + return T; +} + +robotics::Link::Link(float theta, + float d, + float a, + float alpha, + robotics::Joint_Type_e type, + float offset, + float qmin, + float qmax, + float m, + Matrixf<3, 1> rc, + Matrixf<3, 3> I) { + dh_.theta = theta; + dh_.d = d; + dh_.alpha = alpha; + dh_.a = a; + type_ = type; + offset_ = offset; + qmin_ = qmin; + qmax_ = qmax; + m_ = m; + rc_ = rc; + I_ = I; +} + +robotics::Link::Link(const Link& link) { + dh_.theta = link.dh_.theta; + dh_.d = link.dh_.d; + dh_.alpha = link.dh_.alpha; + dh_.a = link.dh_.a; + type_ = link.type_; + offset_ = link.offset_; + qmin_ = link.qmin_; + qmax_ = link.qmax_; + m_ = link.m_; + rc_ = link.rc_; + I_ = link.I_; +} + +robotics::Link& robotics::Link::operator=(Link link) { + dh_.theta = link.dh_.theta; + dh_.d = link.dh_.d; + dh_.alpha = link.dh_.alpha; + dh_.a = link.dh_.a; + type_ = link.type_; + offset_ = link.offset_; + qmin_ = link.qmin_; + qmax_ = link.qmax_; + m_ = link.m_; + rc_ = link.rc_; + I_ = link.I_; + return *this; +} + +Matrixf<4, 4> robotics::Link::T(float q) { + if (type_ == R) { + if (qmin_ >= qmax_) + dh_.theta = q + offset_; + else + dh_.theta = math::limit(q + offset_, qmin_, qmax_); + } else { + if (qmin_ >= qmax_) + dh_.d = q + offset_; + else + dh_.d = math::limit(q + offset_, qmin_, qmax_); + } + return dh_.fkine(); +} diff --git a/engineer/User/component/robotics.hpp b/engineer/User/component/robotics.hpp new file mode 100644 index 0000000..a9b108e --- /dev/null +++ b/engineer/User/component/robotics.hpp @@ -0,0 +1,407 @@ +/** + ****************************************************************************** + * @file robotics.cpp/h + * @brief Robotic toolbox on STM32. STM32机器人学库 + * @author Spoon Guan + * @ref [1] SJTU ME385-2, Robotics, Y.Ding + * [2] Bruno Siciliano, et al., Robotics: Modelling, Planning and + * Control, Springer, 2010. + * [3] R.Murry, Z.X.Li, and S.Sastry, A Mathematical Introduction + * to Robotic Manipulation, CRC Press, 1994. + ****************************************************************************** + * Copyright (c) 2023 Team JiaoLong-SJTU + * All rights reserved. + ****************************************************************************** + */ + +#ifndef ROBOTICS_H +#define ROBOTICS_H + +#include "utils.hpp" +#include "matrix.hpp" + +namespace robotics { +// rotation matrix(R) -> RPY([yaw;pitch;roll]) +Matrixf<3, 1> r2rpy(Matrixf<3, 3> R); +// RPY([yaw;pitch;roll]) -> rotation matrix(R) +Matrixf<3, 3> rpy2r(Matrixf<3, 1> rpy); +// rotation matrix(R) -> angle vector([r;θ]) +Matrixf<4, 1> r2angvec(Matrixf<3, 3> R); +// angle vector([r;θ]) -> rotation matrix(R) +Matrixf<3, 3> angvec2r(Matrixf<4, 1> angvec); +// rotation matrix(R) -> quaternion, [q0;q1;q2;q3]=[cos(θ/2);rsin(θ/2)] +Matrixf<4, 1> r2quat(Matrixf<3, 3> R); +// quaternion, [q0;q1;q2;q3]=[cos(θ/2);rsin(θ/2)] -> rotation matrix(R) +Matrixf<3, 3> quat2r(Matrixf<4, 1> quat); +// quaternion, [q0;q1;q2;q3]=[cos(θ/2);rsin(θ/2)] -> RPY([yaw;pitch;roll]) +Matrixf<3, 1> quat2rpy(Matrixf<4, 1> q); +// RPY([yaw;pitch;roll]) -> quaternion, [q0;q1;q2;q3]=[cos(θ/2);rsin(θ/2)] +Matrixf<4, 1> rpy2quat(Matrixf<3, 1> rpy); +// quaternion, [q0;q1;q2;q3]=[cos(θ/2);rsin(θ/2)] -> angle vector([r;θ]) +Matrixf<4, 1> quat2angvec(Matrixf<4, 1> q); +// angle vector([r;θ]) -> quaternion, [q0;q1;q2;q3]=[cos(θ/2);rsin(θ/2)] +Matrixf<4, 1> angvec2quat(Matrixf<4, 1> angvec); +// homogeneous transformation matrix(T) -> rotation matrix(R) +Matrixf<3, 3> t2r(Matrixf<4, 4> T); +// rotation matrix(R) -> homogeneous transformation matrix(T) +Matrixf<4, 4> r2t(Matrixf<3, 3> R); +// homogeneous transformation matrix(T) -> translation vector(p) +Matrixf<3, 1> t2p(Matrixf<4, 4> T); +// translation vector(p) -> homogeneous transformation matrix(T) +Matrixf<4, 4> p2t(Matrixf<3, 1> p); +// rotation matrix(R) & translation vector(p) -> homogeneous transformation +// matrix(T) +Matrixf<4, 4> rp2t(Matrixf<3, 3> R, Matrixf<3, 1> p); +// homogeneous transformation matrix(T) -> RPY([yaw;pitch;roll]) +Matrixf<3, 1> t2rpy(Matrixf<4, 4> T); +// inverse of homogeneous transformation matrix(T^-1=[R',-R'P;0,1]) +Matrixf<4, 4> invT(Matrixf<4, 4> T); +// RPY([yaw;pitch;roll]) -> homogeneous transformation matrix(T) +Matrixf<4, 4> rpy2t(Matrixf<3, 1> rpy); +// homogeneous transformation matrix(T) -> angle vector([r;θ]) +Matrixf<4, 1> t2angvec(Matrixf<4, 4> T); +// angle vector([r;θ]) -> homogeneous transformation matrix(T) +Matrixf<4, 4> angvec2t(Matrixf<4, 1> angvec); +// homogeneous transformation matrix(T) -> quaternion, +// [q0;q1;q2;q3]=[cos(θ/2);rsin(θ/2)] +Matrixf<4, 1> t2quat(Matrixf<4, 4> T); +// quaternion, [q0;q1;q2;q3]=[cos(θ/2);rsin(θ/2)] -> homogeneous transformation +// matrix(T) +Matrixf<4, 4> quat2t(Matrixf<4, 1> quat); +// homogeneous transformation matrix(T) -> twist coordinates vector ([p;rθ]) +Matrixf<6, 1> t2twist(Matrixf<4, 4> T); +// twist coordinates vector ([p;rθ]) -> homogeneous transformation matrix(T) +Matrixf<4, 4> twist2t(Matrixf<6, 1> twist); + +// joint type: R-revolute joint, P-prismatic joint +typedef enum joint_type { + R = 0, + P = 1, +} Joint_Type_e; + +// Denavit–Hartenberg(DH) method +struct DH_t { + // forward kinematic + Matrixf<4, 4> fkine(); + // DH parameter + float theta; + float d; + float a; + float alpha; + Matrixf<4, 4> T; +}; + +class Link { + public: + Link(){}; + Link(float theta, float d, float a, float alpha, Joint_Type_e type = R, + float offset = 0, float qmin = 0, float qmax = 0, float m = 1, + Matrixf<3, 1> rc = matrixf::zeros<3, 1>(), + Matrixf<3, 3> I = matrixf::zeros<3, 3>()); + Link(const Link& link); + + Link& operator=(Link link); + + float qmin() { return qmin_; } + float qmax() { return qmax_; } + Joint_Type_e type() { return type_; } + float m() { return m_; } + Matrixf<3, 1> rc() { return rc_; } + Matrixf<3, 3> I() { return I_; } + + Matrixf<4, 4> T(float q); // forward kinematic + + public: + // kinematic parameter + DH_t dh_; + float offset_; + // limit(qmin,qmax), no limit if qmin<=qmax + float qmin_; + float qmax_; + // joint type + Joint_Type_e type_; + // dynamic parameter + float m_; // mass + Matrixf<3, 1> rc_; // centroid(link coordinate) + Matrixf<3, 3> I_; // inertia tensor(3*3) +}; + +template +class Serial_Link { + public: + Serial_Link(Link links[_n]) { + for (int i = 0; i < _n; i++) + links_[i] = links[i]; + gravity_ = matrixf::zeros<3, 1>(); + gravity_[2][0] = -9.81f; + } + + Serial_Link(Link links[_n], Matrixf<3, 1> gravity) { + for (int i = 0; i < _n; i++) + links_[i] = links[i]; + gravity_ = gravity; + } + + // forward kinematic: T_n^0 + // param[in] q: joint variable vector + // param[out] T_n^0 + Matrixf<4, 4> fkine(Matrixf<_n, 1> q) { + T_ = matrixf::eye<4, 4>(); + for (int iminus1 = 0; iminus1 < _n; iminus1++) + T_ = T_ * links_[iminus1].T(q[iminus1][0]); + return T_; + } + + // forward kinematic: T_k^0 + // param[in] q: joint variable vector + // param[in] k: joint number + // param[out] T_k^0 + Matrixf<4, 4> fkine(Matrixf<_n, 1> q, uint16_t k) { + if (k > _n) + k = _n; + Matrixf<4, 4> T = matrixf::eye<4, 4>(); + for (int iminus1 = 0; iminus1 < k; iminus1++) + T = T * links_[iminus1].T(q[iminus1][0]); + return T; + } + + // T_k^k-1: homogeneous transformation matrix of link k + // param[in] q: joint variable vector + // param[in] kminus: joint number k, input k-1 + // param[out] T_k^k-1 + Matrixf<4, 4> T(Matrixf<_n, 1> q, uint16_t kminus1) { + if (kminus1 >= _n) + kminus1 = _n - 1; + return links_[kminus1].T(q[kminus1][0]); + } + + // jacobian matrix, J_i = [J_pi;j_oi] + // param[in] q: joint variable vector + // param[out] jacobian matix J_6*n + Matrixf<6, _n> jacob(Matrixf<_n, 1> q) { + Matrixf<3, 1> p_e = t2p(fkine(q)); // p_e + Matrixf<4, 4> T_iminus1 = matrixf::eye<4, 4>(); // T_i-1^0 + Matrixf<3, 1> z_iminus1; // z_i-1^0 + Matrixf<3, 1> p_iminus1; // p_i-1^0 + Matrixf<3, 1> J_pi; + Matrixf<3, 1> J_oi; + for (int iminus1 = 0; iminus1 < _n; iminus1++) { + // revolute joint: J_pi = z_i-1x(p_e-p_i-1), J_oi = z_i-1 + if (links_[iminus1].type() == R) { + z_iminus1 = T_iminus1.block<3, 1>(0, 2); + p_iminus1 = t2p(T_iminus1); + T_iminus1 = T_iminus1 * links_[iminus1].T(q[iminus1][0]); + J_pi = vector3f::cross(z_iminus1, p_e - p_iminus1); + J_oi = z_iminus1; + } + // prismatic joint: J_pi = z_i-1, J_oi = 0 + else { + z_iminus1 = T_iminus1.block<3, 1>(0, 2); + T_iminus1 = T_iminus1 * links_[iminus1].T(q[iminus1][0]); + J_pi = z_iminus1; + J_oi = matrixf::zeros<3, 1>(); + } + J_[0][iminus1] = J_pi[0][0]; + J_[1][iminus1] = J_pi[1][0]; + J_[2][iminus1] = J_pi[2][0]; + J_[3][iminus1] = J_oi[0][0]; + J_[4][iminus1] = J_oi[1][0]; + J_[5][iminus1] = J_oi[2][0]; + } + return J_; + } + + // inverse kinematic, numerical solution(Newton method) + // param[in] T: homogeneous transformation matrix of end effector + // param[in] q: initial joint variable vector(q0) for Newton method's + // iteration + // param[in] tol: tolerance of error (norm(error of twist vector)) + // param[in] max_iter: maximum iterations, default 30 + // param[out] q: joint variable vector + Matrixf<_n, 1> ikine(Matrixf<4, 4> Td, + Matrixf<_n, 1> q = matrixf::zeros<_n, 1>(), + float tol = 1e-4f, uint16_t max_iter = 50) { + Matrixf<4, 4> T; + Matrixf<3, 1> pe, we; + Matrixf<6, 1> err, new_err; + Matrixf<_n, 1> dq; + float step = 1; + for (int i = 0; i < max_iter; i++) { + T = fkine(q); + pe = t2p(Td) - t2p(T); + // angvec(Td*T^-1), transform angular vector(T->Td) in world coordinate + we = t2twist(Td * invT(T)).block<3, 1>(3, 0); + for (int i = 0; i < 3; i++) { + err[i][0] = pe[i][0]; + err[i + 3][0] = we[i][0]; + } + if (err.norm() < tol) + return q; + // adjust iteration step + Matrixf<6, _n> J = jacob(q); + for (int j = 0; j < 5; j++) { + dq = matrixf::inv(J.trans() * J) * (J.trans() * err) * step; + if (dq[0][0] == INFINITY) // J'*J singular + { + dq = matrixf::inv(J.trans() * J + 0.1f * matrixf::eye<_n, _n>()) * + J.trans() * err * step; + // SVD<6, _n> JTJ_svd(J.trans() * J); + // dq = JTJ_svd.solve(err) * step * 5e-2f; + q += dq; + for (int i = 0; i < _n; i++) { + if (links_[i].type() == R) + q[i][0] = math::loopLimit(q[i][0], -PI, PI); + } + break; + } + T = fkine(q + dq); + pe = t2p(Td) - t2p(T); + we = t2twist(Td * invT(T)).block<3, 1>(3, 0); + for (int i = 0; i < 3; i++) { + new_err[i][0] = pe[i][0]; + new_err[i + 3][0] = we[i][0]; + } + if (new_err.norm() < err.norm()) { + q += dq; + for (int i = 0; i < _n; i++) { + if (links_[i].type() == robotics::Joint_Type_e::R) { + q[i][0] = math::loopLimit(q[i][0], -PI, PI); + } + } + break; + } else { + step /= 2.0f; + } + } + if (step < 1e-3f) + return q; + } + return q; + } + + // (Reserved function) inverse kinematic, analytic solution(geometric method) + Matrixf<_n, 1> (*ikine_analytic)(Matrixf<4, 4> T); + + // inverse dynamic, Newton-Euler method + // param[in] q: joint variable vector + // param[in] qv: dq/dt + // param[in] qa: d^2q/dt^2 + // param[in] he: load on end effector [f;μ], default 0 + Matrixf<_n, 1> rne(Matrixf<_n, 1> q, + Matrixf<_n, 1> qv = matrixf::zeros<_n, 1>(), + Matrixf<_n, 1> qa = matrixf::zeros<_n, 1>(), + Matrixf<6, 1> he = matrixf::zeros<6, 1>()) { + // forward propagation + // record each links' motion state in matrices + // [ωi] angular velocity + Matrixf<3, _n + 1> w = matrixf::zeros<3, _n + 1>(); + // [βi] angular acceleration + Matrixf<3, _n + 1> b = matrixf::zeros<3, _n + 1>(); + // [pi] position of joint + Matrixf<3, _n + 1> p = matrixf::zeros<3, _n + 1>(); + // [vi] velocity of joint + Matrixf<3, _n + 1> v = matrixf::zeros<3, _n + 1>(); + // [ai] acceleration of joint + Matrixf<3, _n + 1> a = matrixf::zeros<3, _n + 1>(); + // [aci] acceleration of mass center + Matrixf<3, _n + 1> ac = matrixf::zeros<3, _n + 1>(); + // temperary vectors + Matrixf<3, 1> w_i, b_i, p_i, v_i, ai, ac_i; + // i & i-1 coordinate convert to 0 coordinate + Matrixf<4, 4> T_0i = matrixf::eye<4, 4>(); + Matrixf<4, 4> T_0iminus1 = matrixf::eye<4, 4>(); + Matrixf<3, 3> R_0i = matrixf::eye<3, 3>(); + Matrixf<3, 3> R_0iminus1 = matrixf::eye<3, 3>(); + // unit vector of z-axis + Matrixf<3, 1> ez = matrixf::zeros<3, 1>(); + ez[2][0] = 1; + + for (int i = 1; i <= _n; i++) { + T_0i = T_0i * T(q, i - 1); // T_i^0 + R_0i = t2r(T_0i); // R_i^0 + R_0iminus1 = t2r(T_0iminus1); // R_i-1^0 + // ω_i = ω_i-1+qv_i*R_i-1^0*ez + w_i = w.col(i - 1) + qv[i - 1][0] * R_0iminus1 * ez; + // β_i = β_i-1+ω_i-1x(qv_i*R_i-1^0*ez)+qa_i*R_i-1^0*ez + b_i = b.col(i - 1) + + vector3f::cross(w.col(i - 1), qv[i - 1][0] * R_0iminus1 * ez) + + qa[i - 1][0] * R_0iminus1 * ez; + p_i = t2p(T_0i); // p_i = T_i^0(1:3,4) + // v_i = v_i-1+ω_ix(p_i-p_i-1) + v_i = v.col(i - 1) + vector3f::cross(w_i, p_i - p.col(i - 1)); + // a_i = a_i-1+β_ix(p_i-p_i-1)+ω_ix(ω_ix(p_i-p_i-1)) + ai = a.col(i - 1) + vector3f::cross(b_i, p_i - p.col(i - 1)) + + vector3f::cross(w_i, vector3f::cross(w_i, p_i - p.col(i - 1))); + // ac_i = a_i+β_ix(R_0^i*rc_i^i)+ω_ix(ω_ix(R_0^i*rc_i^i)) + ac_i = + ai + vector3f::cross(b_i, R_0i * links_[i - 1].rc()) + + vector3f::cross(w_i, vector3f::cross(w_i, R_0i * links_[i - 1].rc())); + for (int row = 0; row < 3; row++) { + w[row][i] = w_i[row][0]; + b[row][i] = b_i[row][0]; + p[row][i] = p_i[row][0]; + v[row][i] = v_i[row][0]; + a[row][i] = ai[row][0]; + ac[row][i] = ac_i[row][0]; + } + T_0iminus1 = T_0i; // T_i-1^0 + } + + // backward propagation + // record each links' force + Matrixf<3, _n + 1> f = matrixf::zeros<3, _n + 1>(); // joint force + Matrixf<3, _n + 1> mu = matrixf::zeros<3, _n + 1>(); // joint moment + // temperary vector + Matrixf<3, 1> f_iminus1, mu_iminus1; + // {T,R',P}_i^i-1 + Matrixf<4, 4> T_iminus1i; + Matrixf<3, 3> RT_iminus1i; + Matrixf<3, 1> P_iminus1i; + // I_i-1(in 0 coordinate) + Matrixf<3, 3> I_i; + // joint torque + Matrixf<_n, 1> torq; + + // load on end effector + for (int row = 0; row < 3; row++) { + f[row][_n] = he.block<3, 1>(0, 0)[row][0]; + mu[row][_n] = he.block<3, 1>(3, 0)[row][0]; + } + for (int i = _n; i > 0; i--) { + T_iminus1i = T(q, i - 1); // T_i^i-1 + P_iminus1i = t2p(T_iminus1i); // P_i^i-1 + RT_iminus1i = t2r(T_iminus1i).trans(); // R_i^i-1' + R_0iminus1 = R_0i * RT_iminus1i; // R_i-1^0 + // I_i^0 = R_i^0*I_i^i*(R_i^0)' + I_i = R_0i * links_[i - 1].I() * R_0i.trans(); + // f_i-1 = f_i+m_i*ac_i-m_i*g + f_iminus1 = f.col(i) + links_[i - 1].m() * ac.col(i) - + links_[i - 1].m() * gravity_; + // μ_i-1 = μ_i+f_ixrc_i-f_i-1xrc_i-1->ci+I_i*b_i+ω_ix(I_i*ω_i) + mu_iminus1 = mu.col(i) + + vector3f::cross(f.col(i), R_0i * links_[i - 1].rc()) - + vector3f::cross(f_iminus1, R_0i * (RT_iminus1i * P_iminus1i + + links_[i - 1].rc())) + + I_i * b.col(i) + vector3f::cross(w.col(i), I_i * w.col(i)); + // τ_i = μ_i-1'*(R_i-1^0*ez) + torq[i - 1][0] = (mu_iminus1.trans() * R_0iminus1 * ez)[0][0]; + for (int row = 0; row < 3; row++) { + f[row][i - 1] = f_iminus1[row][0]; + mu[row][i - 1] = mu_iminus1[row][0]; + } + R_0i = R_0iminus1; + } + + return torq; + } + + private: + Link links_[_n]; + Matrixf<3, 1> gravity_; + + Matrixf<4, 4> T_; + Matrixf<6, _n> J_; +}; +}; // namespace robotics + +#endif // ROBOTICS_H diff --git a/engineer/User/component/ui.c b/engineer/User/component/ui.c new file mode 100644 index 0000000..c3126bd --- /dev/null +++ b/engineer/User/component/ui.c @@ -0,0 +1,301 @@ +/* + UI相关命令 +*/ +#include "component/ui.h" + +#include + +/** + * @brief UI_绘制直线段 + * + * @param grapic_line 结构体 + * @param name 图形名首地址 + * @param type_op 操作类型 + * @param layer 图层数 + * @param color 颜色 + * @param width 线条宽度 + * @param x_start 起点x坐标 + * @param y_start 起点y坐标 + * @param x_end 终点x坐标 + * @param y_end 终点y坐标 + * @return int8_t + */ +int8_t UI_DrawLine(UI_Ele_t *grapic_line, const char *name, uint8_t type_op, + uint8_t layer, uint8_t color, uint16_t width, + uint16_t x_start, uint16_t y_start, uint16_t x_end, + uint16_t y_end) { + if (grapic_line == NULL) return -1; + snprintf((char *)grapic_line->name, 2, "%s", name); + grapic_line->layer = layer; + grapic_line->type_op = type_op; + grapic_line->type_ele = 0; + grapic_line->color = color; + grapic_line->width = width; + grapic_line->x_start = x_start; + grapic_line->y_start = y_start; + grapic_line->x_end = x_end; + grapic_line->y_end = y_end; + return 0; +} + +/** + * @brief UI_绘制矩形 + * + * @param grapic_rectangle 结构体 + * @param name 图形名首地址 + * @param type_op 操作类型 + * @param layer 图层数 + * @param color 颜色 + * @param width 线条宽度 + * @param x_start 起点x坐标 + * @param y_start 起点y坐标 + * @param x_end 对角顶点x坐标 + * @param y_end 对角顶点y坐标 + * @return int8_t + */ +int8_t UI_DrawRectangle(UI_Ele_t *grapic_rectangle, const char *name, + uint8_t type_op, uint8_t layer, uint8_t color, + uint16_t width, uint16_t x_start, uint16_t y_start, + uint16_t x_end, uint16_t y_end) { + if (grapic_rectangle == NULL) return -1; + snprintf((char *)grapic_rectangle->name, 2, "%s", name); + grapic_rectangle->type_op = type_op; + grapic_rectangle->type_ele = 1; + grapic_rectangle->layer = layer; + grapic_rectangle->color = color; + grapic_rectangle->width = width; + grapic_rectangle->x_start = x_start; + grapic_rectangle->y_start = y_start; + grapic_rectangle->x_end = x_end; + grapic_rectangle->y_end = y_end; + return 0; +} + +/** + * @brief UI_绘制正圆 + * + * @param grapic_cycle 结构体 + * @param name 图形名首地址 + * @param type_op 操作类型 + * @param layer 图层数 + * @param color 颜色 + * @param width 线条宽度 + * @param x_center 圆心x坐标 + * @param y_center 圆心y坐标 + * @param radius 半径 + * @return int8_t + */ +int8_t UI_DrawCycle(UI_Ele_t *grapic_cycle, const char *name, uint8_t type_op, + uint8_t layer, uint8_t color, uint16_t width, + uint16_t x_center, uint16_t y_center, uint16_t radius) { + if (grapic_cycle == NULL) return -1; + snprintf((char *)grapic_cycle->name, 2, "%s", name); + grapic_cycle->type_op = type_op; + grapic_cycle->layer = layer; + grapic_cycle->type_ele = 2; + grapic_cycle->color = color; + grapic_cycle->width = width; + grapic_cycle->x_start = x_center; + grapic_cycle->y_start = y_center; + grapic_cycle->radius = radius; + return 0; +} + +/** + * @brief UI_绘制椭圆 + * + * @param grapic_oval 结构体 + * @param name 图形名首地址 + * @param type_op 操作类型 + * @param layer 图层数 + * @param color 颜色 + * @param width 线条宽度 + * @param x_center 圆心x坐标 + * @param y_center 圆心y坐标 + * @param x_semiaxis x半轴长度 + * @param y_semiaxis y半轴长度 + * @return int8_t + */ +int8_t UI_DrawOval(UI_Ele_t *grapic_oval, const char *name, uint8_t type_op, + uint8_t layer, uint8_t color, uint16_t width, + uint16_t x_center, uint16_t y_center, uint16_t x_semiaxis, + uint16_t y_semiaxis) { + if (grapic_oval == NULL) return -1; + snprintf((char *)grapic_oval->name, 2, "%s", name); + grapic_oval->type_op = type_op; + grapic_oval->type_ele = 3; + grapic_oval->layer = layer; + grapic_oval->color = color; + grapic_oval->width = width; + grapic_oval->x_start = x_center; + grapic_oval->y_start = y_center; + grapic_oval->x_end = x_semiaxis; + grapic_oval->y_end = y_semiaxis; + return 0; +} + +/** + * @brief UI_绘制圆弧 + * + * @param grapic_arc 结构体 + * @param name 图形名首地址 + * @param type_op 操作类型 + * @param layer 图层数 + * @param color 颜色 + * @param angle_start 起始角度 + * @param angle_end 终止角度 + * @param width 线条宽度 + * @param x_center 圆心x坐标 + * @param y_center 圆心y坐标 + * @param x_semiaxis x半轴长度 + * @param y_semiaxis y半轴长度 + * @return int8_t + */ +int8_t UI_DrawArc(UI_Ele_t *grapic_arc, const char *name, uint8_t type_op, + uint8_t layer, uint8_t color, uint16_t angle_start, + uint16_t angle_end, uint16_t width, uint16_t x_center, + uint16_t y_center, uint16_t x_semiaxis, uint16_t y_semiaxis) { + if (grapic_arc == NULL) return -1; + snprintf((char *)grapic_arc->name, 2, "%s", name); + grapic_arc->type_op = type_op; + grapic_arc->type_ele = 4; + grapic_arc->layer = layer; + grapic_arc->color = color; + grapic_arc->angle_start = angle_start; + grapic_arc->angle_end = angle_end; + grapic_arc->width = width; + grapic_arc->x_start = x_center; + grapic_arc->y_start = y_center; + grapic_arc->x_end = x_semiaxis; + grapic_arc->y_end = y_semiaxis; + return 0; +} + +/** + * @brief UI_绘制浮点数 + * + * @param grapic_float 结构体 + * @param name 图形名首地址 + * @param type_op 操作类型 + * @param layer 图层数 + * @param color 颜色 + * @param font_size 字体大小 + * @param digits 小数点后有效位数 + * @param width 线条宽度 + * @param x_start 起点x坐标 + * @param y_start 起点y坐标 + * @param float_high 32位浮点数 + * @param float_middle 32位浮点数 + * @param float_low 32位浮点数 + * @return int8_t + */ +int8_t UI_DrawFloating(UI_Ele_t *grapic_floating, const char *name, + uint8_t type_op, uint8_t layer, uint8_t color, + uint16_t font_size, uint16_t digits, uint16_t width, + uint16_t x_start, uint16_t y_start, uint16_t float_high, + uint16_t float_middle, uint16_t float_low) { + if (grapic_floating == NULL) return -1; + snprintf((char *)grapic_floating->name, 2, "%s", name); + grapic_floating->type_op = type_op; + grapic_floating->type_ele = 5; + grapic_floating->layer = layer; + grapic_floating->color = color; + grapic_floating->angle_start = font_size; + grapic_floating->angle_end = digits; + grapic_floating->width = width; + grapic_floating->x_start = x_start; + grapic_floating->y_start = y_start; + grapic_floating->radius = float_high; + grapic_floating->x_end = float_middle; + grapic_floating->y_end = float_low; + return 0; +} + +/** + * @brief UI_绘制整型数 + * + * @param grapic_integer 结构体 + * @param name 图形名首地址 + * @param type_op 操作类型 + * @param layer 图层数 + * @param color 颜色 + * @param font_size 字体大小 + * @param width 线条宽度 + * @param x_start 起点x坐标 + * @param y_start 起点y坐标 + * @param int32_t_high 32位整型数 + * @param int32_t_middle 32位整型数 + * @param int32_t_low 32位整型数 + * @return int8_t + */ +int8_t UI_DrawInteger(UI_Ele_t *grapic_integer, const char *name, + uint8_t type_op, uint8_t layer, uint8_t color, + uint16_t font_size, uint16_t width, uint16_t x_start, + uint16_t y_start, uint16_t int32_t_high, + uint16_t int32_t_middle, uint16_t int32_t_low) { + if (grapic_integer == NULL) return -1; + snprintf((char *)grapic_integer->name, 2, "%s", name); + grapic_integer->type_op = type_op; + grapic_integer->type_ele = 6; + grapic_integer->layer = layer; + grapic_integer->color = color; + grapic_integer->angle_start = font_size; + grapic_integer->width = width; + grapic_integer->x_start = x_start; + grapic_integer->y_start = y_start; + grapic_integer->radius = int32_t_high; + grapic_integer->x_end = int32_t_middle; + grapic_integer->y_end = int32_t_low; + return 0; +} + +/** + * @brief UI_绘制字符 + * + * @param grapic_character 结构体 + * @param name 图形名首地址 + * @param type_op 操作类型 + * @param layer 图层数 + * @param color 颜色 + * @param font_size 字体大小 + * @param length 字符长度 + * @param width 线条宽度 + * @param x_start 起点x坐标 + * @param y_start 起点y坐标 + * @param character 字符串首地址 + * @return int8_t + */ +int8_t UI_DrawCharacter(UI_Drawcharacter_t *grapic_character, const char *name, + uint8_t type_op, uint8_t layer, uint8_t color, + uint16_t font_size, uint16_t length, uint16_t width, + uint16_t x_start, uint16_t y_start, + const char *character) { + if (grapic_character == NULL) return -1; + snprintf((char *)grapic_character->grapic.name, 2, "%s", name); + grapic_character->grapic.type_op = type_op; + grapic_character->grapic.type_ele = 7; + grapic_character->grapic.layer = layer; + grapic_character->grapic.color = color; + grapic_character->grapic.angle_start = font_size; + grapic_character->grapic.angle_end = length; + grapic_character->grapic.width = width; + grapic_character->grapic.x_start = x_start; + grapic_character->grapic.y_start = y_start; + snprintf((char *)grapic_character->character, 29, "%s", character); + return 0; +} + +/** + * @brief UI_删除图层 + * + * @param del 结构体 + * @param opt 操作 + * @param layer 图层 + * @return int8_t + */ +int8_t UI_DelLayer(UI_Del_t *del, uint8_t opt, uint8_t layer) { + if (del == NULL) return -1; + del->del_operation = opt; + del->layer = layer; + return 0; +} \ No newline at end of file diff --git a/engineer/User/component/ui.h b/engineer/User/component/ui.h new file mode 100644 index 0000000..4f742d3 --- /dev/null +++ b/engineer/User/component/ui.h @@ -0,0 +1,284 @@ +/* + UI相关命令 +*/ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +#include "component/user_math.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +#define UI_DEL_OPERATION_NOTHING (0) +#define UI_DEL_OPERATION_DEL (1) +#define UI_DEL_OPERATION_DEL_ALL (2) + +#define UI_GRAPIC_OPERATION_NOTHING (0) +#define UI_GRAPIC_OPERATION_ADD (1) +#define UI_GRAPIC_OPERATION_REWRITE (2) +#define UI_GRAPIC_OPERATION_DEL (3) + +#define UI_GRAPIC_LAYER_CONST (0) +#define UI_GRAPIC_LAYER_AUTOAIM (1) +#define UI_GRAPIC_LAYER_CHASSIS (2) +#define UI_GRAPIC_LAYER_CAP (3) +#define UI_GRAPIC_LAYER_GIMBAL (4) +#define UI_GRAPIC_LAYER_SHOOT (5) +#define UI_GRAPIC_LAYER_CMD (6) + +#define UI_DEFAULT_WIDTH (0x01) + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ +#define UI_CHAR_DEFAULT_WIDTH (0x02) + +typedef enum { + RED_BLUE, + YELLOW, + GREEN, + ORANGE, + PURPLISH_RED, + PINK, + CYAN, + BLACK, + WHITE +} UI_Color_t; + +typedef struct __packed { + uint8_t op; + uint8_t num_layer; +} UI_InterStudent_UIDel_t; + +typedef struct __packed { + uint8_t name[3]; + uint8_t type_op : 3; + uint8_t type_ele : 3; + uint8_t layer : 4; + uint8_t color : 4; + uint16_t angle_start : 9; + uint16_t angle_end : 9; + uint16_t width : 10; + uint16_t x_start : 11; + uint16_t y_start : 11; + uint16_t radius : 10; + uint16_t x_end : 11; + uint16_t y_end : 11; +} UI_Ele_t; + +typedef struct __packed { + UI_Ele_t grapic; +} UI_Drawgrapic_1_t; + +typedef struct __packed { + UI_Ele_t grapic[2]; +} UI_Drawgrapic_2_t; + +typedef struct __packed { + UI_Ele_t grapic[5]; +} UI_Drawgrapic_5_t; + +typedef struct __packed { + UI_Ele_t grapic[7]; +} UI_Drawgrapic_7_t; + +typedef struct __packed { + UI_Ele_t grapic; + uint8_t character[30]; +} UI_Drawcharacter_t; + +typedef struct __packed { + uint8_t del_operation; + uint8_t layer; +} UI_Del_t; + +/** + * @brief UI_绘制直线段 + * + * @param grapic_line 结构体 + * @param name 图形名首地址 + * @param type_op 操作类型 + * @param layer 图层数 + * @param color 颜色 + * @param width 线条宽度 + * @param x_start 起点x坐标 + * @param y_start 起点y坐标 + * @param x_end 终点x坐标 + * @param y_end 终点y坐标 + * @return int8_t + */ +int8_t UI_DrawLine(UI_Ele_t *grapic_line, const char *name, uint8_t type_op, + uint8_t layer, uint8_t color, uint16_t width, + uint16_t x_start, uint16_t y_start, uint16_t x_end, + uint16_t y_end); + +/** + * @brief UI_绘制矩形 + * + * @param grapic_rectangle 结构体 + * @param name 图形名首地址 + * @param type_op 操作类型 + * @param layer 图层数 + * @param color 颜色 + * @param width 线条宽度 + * @param x_start 起点x坐标 + * @param y_start 起点y坐标 + * @param x_end 对角顶点x坐标 + * @param y_end 对角顶点y坐标 + * @return int8_t + */ +int8_t UI_DrawRectangle(UI_Ele_t *grapic_rectangle, const char *name, + uint8_t type_op, uint8_t layer, uint8_t color, + uint16_t width, uint16_t x_start, uint16_t y_start, + uint16_t x_end, uint16_t y_end); + +/** + * @brief UI_绘制正圆 + * + * @param grapic_cycle 结构体 + * @param name 图形名首地址 + * @param type_op 操作类型 + * @param layer 图层数 + * @param color 颜色 + * @param width 线条宽度 + * @param x_center 圆心x坐标 + * @param y_center 圆心y坐标 + * @param radius 半径 + * @return int8_t + */ +int8_t UI_DrawCycle(UI_Ele_t *grapic_cycle, const char *name, uint8_t type_op, + uint8_t layer, uint8_t color, uint16_t width, + uint16_t x_center, uint16_t y_center, uint16_t radius); + +/** + * @brief UI_绘制椭圆 + * + * @param grapic_oval 结构体 + * @param name 图形名首地址 + * @param type_op 操作类型 + * @param layer 图层数 + * @param color 颜色 + * @param width 线条宽度 + * @param x_center 圆心x坐标 + * @param y_center 圆心y坐标 + * @param x_semiaxis x半轴长度 + * @param y_semiaxis y半轴长度 + * @return int8_t + */ +int8_t UI_DrawOval(UI_Ele_t *grapic_oval, const char *name, uint8_t type_op, + uint8_t layer, uint8_t color, uint16_t width, + uint16_t x_center, uint16_t y_center, uint16_t x_semiaxis, + uint16_t y_semiaxis); + +/** + * @brief UI_绘制圆弧 + * + * @param grapic_arc 结构体 + * @param name 图形名首地址 + * @param type_op 操作类型 + * @param layer 图层数 + * @param color 颜色 + * @param angle_start 起始角度 + * @param angle_end 终止角度 + * @param width 线条宽度 + * @param x_center 圆心x坐标 + * @param y_center 圆心y坐标 + * @param x_semiaxis x半轴长度 + * @param y_semiaxis y半轴长度 + * @return int8_t + */ +int8_t UI_DrawArc(UI_Ele_t *grapic_arc, const char *name, uint8_t type_op, + uint8_t layer, uint8_t color, uint16_t angle_start, + uint16_t angle_end, uint16_t width, uint16_t x_center, + uint16_t y_center, uint16_t x_semiaxis, uint16_t y_semiaxis); + +/** + * @brief UI_绘制浮点数 + * + * @param grapic_float 结构体 + * @param name 图形名首地址 + * @param type_op 操作类型 + * @param layer 图层数 + * @param color 颜色 + * @param font_size 字体大小 + * @param digits 小数点后有效位数 + * @param width 线条宽度 + * @param x_start 起点x坐标 + * @param y_start 起点y坐标 + * @param float_high 32位浮点数 + * @param float_middle 32位浮点数 + * @param float_low 32位浮点数 + * @return int8_t + */ +int8_t UI_DrawFloating(UI_Ele_t *grapic_floating, const char *name, + uint8_t type_op, uint8_t layer, uint8_t color, + uint16_t font_size, uint16_t digits, uint16_t width, + uint16_t x_start, uint16_t y_start, uint16_t float_high, + uint16_t float_middle, uint16_t float_low); + +/** + * @brief UI_绘制整型数 + * + * @param grapic_integer 结构体 + * @param name 图形名首地址 + * @param type_op 操作类型 + * @param layer 图层数 + * @param color 颜色 + * @param font_size 字体大小 + * @param width 线条宽度 + * @param x_start 起点x坐标 + * @param y_start 起点y坐标 + * @param int32_t_high 32位整型数 + * @param int32_t_middle 32位整型数 + * @param int32_t_low 32位整型数 + * @return int8_t + */ +int8_t UI_DrawInteger(UI_Ele_t *grapic_integer, const char *name, + uint8_t type_op, uint8_t layer, uint8_t color, + uint16_t font_size, uint16_t width, uint16_t x_start, + uint16_t y_start, uint16_t int32_t_high, + uint16_t int32_t_middle, uint16_t int32_t_low); + +/** + * @brief UI_绘制字符 + * + * @param grapic_character 结构体 + * @param name 图形名首地址 + * @param type_op 操作类型 + * @param layer 图层数 + * @param color 颜色 + * @param font_size 字体大小 + * @param length 字符长度 + * @param width 线条宽度 + * @param x_start 起点x坐标 + * @param y_start 起点y坐标 + * @param character 字符串首地址 + * @return int8_t + */ +int8_t UI_DrawCharacter(UI_Drawcharacter_t *grapic_character, const char *name, + uint8_t type_op, uint8_t layer, uint8_t color, + uint16_t font_size, uint16_t length, uint16_t width, + uint16_t x_start, uint16_t y_start, + const char *character); + +/** + * @brief UI_删除图层 + * + * @param del 结构体 + * @param opt 操作 + * @param layer 图层 + * @return int8_t + */ +int8_t UI_DelLayer(UI_Del_t *del, uint8_t opt, uint8_t layer); + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/engineer/User/component/utils.cpp b/engineer/User/component/utils.cpp new file mode 100644 index 0000000..187f67d --- /dev/null +++ b/engineer/User/component/utils.cpp @@ -0,0 +1,56 @@ +/** + ****************************************************************************** + * @file utils.cpp/h + * @brief General math utils. 常用数学工具函数 + * @author Spoon Guan + ****************************************************************************** + * Copyright (c) 2023 Team JiaoLong-SJTU + * All rights reserved. + ****************************************************************************** + */ + +#include "utils.hpp" + +float math::limit(float val, const float& min, const float& max) { + if (min > max) + return val; + else if (val < min) + val = min; + else if (val > max) + val = max; + return val; +} + +float math::limitMin(float val, const float& min) { + if (val < min) + val = min; + return val; +} + +float math::limitMax(float val, const float& max) { + if (val > max) + val = max; + return val; +} + +float math::loopLimit(float val, const float& min, const float& max) { + if (min >= max) + return val; + if (val > max) { + while (val > max) + val -= (max - min); + } else if (val < min) { + while (val < min) + val += (max - min); + } + return val; +} + +float math::sign(const float& val) { + if (val > 0) + return 1; + else if (val < 0) + return -1; + return 0; +} + diff --git a/engineer/User/component/utils.hpp b/engineer/User/component/utils.hpp new file mode 100644 index 0000000..1515eb6 --- /dev/null +++ b/engineer/User/component/utils.hpp @@ -0,0 +1,27 @@ +/** + ****************************************************************************** + * @file utils.cpp/h + * @brief General math utils. 常用数学工具函数 + * @author Spoon Guan + ****************************************************************************** + * Copyright (c) 2023 Team JiaoLong-SJTU + * All rights reserved. + ****************************************************************************** + */ + +#ifndef UTILS_H +#define UTILS_H + +#include + +namespace math { + +float limit(float val, const float& min, const float& max); +float limitMin(float val, const float& min); +float limitMax(float val, const float& max); +float loopLimit(float val, const float& min, const float& max); +float sign(const float& val); + +} // namespace math + +#endif // UTILS_H diff --git a/engineer/User/device/referee.c b/engineer/User/device/referee.c new file mode 100644 index 0000000..18cfb52 --- /dev/null +++ b/engineer/User/device/referee.c @@ -0,0 +1,788 @@ +/* + 裁判系统抽象。 +*/ + +/* Includes ----------------------------------------------------------------- */ +#include "referee.h" + +#include + +#include "bsp/uart.h" +#include "component/crc16.h" +#include "component/crc8.h" +#include "component/user_math.h" + +/* Private define ----------------------------------------------------------- */ +#define REF_HEADER_SOF (0xA5) +#define REF_LEN_RX_BUFF (0xFF) + +#define REF_UI_FAST_REFRESH_FREQ (50) +#define REF_UI_SLOW_REFRESH_FREQ (0.2f) + +#define REF_UI_BOX_UP_OFFSET (4) +#define REF_UI_BOX_BOT_OFFSET (-14) + +#define REF_UI_RIGHT_START_POS (0.85f) + +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +/* Private variables -------------------------------------------------------- */ +static volatile uint32_t drop_message = 0; + +static uint8_t rxbuf[REF_LEN_RX_BUFF]; + +static osThreadId_t thread_alert; +static bool inited = false; + +/* Private function -------------------------------------------------------- */ +static void Referee_RxCpltCallback(void) { + osThreadFlagsSet(thread_alert, SIGNAL_REFEREE_RAW_REDY); +} + +static void Referee_IdleLineCallback(void) { + HAL_UART_AbortReceive_IT(BSP_UART_GetHandle(BSP_UART_REF)); +} + +static void Referee_AbortRxCpltCallback(void) { + osThreadFlagsSet(thread_alert, SIGNAL_REFEREE_RAW_REDY); +} + +static void RefereeFastRefreshTimerCallback(void *arg) { + (void)arg; + osThreadFlagsSet(thread_alert, SIGNAL_REFEREE_FAST_REFRESH_UI); +} + +static void RefereeSlowRefreshTimerCallback(void *arg) { + (void)arg; + osThreadFlagsSet(thread_alert, SIGNAL_REFEREE_SLOW_REFRESH_UI); +} + +/* Exported functions ------------------------------------------------------- */ +int8_t Referee_Init(Referee_t *ref, Referee_UI_t *ui, + const CMD_Screen_t *screen) { + if (ref == NULL) return DEVICE_ERR_NULL; + if (inited) return DEVICE_ERR_INITED; + + if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL; + + ui->screen = screen; + + BSP_UART_RegisterCallback(BSP_UART_REF, BSP_UART_RX_CPLT_CB, + Referee_RxCpltCallback); + BSP_UART_RegisterCallback(BSP_UART_REF, BSP_UART_ABORT_RX_CPLT_CB, + Referee_AbortRxCpltCallback); + BSP_UART_RegisterCallback(BSP_UART_REF, BSP_UART_IDLE_LINE_CB, + Referee_IdleLineCallback); + + uint32_t fast_period_ms = (uint32_t)(1000.0f / REF_UI_FAST_REFRESH_FREQ); + uint32_t slow_period_ms = (uint32_t)(1000.0f / REF_UI_SLOW_REFRESH_FREQ); + + ref->ui_fast_timer_id = + osTimerNew(RefereeFastRefreshTimerCallback, osTimerPeriodic, NULL, NULL); + + ref->ui_slow_timer_id = + osTimerNew(RefereeSlowRefreshTimerCallback, osTimerPeriodic, NULL, NULL); + + osTimerStart(ref->ui_fast_timer_id, fast_period_ms); + osTimerStart(ref->ui_slow_timer_id, slow_period_ms); + + __HAL_UART_ENABLE_IT(BSP_UART_GetHandle(BSP_UART_REF), UART_IT_IDLE); + + inited = true; + return 0; +} + +int8_t Referee_Restart(void) { + __HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_REF)); + __HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_REF)); + return 0; +} + +int8_t Referee_StartReceiving(Referee_t *ref) { + (void)ref; + if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_REF), rxbuf, + REF_LEN_RX_BUFF) == HAL_OK) + return DEVICE_OK; + return DEVICE_ERR; +} + +bool Referee_CheckTXReady() { + return BSP_UART_GetHandle(BSP_UART_REF)->gState == HAL_UART_STATE_READY; +} + +void Referee_HandleOffline(Referee_t *referee) { + referee->ref_status = REF_STATUS_OFFLINE; +} + +int8_t Referee_Parse(Referee_t *ref) { + REF_SWITCH_STATUS(*ref, REF_STATUS_RUNNING); + uint32_t data_length = + REF_LEN_RX_BUFF - + __HAL_DMA_GET_COUNTER(BSP_UART_GetHandle(BSP_UART_REF)->hdmarx); + + uint8_t index = 0; + uint8_t packet_shift; + uint8_t packet_length; + + while (index < data_length && rxbuf[index] == REF_HEADER_SOF) { + packet_shift = index; + Referee_Header_t *header = (Referee_Header_t *)(rxbuf + index); + index += sizeof(*header); + if (index - packet_shift >= data_length) goto error; + + if (!CRC8_Verify((uint8_t *)header, sizeof(*header))) goto error; + + if (header->sof != REF_HEADER_SOF) goto error; + + Referee_CMDID_t *cmd_id = (Referee_CMDID_t *)(rxbuf + index); + index += sizeof(*cmd_id); + if (index - packet_shift >= data_length) goto error; + + void *target = (rxbuf + index); + void *origin; + size_t size; + + switch (*cmd_id) { + case REF_CMD_ID_GAME_STATUS: + origin = &(ref->game_status); + size = sizeof(ref->game_status); + break; + case REF_CMD_ID_GAME_RESULT: + origin = &(ref->game_result); + size = sizeof(ref->game_result); + break; + case REF_CMD_ID_GAME_ROBOT_HP: + origin = &(ref->game_robot_hp); + size = sizeof(ref->game_robot_hp); + break; + case REF_CMD_ID_DART_STATUS: + origin = &(ref->dart_status); + size = sizeof(ref->dart_status); + break; + case REF_CMD_ID_ICRA_ZONE_STATUS: + origin = &(ref->icra_zone); + size = sizeof(ref->icra_zone); + break; + case REF_CMD_ID_FIELD_EVENTS: + origin = &(ref->field_event); + size = sizeof(ref->field_event); + break; + case REF_CMD_ID_SUPPLY_ACTION: + origin = &(ref->supply_action); + size = sizeof(ref->supply_action); + break; + case REF_CMD_ID_REQUEST_SUPPLY: + origin = &(ref->request_supply); + size = sizeof(ref->request_supply); + case REF_CMD_ID_WARNING: + origin = &(ref->warning); + size = sizeof(ref->warning); + break; + case REF_CMD_ID_DART_COUNTDOWN: + origin = &(ref->dart_countdown); + size = sizeof(ref->dart_countdown); + break; + case REF_CMD_ID_ROBOT_STATUS: + origin = &(ref->robot_status); + size = sizeof(ref->robot_status); + break; + case REF_CMD_ID_POWER_HEAT_DATA: + origin = &(ref->power_heat); + size = sizeof(ref->power_heat); + break; + case REF_CMD_ID_ROBOT_POS: + origin = &(ref->robot_pos); + size = sizeof(ref->robot_pos); + break; + case REF_CMD_ID_ROBOT_BUFF: + origin = &(ref->robot_buff); + size = sizeof(ref->robot_buff); + break; + case REF_CMD_ID_DRONE_ENERGY: + origin = &(ref->drone_energy); + size = sizeof(ref->drone_energy); + break; + case REF_CMD_ID_ROBOT_DMG: + origin = &(ref->robot_danage); + size = sizeof(ref->robot_danage); + break; + case REF_CMD_ID_SHOOT_DATA: + origin = &(ref->shoot_data); + size = sizeof(ref->shoot_data); + break; + case REF_CMD_ID_BULLET_REMAINING: + origin = &(ref->bullet_remain); + size = sizeof(ref->bullet_remain); + break; + case REF_CMD_ID_RFID: + origin = &(ref->rfid); + size = sizeof(ref->rfid); + break; + case REF_CMD_ID_DART_CLIENT: + origin = &(ref->dart_client); + size = sizeof(ref->dart_client); + break; + case REF_CMD_ID_INTER_STUDENT_CUSTOM: + origin = &(ref->custom); + size = sizeof(ref->custom); + case REF_CMD_ID_CLIENT_MAP: + origin = &(ref->client_map); + size = sizeof(ref->client_map); + case REF_CMD_ID_KEYBOARD_MOUSE: + origin = &(ref->keyboard_mouse); + size = sizeof(ref->keyboard_mouse); + default: + return DEVICE_ERR; + } + packet_length = sizeof(Referee_Header_t) + sizeof(Referee_CMDID_t) + size + + sizeof(Referee_Tail_t); + index += size; + if (index - packet_shift >= data_length) goto error; + + index += sizeof(Referee_Tail_t); + if (index - packet_shift != packet_length) goto error; + + if (CRC16_Verify((uint8_t *)(rxbuf + packet_shift), packet_length)) + memcpy(origin, target, size); + else + goto error; + } + return DEVICE_OK; + +error: + drop_message++; + return DEVICE_ERR; +} + +int8_t Referee_StartSend(uint8_t *data, uint32_t len) { + if (HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_REF), data, + (size_t)len) == HAL_OK) { + return DEVICE_OK; + } else + return DEVICE_ERR; +} + +int8_t Referee_MoveData(void *data, void *tmp, uint32_t len) { + if (len <= 0 || data == NULL || tmp == NULL) return DEVICE_ERR; + memcpy(tmp, (const void *)data, (size_t)len); + memset(data, 0, (size_t)len); + return DEVICE_OK; +} + +int8_t Referee_SetHeader(Referee_Interactive_Header_t *header, + Referee_StudentCMDID_t cmd_id, uint8_t sender_id) { + header->data_cmd_id = cmd_id; + if (sender_id <= REF_BOT_RED_RADER) switch (sender_id) { + case REF_BOT_RED_HERO: + header->sender_ID = REF_BOT_RED_HERO; + header->receiver_ID = REF_CL_RED_HERO; + break; + case REF_BOT_RED_ENGINEER: + header->sender_ID = REF_BOT_RED_ENGINEER; + header->receiver_ID = REF_CL_RED_ENGINEER; + break; + case REF_BOT_RED_INFANTRY_1: + header->sender_ID = REF_BOT_RED_INFANTRY_1; + header->receiver_ID = REF_CL_RED_INFANTRY_1; + break; + case REF_BOT_RED_INFANTRY_2: + header->sender_ID = REF_BOT_RED_INFANTRY_2; + header->receiver_ID = REF_CL_RED_INFANTRY_2; + break; + case REF_BOT_RED_INFANTRY_3: + header->sender_ID = REF_BOT_RED_INFANTRY_3; + header->receiver_ID = REF_CL_RED_INFANTRY_3; + break; + case REF_BOT_RED_DRONE: + header->sender_ID = REF_BOT_RED_DRONE; + header->receiver_ID = REF_CL_RED_DRONE; + break; + case REF_BOT_RED_SENTRY: + header->sender_ID = REF_BOT_RED_SENTRY; + break; + case REF_BOT_RED_RADER: + header->sender_ID = REF_BOT_RED_RADER; + break; + default: + return -1; + } + else + switch (sender_id) { + case REF_BOT_BLU_HERO: + header->sender_ID = REF_BOT_BLU_HERO; + header->receiver_ID = REF_CL_BLU_HERO; + break; + case REF_BOT_BLU_ENGINEER: + header->sender_ID = REF_BOT_BLU_ENGINEER; + header->receiver_ID = REF_CL_BLU_ENGINEER; + break; + case REF_BOT_BLU_INFANTRY_1: + header->sender_ID = REF_BOT_BLU_INFANTRY_1; + header->receiver_ID = REF_CL_BLU_INFANTRY_1; + break; + case REF_BOT_BLU_INFANTRY_2: + header->sender_ID = REF_BOT_BLU_INFANTRY_2; + header->receiver_ID = REF_CL_BLU_INFANTRY_2; + break; + case REF_BOT_BLU_INFANTRY_3: + header->sender_ID = REF_BOT_BLU_INFANTRY_3; + header->receiver_ID = REF_CL_BLU_INFANTRY_3; + break; + case REF_BOT_BLU_DRONE: + header->sender_ID = REF_BOT_BLU_DRONE; + header->receiver_ID = REF_CL_BLU_DRONE; + break; + case REF_BOT_BLU_SENTRY: + header->sender_ID = REF_BOT_BLU_SENTRY; + break; + case REF_BOT_BLU_RADER: + header->sender_ID = REF_BOT_BLU_RADER; + break; + default: + return -1; + } + return 0; +} + +int8_t Referee_PackUI(Referee_UI_t *ui, Referee_t *ref) { + if (!Referee_CheckTXReady()) return 0; + if (ui->character_counter == 0 && ui->grapic_counter == 0 && + ui->del_counter == 0) + return 0; + + static uint8_t send_data[sizeof(Referee_UI_Drawgrapic_7_t)] = {0}; + uint16_t size; + if (ui->del_counter != 0) { + if (ui->del_counter < 0 || ui->del_counter > REF_UI_MAX_STRING_NUM) + return DEVICE_ERR; + Referee_UI_Del_t *address = (Referee_UI_Del_t *)send_data; + address->header.sof = REF_HEADER_SOF; + address->header.data_length = + sizeof(UI_Del_t) + sizeof(Referee_Interactive_Header_t); + address->header.crc8 = + CRC8_Calc((const uint8_t *)&(address->header), + sizeof(Referee_Header_t) - sizeof(uint8_t), CRC8_INIT); + address->cmd_id = REF_CMD_ID_INTER_STUDENT; + Referee_SetHeader(&(address->IA_header), REF_STDNT_CMD_ID_UI_DEL, + ref->robot_status.robot_id); + Referee_MoveData(&(ui->del[--ui->del_counter]), &(address->data), + sizeof(UI_Del_t)); + address->crc16 = + CRC16_Calc((const uint8_t *)address, + sizeof(Referee_UI_Del_t) - sizeof(uint16_t), CRC16_INIT); + size = sizeof(Referee_UI_Del_t); + + Referee_StartSend(send_data, size); + return DEVICE_OK; + } else if (ui->grapic_counter != 0) { + switch (ui->grapic_counter) { + case 1: + size = sizeof(Referee_UI_Drawgrapic_1_t); + Referee_UI_Drawgrapic_1_t *address_1 = + (Referee_UI_Drawgrapic_1_t *)send_data; + address_1->header.sof = REF_HEADER_SOF; + address_1->header.data_length = + sizeof(UI_Drawgrapic_1_t) + sizeof(Referee_Interactive_Header_t); + address_1->header.crc8 = + CRC8_Calc((const uint8_t *)&(address_1->header), + sizeof(Referee_Header_t) - sizeof(uint8_t), CRC8_INIT); + address_1->cmd_id = REF_CMD_ID_INTER_STUDENT; + Referee_SetHeader(&(address_1->IA_header), REF_STDNT_CMD_ID_UI_DRAW1, + ref->robot_status.robot_id); + Referee_MoveData(&(ui->grapic), &(address_1->data.grapic), + sizeof(UI_Drawgrapic_1_t)); + address_1->crc16 = CRC16_Calc( + (const uint8_t *)address_1, + sizeof(Referee_UI_Drawgrapic_1_t) - sizeof(uint16_t), CRC16_INIT); + break; + case 2: + size = sizeof(Referee_UI_Drawgrapic_2_t); + Referee_UI_Drawgrapic_2_t *address_2 = + (Referee_UI_Drawgrapic_2_t *)send_data; + address_2->header.sof = REF_HEADER_SOF; + address_2->header.data_length = + sizeof(UI_Drawgrapic_2_t) + sizeof(Referee_Interactive_Header_t); + address_2->header.crc8 = + CRC8_Calc((const uint8_t *)&(address_2->header), + sizeof(Referee_Header_t) - sizeof(uint8_t), CRC8_INIT); + address_2->cmd_id = REF_CMD_ID_INTER_STUDENT; + Referee_SetHeader(&(address_2->IA_header), REF_STDNT_CMD_ID_UI_DRAW2, + ref->robot_status.robot_id); + Referee_MoveData(&(ui->grapic), &(address_2->data.grapic), + sizeof(UI_Drawgrapic_2_t)); + address_2->crc16 = CRC16_Calc( + (const uint8_t *)address_2, + sizeof(Referee_UI_Drawgrapic_2_t) - sizeof(uint16_t), CRC16_INIT); + break; + case 3: + case 4: + case 5: + size = sizeof(Referee_UI_Drawgrapic_5_t); + Referee_UI_Drawgrapic_5_t *address_5 = + (Referee_UI_Drawgrapic_5_t *)send_data; + address_5->header.sof = REF_HEADER_SOF; + address_5->header.data_length = + sizeof(UI_Drawgrapic_5_t) + sizeof(Referee_Interactive_Header_t); + address_5->header.crc8 = + CRC8_Calc((const uint8_t *)&(address_5->header), + sizeof(Referee_Header_t) - sizeof(uint8_t), CRC8_INIT); + address_5->cmd_id = REF_CMD_ID_INTER_STUDENT; + Referee_SetHeader(&(address_5->IA_header), REF_STDNT_CMD_ID_UI_DRAW5, + ref->robot_status.robot_id); + Referee_MoveData(&(ui->grapic), &(address_5->data.grapic), + sizeof(UI_Drawgrapic_5_t)); + address_5->crc16 = CRC16_Calc( + (const uint8_t *)address_5, + sizeof(Referee_UI_Drawgrapic_5_t) - sizeof(uint16_t), CRC16_INIT); + break; + case 6: + case 7: + size = sizeof(Referee_UI_Drawgrapic_7_t); + Referee_UI_Drawgrapic_7_t *address_7 = + (Referee_UI_Drawgrapic_7_t *)send_data; + address_7->header.sof = REF_HEADER_SOF; + address_7->header.data_length = + sizeof(UI_Drawgrapic_7_t) + sizeof(Referee_Interactive_Header_t); + address_7->header.crc8 = + CRC8_Calc((const uint8_t *)&(address_7->header), + sizeof(Referee_Header_t) - sizeof(uint8_t), CRC8_INIT); + address_7->cmd_id = REF_CMD_ID_INTER_STUDENT; + Referee_SetHeader(&(address_7->IA_header), REF_STDNT_CMD_ID_UI_DRAW7, + ref->robot_status.robot_id); + Referee_MoveData(&(ui->grapic), &(address_7->data.grapic), + sizeof(UI_Drawgrapic_7_t)); + address_7->crc16 = CRC16_Calc( + (const uint8_t *)address_7, + sizeof(Referee_UI_Drawgrapic_7_t) - sizeof(uint16_t), CRC16_INIT); + break; + default: + return DEVICE_ERR; + } + if (Referee_StartSend(send_data, size) == HAL_OK) { + ui->grapic_counter = 0; + return DEVICE_OK; + } + } else if (ui->character_counter != 0) { + if (ui->character_counter < 0 || + ui->character_counter > REF_UI_MAX_STRING_NUM) + return DEVICE_ERR; + Referee_UI_Drawcharacter_t *address = + (Referee_UI_Drawcharacter_t *)send_data; + address->header.sof = REF_HEADER_SOF; + address->header.data_length = + sizeof(UI_Drawcharacter_t) + sizeof(Referee_Interactive_Header_t); + address->header.crc8 = + CRC8_Calc((const uint8_t *)&(address->header), + sizeof(Referee_Header_t) - sizeof(uint8_t), CRC8_INIT); + address->cmd_id = REF_CMD_ID_INTER_STUDENT; + Referee_SetHeader(&(address->IA_header), REF_STDNT_CMD_ID_UI_STR, + ref->robot_status.robot_id); + Referee_MoveData(&(ui->character_data[--ui->character_counter]), + &(address->data.grapic), sizeof(UI_Drawcharacter_t)); + address->crc16 = CRC16_Calc( + (const uint8_t *)address, + sizeof(Referee_UI_Drawcharacter_t) - sizeof(uint16_t), CRC16_INIT); + size = sizeof(Referee_UI_Drawcharacter_t); + + Referee_StartSend(send_data, size); + return DEVICE_OK; + } + return DEVICE_ERR_NULL; +} + +UI_Ele_t *Referee_GetGrapicAdd(Referee_UI_t *ref_ui) { + if (ref_ui->grapic_counter >= REF_UI_MAX_GRAPIC_NUM || + ref_ui->grapic_counter < 0) + return NULL; + else + return &(ref_ui->grapic[ref_ui->grapic_counter++]); +} + +UI_Drawcharacter_t *Referee_GetCharacterAdd(Referee_UI_t *ref_ui) { + if (ref_ui->character_counter >= REF_UI_MAX_STRING_NUM || + ref_ui->character_counter < 0) + return NULL; + else + return &(ref_ui->character_data[ref_ui->character_counter++]); +} + +UI_Del_t *Referee_GetDelAdd(Referee_UI_t *ref_ui) { + if (ref_ui->del_counter >= REF_UI_MAX_DEL_NUM || ref_ui->del_counter < 0) + return NULL; + else + return &(ref_ui->del[ref_ui->del_counter++]); +} + +uint8_t Referee_PraseCmd(Referee_UI_t *ref_ui, CMD_UI_t cmd) { + switch (cmd) { + /* Demo */ + case CMD_UI_NOTHING: + /* 字符 */ + UI_DrawCharacter(Referee_GetCharacterAdd(ref_ui), "0", + UI_GRAPIC_OPERATION_ADD, UI_GRAPIC_LAYER_AUTOAIM, + RED_BLUE, UI_DEFAULT_WIDTH, 100, 100, 200, 200, "Demo"); + /* 直线 */ + UI_DrawLine(Referee_GetGrapicAdd(ref_ui), "2", UI_GRAPIC_OPERATION_ADD, + UI_GRAPIC_LAYER_AUTOAIM, RED_BLUE, UI_DEFAULT_WIDTH, 960, 540, + 960, 240); + /* 圆形 */ + UI_DrawCycle(Referee_GetGrapicAdd(ref_ui), "1", UI_GRAPIC_OPERATION_ADD, + UI_GRAPIC_LAYER_AUTOAIM, RED_BLUE, UI_DEFAULT_WIDTH, 900, + 500, 10); + /* 删除 */ + UI_DelLayer(Referee_GetDelAdd(ref_ui), UI_DEL_OPERATION_DEL, + UI_GRAPIC_LAYER_AUTOAIM); + break; + case CMD_UI_AUTO_AIM_START: + UI_DrawCharacter(Referee_GetCharacterAdd(ref_ui), "1", + UI_GRAPIC_OPERATION_ADD, UI_GRAPIC_LAYER_AUTOAIM, + RED_BLUE, UI_DEFAULT_WIDTH * 10, 50, UI_DEFAULT_WIDTH, + ref_ui->screen->width * 0.8, + ref_ui->screen->height * 0.5, "AUTO"); + break; + case CMD_UI_AUTO_AIM_STOP: + UI_DelLayer(Referee_GetDelAdd(ref_ui), UI_DEL_OPERATION_DEL, + UI_GRAPIC_LAYER_AUTOAIM); + + default: + return -1; + } + return 0; +} + +uint8_t Referee_PackCap(Referee_ForCap_t *cap, const Referee_t *ref) { + cap->chassis_power_limit = ref->robot_status.chassis_power_limit; + cap->chassis_pwr_buff = ref->power_heat.chassis_pwr_buff; + cap->chassis_watt = ref->power_heat.chassis_watt; + cap->ref_status = ref->ref_status; + return 0; +} + +uint8_t Referee_PackAI(Referee_ForAI_t *ai, const Referee_t *ref) { + ai->ref_status = ref->ref_status; + return 0; +} + +uint8_t Referee_PackChassis(Referee_ForChassis_t *chassis, + const Referee_t *ref) { + chassis->chassis_power_limit = ref->robot_status.chassis_power_limit; + chassis->chassis_pwr_buff = ref->power_heat.chassis_pwr_buff; + chassis->ref_status = ref->ref_status; + return 0; +} + +uint8_t Referee_PackShoot(Referee_ForShoot_t *shoot, Referee_t *ref) { + memcpy(&(shoot->power_heat), &(ref->power_heat), sizeof(shoot->power_heat)); + memcpy(&(shoot->robot_status), &(ref->robot_status), + sizeof(shoot->robot_status)); + memcpy(&(shoot->shoot_data), &(ref->shoot_data), sizeof(shoot->shoot_data)); + shoot->ref_status = ref->ref_status; + return 0; +} + +uint8_t Referee_UIRefresh(Referee_UI_t *ui) { + static uint8_t fsm = 0; + if (osThreadFlagsGet() & SIGNAL_REFEREE_FAST_REFRESH_UI) { + osThreadFlagsClear(SIGNAL_REFEREE_FAST_REFRESH_UI); + switch (fsm) { + case 0: { + fsm++; + UI_DelLayer(Referee_GetDelAdd(ui), UI_DEL_OPERATION_DEL, + UI_GRAPIC_LAYER_CHASSIS); + UI_DrawLine(Referee_GetGrapicAdd(ui), "6", UI_GRAPIC_OPERATION_ADD, + UI_GRAPIC_LAYER_CHASSIS, GREEN, UI_DEFAULT_WIDTH * 12, + ui->screen->width * 0.4, ui->screen->height * 0.2, + ui->screen->width * 0.4 + sin(ui->chassis_ui.angle) * 46, + ui->screen->height * 0.2 + cos(ui->chassis_ui.angle) * 46); + float start_pos_h = 0.0f; + switch (ui->chassis_ui.mode) { + case CHASSIS_MODE_FOLLOW_GIMBAL: + start_pos_h = 0.68f; + break; + case CHASSIS_MODE_FOLLOW_GIMBAL_35: + start_pos_h = 0.66f; + break; + case CHASSIS_MODE_ROTOR: + start_pos_h = 0.64f; + break; + default: + break; + } + if (start_pos_h != 0.0f) + UI_DrawRectangle( + Referee_GetGrapicAdd(ui), "8", UI_GRAPIC_OPERATION_ADD, + UI_GRAPIC_LAYER_CHASSIS, GREEN, UI_DEFAULT_WIDTH, + ui->screen->width * REF_UI_RIGHT_START_POS - 6, + ui->screen->height * start_pos_h + REF_UI_BOX_UP_OFFSET, + ui->screen->width * REF_UI_RIGHT_START_POS + 44, + ui->screen->height * start_pos_h + REF_UI_BOX_BOT_OFFSET); + break; + } + case 1: + fsm++; + UI_DelLayer(Referee_GetDelAdd(ui), UI_DEL_OPERATION_DEL, + UI_GRAPIC_LAYER_CAP); + // switch (ui->cap_ui.status) { + // case CAN_CAP_STATUS_OFFLINE: + // UI_DrawArc(Referee_GetGrapicAdd(ui), "9", UI_GRAPIC_OPERATION_ADD, + // UI_GRAPIC_LAYER_CAP, YELLOW, 0, 360, + // UI_DEFAULT_WIDTH * 5, ui->screen->width * 0.6, + // ui->screen->height * 0.2, 50, 50); + // break; + // break; + // case CAN_CAP_STATUS_RUNNING: + // UI_DrawArc(Referee_GetGrapicAdd(ui), "9", UI_GRAPIC_OPERATION_ADD, + // UI_GRAPIC_LAYER_CAP, GREEN, 0, + // ui->cap_ui.percentage * 360, UI_DEFAULT_WIDTH * 5, + // ui->screen->width * 0.6, ui->screen->height * 0.2, 50, + // 50); + // break; + // } + break; + case 2: { + fsm++; + UI_DelLayer(Referee_GetDelAdd(ui), UI_DEL_OPERATION_DEL, + UI_GRAPIC_LAYER_GIMBAL); + float start_pos_h = 0.0f; + switch (ui->gimbal_ui.mode) { + case GIMBAL_MODE_RELAX: + start_pos_h = 0.68f; + break; + case GIMBAL_MODE_RELATIVE: + start_pos_h = 0.66f; + break; + case GIMBAL_MODE_ABSOLUTE: + start_pos_h = 0.64f; + break; + default: + break; + } + UI_DrawRectangle( + Referee_GetGrapicAdd(ui), "a", UI_GRAPIC_OPERATION_ADD, + UI_GRAPIC_LAYER_GIMBAL, GREEN, UI_DEFAULT_WIDTH, + ui->screen->width * REF_UI_RIGHT_START_POS + 54, + ui->screen->height * start_pos_h + REF_UI_BOX_UP_OFFSET, + ui->screen->width * REF_UI_RIGHT_START_POS + 102, + ui->screen->height * start_pos_h + REF_UI_BOX_BOT_OFFSET); + break; + } + case 3: { + fsm++; + UI_DelLayer(Referee_GetDelAdd(ui), UI_DEL_OPERATION_DEL, + UI_GRAPIC_LAYER_SHOOT); + float start_pos_h = 0.0f; + switch (ui->shoot_ui.mode) { + case SHOOT_MODE_RELAX: + start_pos_h = 0.68f; + break; + case SHOOT_MODE_SAFE: + start_pos_h = 0.66f; + break; + case SHOOT_MODE_LOADED: + start_pos_h = 0.64f; + break; + default: + break; + } + UI_DrawRectangle( + Referee_GetGrapicAdd(ui), "b", UI_GRAPIC_OPERATION_ADD, + UI_GRAPIC_LAYER_SHOOT, GREEN, UI_DEFAULT_WIDTH, + ui->screen->width * REF_UI_RIGHT_START_POS + 114, + ui->screen->height * start_pos_h + REF_UI_BOX_UP_OFFSET, + ui->screen->width * REF_UI_RIGHT_START_POS + 162, + ui->screen->height * start_pos_h + REF_UI_BOX_BOT_OFFSET); + + switch (ui->shoot_ui.fire) { + case FIRE_MODE_SINGLE: + start_pos_h = 0.68f; + break; + case FIRE_MODE_BURST: + start_pos_h = 0.66f; + break; + case FIRE_MODE_CONT: + start_pos_h = 0.64f; + default: + break; + } + UI_DrawRectangle( + Referee_GetGrapicAdd(ui), "f", UI_GRAPIC_OPERATION_ADD, + UI_GRAPIC_LAYER_SHOOT, GREEN, UI_DEFAULT_WIDTH, + ui->screen->width * REF_UI_RIGHT_START_POS + 174, + ui->screen->height * start_pos_h + REF_UI_BOX_UP_OFFSET, + ui->screen->width * REF_UI_RIGHT_START_POS + 222, + ui->screen->height * start_pos_h + REF_UI_BOX_BOT_OFFSET); + break; + } + case 4: + fsm++; + UI_DelLayer(Referee_GetDelAdd(ui), UI_DEL_OPERATION_DEL, + UI_GRAPIC_LAYER_CMD); + if (ui->cmd_pc) { + UI_DrawRectangle(Referee_GetGrapicAdd(ui), "c", + UI_GRAPIC_OPERATION_ADD, UI_GRAPIC_LAYER_CMD, GREEN, + UI_DEFAULT_WIDTH, + ui->screen->width * REF_UI_RIGHT_START_POS + 96, + ui->screen->height * 0.4 + REF_UI_BOX_UP_OFFSET, + ui->screen->width * REF_UI_RIGHT_START_POS + 120, + ui->screen->height * 0.4 + REF_UI_BOX_BOT_OFFSET); + } else { + UI_DrawRectangle(Referee_GetGrapicAdd(ui), "c", + UI_GRAPIC_OPERATION_ADD, UI_GRAPIC_LAYER_CMD, GREEN, + UI_DEFAULT_WIDTH, + ui->screen->width * REF_UI_RIGHT_START_POS + 56, + ui->screen->height * 0.4 + REF_UI_BOX_UP_OFFSET, + ui->screen->width * REF_UI_RIGHT_START_POS + 80, + ui->screen->height * 0.4 + REF_UI_BOX_BOT_OFFSET); + } + break; + + default: + fsm = 0; + if (ui->del_counter >= REF_UI_MAX_DEL_NUM || + ui->character_counter > REF_UI_MAX_STRING_NUM || + ui->grapic_counter > REF_UI_MAX_GRAPIC_NUM) + BSP_UART_GetHandle(BSP_UART_REF)->gState = HAL_UART_STATE_READY; + } + } + + if (osThreadFlagsGet() & SIGNAL_REFEREE_SLOW_REFRESH_UI) { + osThreadFlagsClear(SIGNAL_REFEREE_SLOW_REFRESH_UI); + UI_DelLayer(Referee_GetDelAdd(ui), UI_DEL_OPERATION_DEL, + UI_GRAPIC_LAYER_CONST); + UI_DrawCharacter(Referee_GetCharacterAdd(ui), "1", UI_GRAPIC_OPERATION_ADD, + UI_GRAPIC_LAYER_CONST, GREEN, UI_DEFAULT_WIDTH * 10, 80, + UI_CHAR_DEFAULT_WIDTH, + ui->screen->width * REF_UI_RIGHT_START_POS, + ui->screen->height * 0.7, "CHAS GMBL SHOT FIRE"); + UI_DrawCharacter(Referee_GetCharacterAdd(ui), "2", UI_GRAPIC_OPERATION_ADD, + UI_GRAPIC_LAYER_CONST, GREEN, UI_DEFAULT_WIDTH * 10, 80, + UI_CHAR_DEFAULT_WIDTH, + ui->screen->width * REF_UI_RIGHT_START_POS, + ui->screen->height * 0.68, "FLLW RELX RELX SNGL"); + UI_DrawCharacter(Referee_GetCharacterAdd(ui), "3", UI_GRAPIC_OPERATION_ADD, + UI_GRAPIC_LAYER_CONST, GREEN, UI_DEFAULT_WIDTH * 10, 80, + UI_CHAR_DEFAULT_WIDTH, + ui->screen->width * REF_UI_RIGHT_START_POS, + ui->screen->height * 0.66, "FL35 ABSL SAFE BRST"); + UI_DrawCharacter(Referee_GetCharacterAdd(ui), "4", UI_GRAPIC_OPERATION_ADD, + UI_GRAPIC_LAYER_CONST, GREEN, UI_DEFAULT_WIDTH * 10, 80, + UI_CHAR_DEFAULT_WIDTH, + ui->screen->width * REF_UI_RIGHT_START_POS, + ui->screen->height * 0.64, "ROTR RLTV LOAD CONT"); + UI_DrawLine(Referee_GetGrapicAdd(ui), "5", UI_GRAPIC_OPERATION_ADD, + UI_GRAPIC_LAYER_CONST, GREEN, UI_DEFAULT_WIDTH * 3, + ui->screen->width * 0.4, ui->screen->height * 0.2, + ui->screen->width * 0.4, ui->screen->height * 0.2 + 50); + UI_DrawCharacter(Referee_GetCharacterAdd(ui), "d", UI_GRAPIC_OPERATION_ADD, + UI_GRAPIC_LAYER_CONST, GREEN, UI_DEFAULT_WIDTH * 10, 80, + UI_CHAR_DEFAULT_WIDTH, + ui->screen->width * REF_UI_RIGHT_START_POS, + ui->screen->height * 0.4, "CTRL RC PC"); + UI_DrawCharacter(Referee_GetCharacterAdd(ui), "e", UI_GRAPIC_OPERATION_ADD, + UI_GRAPIC_LAYER_CONST, GREEN, UI_DEFAULT_WIDTH * 20, 80, + UI_CHAR_DEFAULT_WIDTH * 2, ui->screen->width * 0.6 - 26, + ui->screen->height * 0.2 + 10, "CAP"); + } + + return 0; +} diff --git a/engineer/User/device/referee.h b/engineer/User/device/referee.h new file mode 100644 index 0000000..b2d077f --- /dev/null +++ b/engineer/User/device/referee.h @@ -0,0 +1,508 @@ +/* + 裁判系统抽象。 +*/ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include +#include + +#include "component/cmd.h" +#include "component/ui.h" +#include "component/user_math.h" +#include "bsp/can.h" +#include "device/device.h" + +/* Exported constants ------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +#define REF_SWITCH_STATUS(ref, stat) ((ref).ref_status = (stat)) +#define CHASSIS_POWER_MAX_WITHOUT_REF 50.0f /* 裁判系统离线底盘最大功率 */ + +#define REF_UI_MAX_GRAPIC_NUM (7) +#define REF_UI_MAX_STRING_NUM (7) +#define REF_UI_MAX_DEL_NUM (3) + +#define BSP_UART_REF BSP_UART_1 +/* Exported types ----------------------------------------------------------- */ +typedef struct __packed { + uint8_t sof; + uint16_t data_length; + uint8_t seq; + uint8_t crc8; +} Referee_Header_t; + +typedef enum { REF_STATUS_OFFLINE = 0, REF_STATUS_RUNNING } Referee_Status_t; + +typedef enum { + REF_CMD_ID_GAME_STATUS = 0x0001, + REF_CMD_ID_GAME_RESULT = 0x0002, + REF_CMD_ID_GAME_ROBOT_HP = 0x0003, + REF_CMD_ID_DART_STATUS = 0x0004, + REF_CMD_ID_ICRA_ZONE_STATUS = 0x0005, + REF_CMD_ID_FIELD_EVENTS = 0x0101, + REF_CMD_ID_SUPPLY_ACTION = 0x0102, + REF_CMD_ID_REQUEST_SUPPLY = 0x0103, + REF_CMD_ID_WARNING = 0x0104, + REF_CMD_ID_DART_COUNTDOWN = 0x0105, + REF_CMD_ID_ROBOT_STATUS = 0x0201, + REF_CMD_ID_POWER_HEAT_DATA = 0x0202, + REF_CMD_ID_ROBOT_POS = 0x0203, + REF_CMD_ID_ROBOT_BUFF = 0x0204, + REF_CMD_ID_DRONE_ENERGY = 0x0205, + REF_CMD_ID_ROBOT_DMG = 0x0206, + REF_CMD_ID_SHOOT_DATA = 0x0207, + REF_CMD_ID_BULLET_REMAINING = 0x0208, + REF_CMD_ID_RFID = 0x0209, + REF_CMD_ID_DART_CLIENT = 0x020A, + REF_CMD_ID_INTER_STUDENT = 0x0301, + REF_CMD_ID_INTER_STUDENT_CUSTOM = 0x0302, + REF_CMD_ID_CLIENT_MAP = 0x0303, + REF_CMD_ID_KEYBOARD_MOUSE = 0x0304, +} Referee_CMDID_t; + +typedef struct __packed { + uint8_t game_type : 4; + uint8_t game_progress : 4; + uint16_t stage_remain_time; + uint64_t sync_time_stamp; +} Referee_GameStatus_t; + +typedef struct __packed { + uint8_t winner; +} Referee_GameResult_t; + +typedef struct __packed { + uint16_t red_1; + uint16_t red_2; + uint16_t red_3; + uint16_t red_4; + uint16_t red_5; + uint16_t red_6; + uint16_t red_7; + uint16_t red_outpose; + uint16_t red_base; + uint16_t blue_1; + uint16_t blue_2; + uint16_t blue_3; + uint16_t blue_4; + uint16_t blue_5; + uint16_t blue_6; + uint16_t blue_7; + uint16_t blue_outpose; + uint16_t blue_base; +} Referee_GameRobotHP_t; + +typedef struct __packed { + uint8_t dart_belong; + uint16_t stage_remain_time; +} Referee_DartStatus_t; + +typedef struct __packed { + uint8_t f1_status : 1; + uint8_t f1_buff_status : 3; + uint8_t f2_status : 1; + uint8_t f2_buff_status : 3; + uint8_t f3_status : 1; + uint8_t f3_buff_status : 3; + uint8_t f4_status : 1; + uint8_t f4_buff_status : 3; + uint8_t f5_status : 1; + uint8_t f5_buff_status : 3; + uint8_t f6_status : 1; + uint8_t f6_buff_status : 3; + uint16_t red1_bullet_remain; + uint16_t red2_bullet_remain; + uint16_t blue1_bullet_remain; + uint16_t blue2_bullet_remain; +} Referee_ICRAZoneStatus_t; + +typedef struct __packed { + uint8_t copter_pad : 2; + uint8_t energy_mech : 2; + uint8_t virtual_shield : 1; + uint32_t res : 27; +} Referee_FieldEvents_t; + +typedef struct __packed { + uint8_t supply_id; + uint8_t robot_id; + uint8_t supply_step; + uint8_t supply_sum; +} Referee_SupplyAction_t; + +typedef struct __packed { + uint8_t place_holder; /* TODO */ +} Referee_RequestSupply_t; + +typedef struct __packed { + uint8_t level; + uint8_t robot_id; +} Referee_Warning_t; + +typedef struct __packed { + uint8_t countdown; +} Referee_DartCountdown_t; + +// typedef struct __packed { +// uint8_t robot_id; +// uint8_t robot_level; +// uint16_t remain_hp; +// uint16_t max_hp; +// uint16_t shoot_id1_17_cooling_rate; +// uint16_t shoot_id1_17_heat_limit; +// uint16_t shoot_id1_17_speed_limit; +// uint16_t shoot_id2_17_cooling_rate; +// uint16_t shoot_id2_17_heat_limit; +// uint16_t shoot_id2_17_speed_limit; +// uint16_t shoot_42_cooling_rate; +// uint16_t shoot_42_heat_limit; +// uint16_t shoot_42_speed_limit; +// uint16_t chassis_power_limit; +// uint8_t power_gimbal_output : 1; +// uint8_t power_chassis_output : 1; +// uint8_t power_shoot_output : 1; +// } Referee_RobotStatus_t; + +typedef struct __packed { + uint8_t robot_id; + uint8_t robot_level; + uint16_t remain_hp; + uint16_t max_hp; + uint16_t shooter_cooling_value; + uint16_t shooter_heat_limit; + uint16_t chassis_power_limit; + uint8_t power_gimbal_output : 1; + uint8_t power_chassis_output : 1; + uint8_t power_shoot_output : 1; +} Referee_RobotStatus_t; /* 0x0201 */ + +typedef struct __packed { + uint16_t chassis_volt; + uint16_t chassis_amp; + float chassis_watt; + uint16_t chassis_pwr_buff; + uint16_t shoot_id1_17_heat; + uint16_t shoot_id2_17_heat; + uint16_t shoot_42_heat; +} Referee_PowerHeat_t; + +typedef struct __packed { + float x; + float y; + float z; + float yaw; +} Referee_RobotPos_t; + +// typedef struct __packed { +// uint8_t healing : 1; +// uint8_t cooling_acc : 1; +// uint8_t defense_buff : 1; +// uint8_t attack_buff : 1; +// uint8_t res : 4; +// } Referee_RobotBuff_t; + +typedef struct __packed { + uint8_t healing_buff; + uint8_t cooling_acc; + uint8_t defense_buff; + uint8_t vulnerability_buff; + uint16_t attack_buff; +} Referee_RobotBuff_t; + +typedef struct __packed { + uint8_t attack_countdown; +} Referee_DroneEnergy_t; + +typedef struct __packed { + uint8_t armor_id : 4; + uint8_t damage_type : 4; +} Referee_RobotDamage_t; + +typedef struct __packed { + uint8_t bullet_type; + uint8_t shooter_id; + uint8_t bullet_freq; + float bullet_speed; +} Referee_ShootData_t;/* 0x0207 */ + +typedef struct __packed { + uint16_t bullet_17_remain; + uint16_t bullet_42_remain; + uint16_t coin_remain; +} Referee_BulletRemain_t; + +typedef struct __packed { + uint8_t base : 1; + uint8_t high_ground : 1; + uint8_t energy_mech : 1; + uint8_t slope : 1; + uint8_t outpose : 1; + uint8_t resource : 1; + uint8_t healing_card : 1; + uint32_t res : 24; +} Referee_RFID_t; + +typedef struct __packed { + uint8_t opening; + uint8_t target; + uint8_t target_changable_countdown; + uint8_t dart1_speed; + uint8_t dart2_speed; + uint8_t dart3_speed; + uint8_t dart4_speed; + uint16_t last_dart_launch_time; + uint16_t operator_cmd_launch_time; +} Referee_DartClient_t; + +typedef struct __packed { + float position_x; + float position_y; + float position_z; + uint8_t commd_keyboard; + uint16_t robot_id; +} Referee_ClientMap_t; + +typedef struct __packed { + int16_t mouse_x; + int16_t mouse_y; + int16_t mouse_wheel; + int8_t button_l; + int8_t button_r; + uint16_t keyboard_value; + uint16_t res; +} Referee_KeyboardMouse_t; + +typedef uint16_t Referee_Tail_t; + +typedef enum { + REF_BOT_RED_HERO = 1, + REF_BOT_RED_ENGINEER = 2, + REF_BOT_RED_INFANTRY_1 = 3, + REF_BOT_RED_INFANTRY_2 = 4, + REF_BOT_RED_INFANTRY_3 = 5, + REF_BOT_RED_DRONE = 6, + REF_BOT_RED_SENTRY = 7, + REF_BOT_RED_RADER = 9, + REF_BOT_BLU_HERO = 101, + REF_BOT_BLU_ENGINEER = 102, + REF_BOT_BLU_INFANTRY_1 = 103, + REF_BOT_BLU_INFANTRY_2 = 104, + REF_BOT_BLU_INFANTRY_3 = 105, + REF_BOT_BLU_DRONE = 106, + REF_BOT_BLU_SENTRY = 107, + REF_BOT_BLU_RADER = 109, +} Referee_RobotID_t; + +typedef enum { + REF_CL_RED_HERO = 0x0101, + REF_CL_RED_ENGINEER = 0x0102, + REF_CL_RED_INFANTRY_1 = 0x0103, + REF_CL_RED_INFANTRY_2 = 0x0104, + REF_CL_RED_INFANTRY_3 = 0x0105, + REF_CL_RED_DRONE = 0x0106, + REF_CL_BLU_HERO = 0x0165, + REF_CL_BLU_ENGINEER = 0x0166, + REF_CL_BLU_INFANTRY_1 = 0x0167, + REF_CL_BLU_INFANTRY_2 = 0x0168, + REF_CL_BLU_INFANTRY_3 = 0x0169, + REF_CL_BLU_DRONE = 0x016A, +} Referee_ClientID_t; + +typedef enum { + REF_STDNT_CMD_ID_UI_DEL = 0x0100, + REF_STDNT_CMD_ID_UI_DRAW1 = 0x0101, + REF_STDNT_CMD_ID_UI_DRAW2 = 0x0102, + REF_STDNT_CMD_ID_UI_DRAW5 = 0x0103, + REF_STDNT_CMD_ID_UI_DRAW7 = 0x0104, + REF_STDNT_CMD_ID_UI_STR = 0x0110, + + REF_STDNT_CMD_ID_CUSTOM = 0x0200, +} Referee_StudentCMDID_t; + +typedef struct __packed { + Referee_StudentCMDID_t data_cmd_id; + uint16_t id_sender; + uint16_t id_receiver; + uint8_t *data; +} Referee_InterStudent_t; + +typedef struct __packed { + uint8_t place_holder; +} Referee_InterStudent_Custom_t; + +typedef struct { + Referee_Status_t ref_status; + Referee_GameStatus_t game_status; + Referee_GameResult_t game_result; + Referee_GameRobotHP_t game_robot_hp; + Referee_DartStatus_t dart_status; + Referee_ICRAZoneStatus_t icra_zone; + Referee_FieldEvents_t field_event; + Referee_SupplyAction_t supply_action; + Referee_RequestSupply_t request_supply; + Referee_Warning_t warning; + Referee_DartCountdown_t dart_countdown; + Referee_RobotStatus_t robot_status; + Referee_PowerHeat_t power_heat; + Referee_RobotPos_t robot_pos; + Referee_RobotBuff_t robot_buff; + Referee_DroneEnergy_t drone_energy; + Referee_RobotDamage_t robot_danage; + Referee_ShootData_t shoot_data; + Referee_BulletRemain_t bullet_remain; + Referee_RFID_t rfid; + Referee_DartClient_t dart_client; + Referee_InterStudent_Custom_t custom; + Referee_ClientMap_t client_map; + Referee_KeyboardMouse_t keyboard_mouse; + + osTimerId_t ui_fast_timer_id; + osTimerId_t ui_slow_timer_id; +} Referee_t; + +typedef struct { + CMD_ChassisMode_t mode; + float angle; +} Referee_ChassisUI_t; + +typedef struct { + float percentage; + // CAN_CapStatus_t status; +} Referee_CapUI_t; + +typedef struct { + CMD_GimbalMode_t mode; +} Referee_GimbalUI_t; + +typedef struct { + CMD_ShootMode_t mode; + CMD_FireMode_t fire; +} Referee_ShootUI_t; + +typedef struct __packed { + /* UI缓冲数据 */ + UI_Ele_t grapic[REF_UI_MAX_GRAPIC_NUM]; + UI_Drawcharacter_t character_data[REF_UI_MAX_STRING_NUM]; + UI_Del_t del[REF_UI_MAX_DEL_NUM]; + /* 待发送数量 */ + uint8_t grapic_counter; + uint8_t character_counter; + uint8_t del_counter; + /* UI所需信息 */ + Referee_CapUI_t cap_ui; + Referee_ChassisUI_t chassis_ui; + Referee_ShootUI_t shoot_ui; + Referee_GimbalUI_t gimbal_ui; + bool cmd_pc; + /* 屏幕分辨率 */ + const CMD_Screen_t *screen; +} Referee_UI_t; + +typedef struct __packed { + uint16_t data_cmd_id; + uint16_t sender_ID; + uint16_t receiver_ID; +} Referee_Interactive_Header_t; + +typedef struct __packed { + Referee_Header_t header; + uint16_t cmd_id; + Referee_Interactive_Header_t IA_header; + UI_Drawgrapic_1_t data; + uint16_t crc16; +} Referee_UI_Drawgrapic_1_t; + +typedef struct __packed { + Referee_Header_t header; + uint16_t cmd_id; + Referee_Interactive_Header_t IA_header; + UI_Drawgrapic_2_t data; + uint16_t crc16; +} Referee_UI_Drawgrapic_2_t; + +typedef struct __packed { + Referee_Header_t header; + uint16_t cmd_id; + Referee_Interactive_Header_t IA_header; + UI_Drawgrapic_5_t data; + uint16_t crc16; +} Referee_UI_Drawgrapic_5_t; + +typedef struct __packed { + Referee_Header_t header; + uint16_t cmd_id; + Referee_Interactive_Header_t IA_header; + UI_Drawgrapic_7_t data; + uint16_t crc16; +} Referee_UI_Drawgrapic_7_t; + +typedef struct __packed { + Referee_Header_t header; + uint16_t cmd_id; + Referee_Interactive_Header_t IA_header; + UI_Drawcharacter_t data; + uint16_t crc16; +} Referee_UI_Drawcharacter_t; + +typedef struct __packed { + Referee_Header_t header; + uint16_t cmd_id; + Referee_Interactive_Header_t IA_header; + UI_Del_t data; + uint16_t crc16; +} Referee_UI_Del_t; + +typedef struct { + Referee_Status_t ref_status; + float chassis_watt; + float chassis_power_limit; + float chassis_pwr_buff; +} Referee_ForCap_t; + +typedef struct { + Referee_Status_t ref_status; +} Referee_ForAI_t; + +typedef struct { + Referee_Status_t ref_status; + float chassis_power_limit; + float chassis_pwr_buff; +} Referee_ForChassis_t; + +typedef struct { + Referee_Status_t ref_status; + Referee_PowerHeat_t power_heat; + Referee_RobotStatus_t robot_status; + Referee_ShootData_t shoot_data; +} Referee_ForShoot_t; + +/* Exported functions prototypes -------------------------------------------- */ +int8_t Referee_Init(Referee_t *ref, Referee_UI_t *ui, + const CMD_Screen_t *screen); +int8_t Referee_Restart(void); + +int8_t Referee_StartReceiving(Referee_t *ref); +int8_t Referee_Parse(Referee_t *ref); +void Referee_HandleOffline(Referee_t *referee); +int8_t Referee_SetHeader(Referee_Interactive_Header_t *header, + Referee_StudentCMDID_t cmd_id, uint8_t sender_id); +int8_t Referee_StartSend(uint8_t *data, uint32_t len); +int8_t Referee_MoveData(void *data, void *tmp, uint32_t len); +int8_t Referee_PackUI(Referee_UI_t *ui, Referee_t *ref); +UI_Ele_t *Referee_GetGrapicAdd(Referee_UI_t *ref_ui); +UI_Drawcharacter_t *Referee_GetCharacterAdd(Referee_UI_t *ref_ui); +uint8_t Referee_PraseCmd(Referee_UI_t *ref_ui, CMD_UI_t cmd); +uint8_t Referee_PackCap(Referee_ForCap_t *cap, const Referee_t *ref); +uint8_t Referee_PackShoot(Referee_ForShoot_t *ai, Referee_t *ref); +uint8_t Referee_PackChassis(Referee_ForChassis_t *chassis, + const Referee_t *ref); +uint8_t Referee_PackAI(Referee_ForAI_t *shoot, const Referee_t *ref); +uint8_t Referee_UIRefresh(Referee_UI_t *ui); +#ifdef __cplusplus +} +#endif diff --git a/engineer/User/module/arm.c b/engineer/User/module/arm.c index 9aecb3f..e718864 100644 --- a/engineer/User/module/arm.c +++ b/engineer/User/module/arm.c @@ -6,6 +6,7 @@ #include "component/pid.h" #include "component/user_math.h" #include "bsp/dwt.h" +#include "component/cmd.h" @@ -262,7 +263,7 @@ void arm_status_reset(arm_t* arm) arm->status.step=0; } -void arm_follow_control(arm_t* arm,arm_rx_data_t* data) +void arm_follow_control(arm_t* arm,CMD_Arm_t* data) { assert_param(!(arm==NULL||data==NULL)); switch (arm->status.step) { @@ -270,7 +271,7 @@ void arm_follow_control(arm_t* arm,arm_rx_data_t* data) { if(arm->status.busy==0) //检测是否非忙碌,并进行第一次初始化 { - arm_path_start(arm, data->ref_angle); //初始化路径规划,设定终点为控制器当前值 + arm_path_start(arm, data->data.ref_angle); //初始化路径规划,设定终点为控制器当前值 arm->status.busy=1; //开始路径，进入忙碌 } arm_path_process(arm); //计算并设定一次路径点 @@ -285,14 +286,14 @@ void arm_follow_control(arm_t* arm,arm_rx_data_t* data) { arm->status.busy=0; //退出忙碌，可以接收指令 for (int i=0; ijoint[i].ref.position += data->ref_angle[i]; //跟踪控制器 + arm->joint[i].ref.position += data->data.ref_angle[i]; //跟踪控制器 } break; } } } -void arm_controler_process(arm_t* arm,arm_rx_data_t* data) //自定义控制器指令处理 +void arm_controler_process(arm_t* arm,CMD_Arm_t* data) //自定义控制器指令处理 { assert_param(!(arm==NULL||data==NULL)); @@ -304,24 +305,27 @@ void arm_controler_process(arm_t* arm,arm_rx_data_t* data) //自定义控制器 if(data->cmd_prepare==0&&arm->status.cmd_prepare==1) //接收准备信号结束，并且已处于接收准备状态 { arm_status_reset(arm); //清除状态 - switch (data->cmd) { - case set_relax_mode: + switch (data->mode) { + case SET_ARM_MODE_RELAX: arm->mode = RELAX_MODE; break; - case set_follow_mode: + case SET_ARM_MODE_FOLLOW: arm->mode = FOLLOW_MODE; break; - case start_storage: + case SET_ARM_MODE_STORAGE: arm->mode=STORAGE_MODE; break; - case start_take: + case SET_ARM_MODE_TAKE: arm->mode=TAKE_MODE; break; + case SET_ARM_MODE_POSITION: + arm->mode=SET_ARM_MODE_POSITION; + break; } } } -void arm_control_process(arm_t* arm,arm_rx_data_t* data) +void arm_control_process(arm_t* arm,CMD_Arm_t* data) { switch (arm->mode) { case RELAX_MODE: @@ -334,6 +338,8 @@ void arm_control_process(arm_t* arm,arm_rx_data_t* data) break; case TAKE_MODE: break; + case POSITION_MODE: + break; } } diff --git a/engineer/User/module/arm.h b/engineer/User/module/arm.h index 719a9bf..3c86d95 100644 --- a/engineer/User/module/arm.h +++ b/engineer/User/module/arm.h @@ -1,4 +1,9 @@ #pragma once + +#ifdef __cplusplus +extern "C" { +#endif + #include "main.h" #include "component/kinematics.h" #include "device/motor_dm.h" @@ -87,7 +92,6 @@ typedef struct{ typedef struct hand_t { float mass[6]; - robot_t kine; joint_t joint[JOINT_NUM]; struct { uint8_t cmd_prepare; @@ -99,10 +103,14 @@ typedef struct hand_t RELAX_MODE=0, FOLLOW_MODE, STORAGE_MODE, - TAKE_MODE + TAKE_MODE, + POSITION_MODE }mode; }arm_t; +#ifdef __cplusplus +} +#endif diff --git a/engineer/User/task/control.c b/engineer/User/task/control.c index 3121532..be1e514 100644 --- a/engineer/User/task/control.c +++ b/engineer/User/task/control.c @@ -8,6 +8,9 @@ /* USER INCLUDE BEGIN */ #include "component/kinematics.h" #include "device/motor_lz.h" +#include "device/motor_dm.h" +#include "component/pid.h" +#include "component/filter.h" /* USER INCLUDE END */ /* Private typedef ---------------------------------------------------------- */ @@ -130,4 +133,4 @@ void Task_control(void *argument) { osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ } -} \ No newline at end of file +} diff --git a/engineer/User/task/init.c b/engineer/User/task/init.c index 9028559..50cf2ef 100644 --- a/engineer/User/task/init.c +++ b/engineer/User/task/init.c @@ -31,7 +31,6 @@ void Task_Init(void *argument) { /* 创建任务线程 */ task_runtime.thread.control = osThreadNew(Task_control, NULL, &attr_control); - task_runtime.thread.display = osThreadNew(Task_display, NULL, &attr_display); // 创建消息队列 /* USER MESSAGE BEGIN */ @@ -40,4 +39,4 @@ void Task_Init(void *argument) { osKernelUnlock(); // 解锁内核 osThreadTerminate(osThreadGetId()); // 任务完成后结束自身 -} \ No newline at end of file +} diff --git a/engineer/cmake/stm32cubemx/CMakeLists.txt b/engineer/cmake/stm32cubemx/CMakeLists.txt index 20b3af4..a2e5836 100644 --- a/engineer/cmake/stm32cubemx/CMakeLists.txt +++ b/engineer/cmake/stm32cubemx/CMakeLists.txt @@ -26,6 +26,7 @@ set(MX_Application_Src ${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/gpio.c ${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/freertos.c ${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/can.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/usart.c ${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/stm32f4xx_it.c ${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/stm32f4xx_hal_msp.c ${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/stm32f4xx_hal_timebase_tim.c @@ -52,7 +53,8 @@ set(STM32_Drivers_Src ${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pwr_ex.c ${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_cortex.c ${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal.c - ${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_exti.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_exti.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_uart.c ) # Drivers Midllewares