diff --git a/soccer_firmware/.cproject b/soccer_firmware/.cproject
index 26324dfde..c8fb513aa 100644
--- a/soccer_firmware/.cproject
+++ b/soccer_firmware/.cproject
@@ -24,7 +24,7 @@
-
+
@@ -76,12 +76,7 @@
-
-
-
-
-
-
+
@@ -179,12 +174,7 @@
-
-
-
-
-
-
+
@@ -212,172 +202,6 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
@@ -386,25 +210,5 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
diff --git a/soccer_firmware/.mxproject b/soccer_firmware/.mxproject
index 3088a6cfa..29fc22afc 100644
--- a/soccer_firmware/.mxproject
+++ b/soccer_firmware/.mxproject
@@ -1,5 +1,5 @@
[PreviousLibFiles]
-LibFiles=Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_pcd.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_pcd_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_usb.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_i2c.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_i2c.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_i2c_ex.h;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_uart.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_usart.h;Middlewares/ST/STM32_USB_Device_Library/Core/Inc/usbd_core.h;Middlewares/ST/STM32_USB_Device_Library/Core/Inc/usbd_ctlreq.h;Middlewares/ST/STM32_USB_Device_Library/Core/Inc/usbd_def.h;Middlewares/ST/STM32_USB_Device_Library/Core/Inc/usbd_ioreq.h;Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc/usbd_cdc.h;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pcd.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pcd_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_ll_usb.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_i2c.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_i2c_ex.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_uart.c;Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_core.c;Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ctlreq.c;Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ioreq.c;Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.c;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_pcd.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_pcd_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_usb.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_i2c.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_i2c.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_i2c_ex.h;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_uart.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_usart.h;Middlewares/ST/STM32_USB_Device_Library/Core/Inc/usbd_core.h;Middlewares/ST/STM32_USB_Device_Library/Core/Inc/usbd_ctlreq.h;Middlewares/ST/STM32_USB_Device_Library/Core/Inc/usbd_def.h;Middlewares/ST/STM32_USB_Device_Library/Core/Inc/usbd_ioreq.h;Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc/usbd_cdc.h;Drivers/CMSIS/Device/ST/STM32F4xx/Include/stm32f446xx.h;Drivers/CMSIS/Device/ST/STM32F4xx/Include/stm32f4xx.h;Drivers/CMSIS/Device/ST/STM32F4xx/Include/system_stm32f4xx.h;Drivers/CMSIS/Device/ST/STM32F4xx/Source/Templates/system_stm32f4xx.c;Drivers/CMSIS/Include/mpu_armv8.h;Drivers/CMSIS/Include/core_sc000.h;Drivers/CMSIS/Include/cmsis_gcc.h;Drivers/CMSIS/Include/core_sc300.h;Drivers/CMSIS/Include/core_armv8mml.h;Drivers/CMSIS/Include/cmsis_armcc.h;Drivers/CMSIS/Include/core_cm3.h;Drivers/CMSIS/Include/core_cm0.h;Drivers/CMSIS/Include/tz_context.h;Drivers/CMSIS/Include/mpu_armv7.h;Drivers/CMSIS/Include/core_cm4.h;Drivers/CMSIS/Include/cmsis_version.h;Drivers/CMSIS/Include/cmsis_compiler.h;Drivers/CMSIS/Include/cmsis_iccarm.h;Drivers/CMSIS/Include/core_cm1.h;Drivers/CMSIS/Include/core_armv8mbl.h;Drivers/CMSIS/Include/cmsis_armclang.h;Drivers/CMSIS/Include/core_cm23.h;Drivers/CMSIS/Include/core_cm33.h;Drivers/CMSIS/Include/core_cm0plus.h;Drivers/CMSIS/Include/core_cm7.h;
+LibFiles=Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_pcd.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_pcd_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_usb.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_i2c.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_i2c.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_i2c_ex.h;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_uart.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_usart.h;Middlewares/ST/STM32_USB_Device_Library/Core/Inc/usbd_core.h;Middlewares/ST/STM32_USB_Device_Library/Core/Inc/usbd_ctlreq.h;Middlewares/ST/STM32_USB_Device_Library/Core/Inc/usbd_def.h;Middlewares/ST/STM32_USB_Device_Library/Core/Inc/usbd_ioreq.h;Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc/usbd_cdc.h;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pcd.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pcd_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_ll_usb.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_i2c.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_i2c_ex.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_uart.c;Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_core.c;Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ctlreq.c;Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ioreq.c;Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.c;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_pcd.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_pcd_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_usb.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_i2c.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_i2c.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_i2c_ex.h;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_uart.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_usart.h;Middlewares/ST/STM32_USB_Device_Library/Core/Inc/usbd_core.h;Middlewares/ST/STM32_USB_Device_Library/Core/Inc/usbd_ctlreq.h;Middlewares/ST/STM32_USB_Device_Library/Core/Inc/usbd_def.h;Middlewares/ST/STM32_USB_Device_Library/Core/Inc/usbd_ioreq.h;Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc/usbd_cdc.h;Drivers/CMSIS/Device/ST/STM32F4xx/Include/stm32f446xx.h;Drivers/CMSIS/Device/ST/STM32F4xx/Include/stm32f4xx.h;Drivers/CMSIS/Device/ST/STM32F4xx/Include/system_stm32f4xx.h;Drivers/CMSIS/Device/ST/STM32F4xx/Source/Templates/system_stm32f4xx.c;Drivers/CMSIS/Include/cmsis_armclang.h;Drivers/CMSIS/Include/core_cm1.h;Drivers/CMSIS/Include/mpu_armv8.h;Drivers/CMSIS/Include/core_cm23.h;Drivers/CMSIS/Include/core_sc000.h;Drivers/CMSIS/Include/core_cm33.h;Drivers/CMSIS/Include/cmsis_armcc.h;Drivers/CMSIS/Include/core_armv8mml.h;Drivers/CMSIS/Include/core_cm0plus.h;Drivers/CMSIS/Include/core_sc300.h;Drivers/CMSIS/Include/core_armv8mbl.h;Drivers/CMSIS/Include/core_cm0.h;Drivers/CMSIS/Include/cmsis_gcc.h;Drivers/CMSIS/Include/core_cm7.h;Drivers/CMSIS/Include/core_cm3.h;Drivers/CMSIS/Include/cmsis_version.h;Drivers/CMSIS/Include/core_cm4.h;Drivers/CMSIS/Include/cmsis_compiler.h;Drivers/CMSIS/Include/mpu_armv7.h;Drivers/CMSIS/Include/tz_context.h;Drivers/CMSIS/Include/cmsis_iccarm.h;
[PreviousUsedCubeIDEFiles]
SourceFiles=Core/Src/main.c;USB_DEVICE/App/usb_device.c;USB_DEVICE/Target/usbd_conf.c;USB_DEVICE/App/usbd_desc.c;USB_DEVICE/App/usbd_cdc_if.c;Core/Src/stm32f4xx_it.c;Core/Src/stm32f4xx_hal_msp.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pcd.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pcd_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_ll_usb.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_i2c.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_i2c_ex.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_uart.c;Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_core.c;Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ctlreq.c;Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ioreq.c;Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.c;Drivers/CMSIS/Device/ST/STM32F4xx/Source/Templates/system_stm32f4xx.c;Core/Src/system_stm32f4xx.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pcd.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pcd_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_ll_usb.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_i2c.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_i2c_ex.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_uart.c;Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_core.c;Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ctlreq.c;Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ioreq.c;Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.c;Drivers/CMSIS/Device/ST/STM32F4xx/Source/Templates/system_stm32f4xx.c;Core/Src/system_stm32f4xx.c;;;Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_core.c;Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ctlreq.c;Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ioreq.c;Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.c;
@@ -34,4 +34,3 @@ SourcePath#0=../USB_DEVICE/App
SourcePath#1=../USB_DEVICE/Target
SourcePath#2=../Core/Src
SourceFiles=;
-
diff --git a/soccer_firmware/Core/Inc/BMI088.h b/soccer_firmware/Core/Inc/BMI088.h
new file mode 100644
index 000000000..d4584a78d
--- /dev/null
+++ b/soccer_firmware/Core/Inc/BMI088.h
@@ -0,0 +1,102 @@
+#ifndef BMI088_IMU_H
+#define BMI088_IMU_H
+
+#include "stm32f4xx_hal.h"
+#include "main.h"
+
+/* Register defines */
+#define BMI_GYR_I2C_ADDRESS (0x68 << 1)
+#define BMI_ACC_I2C_ADDRESS (0x18 << 1)
+
+#define BMI_ACC_CHIP_ID 0x00
+#define BMI_ACC_DATA 0x12
+#define BMI_TEMP_DATA 0x22
+#define BMI_ACC_CONF 0x40
+#define BMI_ACC_RANGE 0x41
+#define BMI_INT1_IO_CONF 0x53
+#define BMI_INT1_INT2_MAP_DATA 0x58
+#define BMI_ACC_PWR_CONF 0x7C
+#define BMI_ACC_PWR_CTRL 0x7D
+#define BMI_ACC_SOFTRESET 0x7E
+
+#define BMI_GYR_CHIP_ID 0x00
+#define BMI_GYR_DATA 0x02
+#define BMI_GYR_RANGE 0x0F
+#define BMI_GYR_BANDWIDTH 0x10
+#define BMI_GYR_SOFTRESET 0x14
+#define BMI_GYR_INT_CTRL 0x15
+#define BMI_INT3_INT4_IO_CONF 0x16
+#define BMI_INT3_INT4_IO_MAP 0x18
+
+typedef struct {
+
+ /* I2C */
+ I2C_HandleTypeDef *m_i2c_handle;
+// GPIO_TypeDef *csAccPinBank;
+// GPIO_TypeDef *csGyrPinBank;
+// uint16_t csAccPin;
+// uint16_t csGyrPin;
+
+ /* DMA */
+ uint8_t readingAcc;
+ uint8_t readingGyr;
+ uint8_t accTxBuf[8];
+ uint8_t gyrTxBuf[7];
+ volatile uint8_t accRxBuf[8];
+ volatile uint8_t gyrRxBuf[7];
+
+ /* Conversion constants (raw to m/s^2 and raw to rad/s) */
+ float accConversion;
+ float gyrConversion;
+
+ /* x-y-z measurements */
+ float acc_mps2[3];
+ float gyr_rps[3];
+
+} BMI088;
+
+/*
+ *
+ * INITIALISATION
+ *
+ */
+uint8_t BMI088_Init(BMI088 *imu, I2C_HandleTypeDef *m_i2c_handle);
+
+/*
+ *
+ * LOW-LEVEL REGISTER FUNCTIONS
+ *
+ */
+uint8_t BMI088_ReadAccRegister(BMI088 *imu, uint8_t regAddr, uint8_t *data);
+uint8_t BMI088_ReadGyrRegister(BMI088 *imu, uint8_t regAddr, uint8_t *data);
+
+uint8_t BMI088_WriteAccRegister(BMI088 *imu, uint8_t regAddr, uint8_t data);
+uint8_t BMI088_WriteGyrRegister(BMI088 *imu, uint8_t regAddr, uint8_t data);
+
+/*
+ *
+ * POLLING
+ *
+ */
+uint8_t BMI088_ReadAccelerometer(BMI088 *imu, I2C_HandleTypeDef *hi2c, int16_t *accX, int16_t *accY, int16_t *accZ);
+uint8_t BMI088_ReadGyroscope(I2C_HandleTypeDef *hi2c, int16_t *gyroX, int16_t *gyroY, int16_t *gyroZ);
+
+/*
+ *
+ * DMA
+ *
+ */
+uint8_t BMI088_ReadAccelerometerDMA(BMI088 *imu);
+void BMI088_ReadAccelerometerDMA_Complete(BMI088 *imu);
+
+uint8_t BMI088_ReadGyroscopeDMA(BMI088 *imu);
+void BMI088_ReadGyroscopeDMA_Complete(BMI088 *imu);
+
+/*
+ *
+ * WORKAROUND
+ *
+ */
+void generateClocks2(uint8_t num_clocks, uint8_t send_stop_bits);
+
+#endif
diff --git a/soccer_firmware/Core/Inc/dynamixel_p2.h b/soccer_firmware/Core/Inc/dynamixel_p2.h
index 1d966788c..0c9970d15 100644
--- a/soccer_firmware/Core/Inc/dynamixel_p2.h
+++ b/soccer_firmware/Core/Inc/dynamixel_p2.h
@@ -19,6 +19,8 @@ void motor_torque_en_p2(MotorPort *p, uint8_t id, uint8_t val);
void read_motor_id_p2(MotorPort * p);
void read_motor_present_position_p2(MotorPort * p, uint8_t id);
+void write_min_position_limit_p2(MotorPort *port, uint8_t id, uint32_t limit);
+void write_max_position_limit_p2(MotorPort *port, uint8_t id, uint32_t limit);
void _motor_write_p2(MotorPort *p, uint8_t id, uint16_t addr, uint8_t* data, uint16_t dataLen);
void _motor_read_p2(MotorPort *p, uint8_t id, uint16_t addr, uint16_t dataLen);
diff --git a/soccer_firmware/Core/Src/BMI088.c b/soccer_firmware/Core/Src/BMI088.c
new file mode 100644
index 000000000..eeeca1661
--- /dev/null
+++ b/soccer_firmware/Core/Src/BMI088.c
@@ -0,0 +1,393 @@
+#include "BMI088.h"
+
+/*
+ *
+ * INITIALISATION
+ *
+ */
+uint8_t BMI088_Init(BMI088 *imu, I2C_HandleTypeDef *m_i2c_handle) {
+
+ /* Store interface parameters in struct */
+ imu->m_i2c_handle = m_i2c_handle;
+
+ /* Clear DMA flags */
+ imu->readingAcc = 0;
+ imu->readingGyr = 0;
+
+ uint8_t status = 0;
+
+ /*
+ *
+ * ACCELEROMETER
+ *
+ */
+
+ /* Perform accelerometer soft reset */
+ status += BMI088_WriteAccRegister(imu, BMI_ACC_SOFTRESET, 0xB6);
+ HAL_Delay(50);
+
+ /* Check chip ID */
+ uint8_t chipID = 0;
+ status = BMI088_ReadAccRegister(imu, BMI_ACC_CHIP_ID, &chipID);
+
+ // chipID bmi085 is 0x1f, chipID bmi088 is 0x1e
+ if (chipID != 0x1e) {
+ return 0;
+ }
+ HAL_Delay(10);
+
+ /* Configure accelerometer */
+ status += BMI088_WriteAccRegister(imu, BMI_ACC_CONF, 0xA8); /* (no oversampling, ODR = 100 Hz, BW = 40 Hz) */
+ HAL_Delay(10);
+
+ status += BMI088_WriteAccRegister(imu, BMI_ACC_RANGE, 0x00); /* +- 3g range (for bmi085 +- 2g range)*/
+ HAL_Delay(10);
+
+ /* Enable accelerometer data ready interrupt */
+ status += BMI088_WriteAccRegister(imu, BMI_INT1_IO_CONF, 0x0A); /* INT1 = push-pull output, active high */
+ HAL_Delay(10);
+
+ status += BMI088_WriteAccRegister(imu, BMI_INT1_INT2_MAP_DATA, 0x04);
+ HAL_Delay(10);
+
+ /* Put accelerometer into active mode */
+ status += BMI088_WriteAccRegister(imu, BMI_ACC_PWR_CONF, 0x00);
+ HAL_Delay(10);
+
+ /* Turn accelerometer on */
+ status += BMI088_WriteAccRegister(imu, BMI_ACC_PWR_CTRL, 0x04);
+ HAL_Delay(10);
+
+ /* Pre-compute accelerometer conversion constant (raw to m/s^2) */
+ imu->accConversion = 9.81f / 32768.0f * 2.0f * 1.5f; /* Datasheet page 27 */
+
+ /* Set accelerometer TX buffer for DMA */
+ imu->accTxBuf[0] = BMI_ACC_DATA | 0x80;
+
+ /*
+ *
+ * GYROSCOPE
+ *
+ */
+
+// HAL_GPIO_WritePin(imu->csGyrPinBank, imu->csGyrPin, GPIO_PIN_SET);
+
+ /* Perform gyro soft reset */
+ status += BMI088_WriteGyrRegister(imu, BMI_GYR_SOFTRESET, 0xB6);
+ HAL_Delay(250);
+
+ /* Check chip ID */
+ status += BMI088_ReadGyrRegister(imu, BMI_GYR_CHIP_ID, &chipID);
+
+ // chip ID is 0x0F for both BMI088/085
+ if (chipID != 0x0F) {
+ return 0;
+ }
+ HAL_Delay(10);
+
+ /* Configure gyroscope */
+ status += BMI088_WriteGyrRegister(imu, BMI_GYR_RANGE, 0x01); /* +- 1000 deg/s (bmi085 also +-1000deg/s)*/
+ HAL_Delay(10);
+
+ status += BMI088_WriteGyrRegister(imu, BMI_GYR_BANDWIDTH, 0x07); /* ODR = 100 Hz, Filter bandwidth = 32 Hz */
+ HAL_Delay(10);
+
+ /* Enable gyroscope data ready interrupt */
+ status += BMI088_WriteGyrRegister(imu, BMI_GYR_INT_CTRL, 0x80); /* New data interrupt enabled */
+ HAL_Delay(10);
+
+ status += BMI088_WriteGyrRegister(imu, BMI_INT3_INT4_IO_CONF, 0x01); /* INT3 = push-pull, active high */
+ HAL_Delay(10);
+
+ status += BMI088_WriteGyrRegister(imu, BMI_INT3_INT4_IO_MAP, 0x01); /* Data ready interrupt mapped to INT3 pin */
+ HAL_Delay(10);
+
+ /* Pre-compute gyroscope conversion constant (raw to rad/s) */
+ imu->gyrConversion = 0.01745329251f * 1000.0f / 32768.0f; /* Datasheet page 39 */
+
+ /* Set gyroscope TX buffer for DMA */
+ imu->gyrTxBuf[0] = BMI_GYR_DATA | 0x80;
+
+ return status;
+
+}
+
+/*
+ *
+ * LOW-LEVEL REGISTER FUNCTIONS
+ *
+ */
+
+/* ACCELEROMETER READS ARE DIFFERENT TO GYROSCOPE READS. SEND ONE BYTE ADDRESS, READ ONE DUMMY BYTE, READ TRUE DATA !!! */
+uint8_t BMI088_ReadAccRegister(BMI088 *imu, uint8_t regAddr, uint8_t *data) {
+ HAL_StatusTypeDef status;
+
+ // Read data from the specified register address
+ status = HAL_I2C_Mem_Read(imu->m_i2c_handle, BMI_ACC_I2C_ADDRESS, regAddr, I2C_MEMADD_SIZE_8BIT, data, 1, HAL_MAX_DELAY);
+ if (status != HAL_OK) {
+ return status;
+ }
+
+ return HAL_OK;
+}
+
+uint8_t BMI088_ReadGyrRegister(BMI088 *imu, uint8_t regAddr, uint8_t *data) {
+ HAL_StatusTypeDef status;
+
+ // Read data from the specified register address
+ status = HAL_I2C_Mem_Read(imu->m_i2c_handle, BMI_GYR_I2C_ADDRESS, regAddr, I2C_MEMADD_SIZE_8BIT, data, 1, HAL_MAX_DELAY);
+ if (status != HAL_OK) {
+ return status;
+ }
+
+ return HAL_OK;
+}
+
+uint8_t BMI088_WriteAccRegister(BMI088 *imu, uint8_t regAddr, uint8_t data) {
+ HAL_StatusTypeDef status;
+
+ // Write data to the specified register address
+ status = HAL_I2C_Mem_Write(imu->m_i2c_handle, BMI_ACC_I2C_ADDRESS, regAddr, I2C_MEMADD_SIZE_8BIT, &data, 1, HAL_MAX_DELAY);
+ if (status != HAL_OK) {
+ return status;
+ }
+
+ return HAL_OK;
+}
+
+uint8_t BMI088_WriteGyrRegister(BMI088 *imu, uint8_t regAddr, uint8_t data) {
+ HAL_StatusTypeDef status;
+
+ // Write data to the specified register address
+ status = HAL_I2C_Mem_Write(imu->m_i2c_handle, BMI_GYR_I2C_ADDRESS, regAddr, I2C_MEMADD_SIZE_8BIT, &data, 1, HAL_MAX_DELAY);
+ if (status != HAL_OK) {
+ return status;
+ }
+
+ return HAL_OK;
+}
+
+
+
+/*
+ *
+ * POLLING
+ *
+ */
+uint8_t BMI088_ReadAccelerometer(BMI088 *imu, I2C_HandleTypeDef *hi2c, int16_t *accX, int16_t *accY, int16_t *accZ) {
+ HAL_StatusTypeDef status;
+ uint8_t data[6]; // Data buffer to store gyroscope data
+
+ status = HAL_I2C_Mem_Read(hi2c, BMI_ACC_I2C_ADDRESS, BMI_ACC_DATA, I2C_MEMADD_SIZE_8BIT, data, 6, HAL_MAX_DELAY);
+ if (status != HAL_OK) {
+ return status;
+ }
+
+ /* Form signed 16-bit integers */
+ *accX = (int16_t) ((data[1] << 8) | data[0]);
+ *accY = (int16_t) ((data[3] << 8) | data[2]);
+ *accZ = (int16_t) ((data[5] << 8) | data[4]);
+
+ /* Convert to m/s^2 */
+ imu->acc_mps2[0] = imu->accConversion * (*accX);
+ imu->acc_mps2[1] = imu->accConversion * (*accY);
+ imu->acc_mps2[2] = imu->accConversion * (*accZ);
+
+ return status;
+}
+
+
+// Function to read gyroscope values from BMI088 sensor
+HAL_StatusTypeDef BMI088_ReadGyroscope(I2C_HandleTypeDef *hi2c, int16_t *gyroX, int16_t *gyroY, int16_t *gyroZ) {
+ HAL_StatusTypeDef status;
+ uint8_t data[6]; // Data buffer to store gyroscope data
+
+ status = HAL_I2C_Mem_Read(hi2c, BMI_GYR_I2C_ADDRESS, BMI_GYR_DATA, I2C_MEMADD_SIZE_8BIT, data, 6, HAL_MAX_DELAY);
+
+ if (status != HAL_OK) {
+ return status;
+ }
+
+ // Parse gyroscope data (each value is 16 bits, little-endian)
+ *gyroX = (int16_t)((data[1] << 8) | data[0]);
+ *gyroY = (int16_t)((data[3] << 8) | data[2]);
+ *gyroZ = (int16_t)((data[5] << 8) | data[4]);
+
+ // do gyro conversion here?
+
+ return status;
+}
+
+/*
+ *
+ * DMA
+ *
+ */
+uint8_t BMI088_ReadAccelerometerDMA(BMI088 *imu) {
+
+// HAL_GPIO_WritePin(imu->csAccPinBank, imu->csAccPin, GPIO_PIN_RESET);
+// if (HAL_SPI_TransmitReceive_DMA(imu->spiHandle, imu->accTxBuf, (uint8_t *) imu->accRxBuf, 8) == HAL_OK) {
+//
+// imu->readingAcc = 1;
+// return 1;
+//
+// } else {
+//
+// HAL_GPIO_WritePin(imu->csAccPinBank, imu->csAccPin, GPIO_PIN_SET);
+// return 0;
+//
+// }
+
+}
+
+void BMI088_ReadAccelerometerDMA_Complete(BMI088 *imu) {
+
+// HAL_GPIO_WritePin(imu->csAccPinBank, imu->csAccPin, GPIO_PIN_SET);
+// imu->readingAcc = 0;
+//
+// /* Form signed 16-bit integers */
+// int16_t accX = (int16_t) ((imu->accRxBuf[3] << 8) | imu->accRxBuf[2]);
+// int16_t accY = (int16_t) ((imu->accRxBuf[5] << 8) | imu->accRxBuf[4]);
+// int16_t accZ = (int16_t) ((imu->accRxBuf[7] << 8) | imu->accRxBuf[6]);
+//
+// /* Convert to m/s^2 */
+// imu->acc_mps2[0] = imu->accConversion * accX;
+// imu->acc_mps2[1] = imu->accConversion * accY;
+// imu->acc_mps2[2] = imu->accConversion * accZ;
+
+}
+
+uint8_t BMI088_ReadGyroscopeDMA(BMI088 *imu) {
+
+// HAL_GPIO_WritePin(imu->csGyrPinBank, imu->csGyrPin, GPIO_PIN_RESET);
+// if (HAL_SPI_TransmitReceive_DMA(imu->spiHandle, imu->gyrTxBuf, (uint8_t *) imu->gyrRxBuf, 7) == HAL_OK) {
+//
+// imu->readingGyr = 1;
+// return 1;
+//
+// } else {
+//
+// HAL_GPIO_WritePin(imu->csGyrPinBank, imu->csGyrPin, GPIO_PIN_SET);
+// return 0;
+//
+// }
+
+}
+
+void BMI088_ReadGyroscopeDMA_Complete(BMI088 *imu) {
+
+// HAL_GPIO_WritePin(imu->csGyrPinBank, imu->csGyrPin, GPIO_PIN_SET);
+// imu->readingGyr = 0;
+//
+// /* Form signed 16-bit integers */
+// int16_t gyrX = (int16_t) ((imu->gyrRxBuf[2] << 8) | imu->gyrRxBuf[1]);
+// int16_t gyrY = (int16_t) ((imu->gyrRxBuf[4] << 8) | imu->gyrRxBuf[3]);
+// int16_t gyrZ = (int16_t) ((imu->gyrRxBuf[6] << 8) | imu->gyrRxBuf[5]);
+//
+// /* Convert to deg/s */
+// imu->gyr_rps[0] = imu->gyrConversion * gyrX;
+// imu->gyr_rps[1] = imu->gyrConversion * gyrY;
+// imu->gyr_rps[2] = imu->gyrConversion * gyrZ;
+
+}
+
+/**
+ * @brief This function big-bangs the I2C master clock
+ * https://electronics.stackexchange.com/questions/267972/i2c-busy-flag-strange-behaviour/281046#281046
+ * https://community.st.com/thread/35884-cant-reset-i2c-in-stm32f407-to-release-i2c-lines
+ * https://electronics.stackexchange.com/questions/272427/stm32-busy-flag-is-set-after-i2c-initialization
+ * http://www.st.com/content/ccc/resource/technical/document/errata_sheet/f5/50/c9/46/56/db/4a/f6/CD00197763.pdf/files/CD00197763.pdf/jcr:content/translations/en.CD00197763.pdf
+ * @param num_clocks The number of times to cycle the I2C master clock
+ * @param send_stop_bits 1 if stop bits are to be sent on SDA
+ * @return None
+ */
+void generateClocks2(uint8_t num_clocks, uint8_t send_stop_bits){
+ static struct I2C_Module{
+ I2C_HandleTypeDef* instance;
+ uint16_t sda_pin;
+ GPIO_TypeDef* sda_port;
+ uint16_t scl_pin;
+ GPIO_TypeDef* scl_port;
+ }i2cmodule = {&hi2c1, GPIO_PIN_7, GPIOB, GPIO_PIN_6, GPIOB};
+
+ static struct I2C_Module* i2c = &i2cmodule;
+ static uint8_t timeout = 1;
+
+ GPIO_InitTypeDef gpio_init_struct;
+
+ I2C_HandleTypeDef* handler = NULL;
+
+ handler = i2c->instance;
+
+ // 1. Clear PE bit.
+ CLEAR_BIT(handler->Instance->CR1, I2C_CR1_PE);
+
+ gpio_init_struct.Mode = GPIO_MODE_OUTPUT_OD;
+ gpio_init_struct.Pull = GPIO_NOPULL;
+ gpio_init_struct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
+
+ gpio_init_struct.Pin = i2c->scl_pin;
+ HAL_GPIO_Init(i2c->scl_port, &gpio_init_struct);
+
+ gpio_init_struct.Pin = i2c->sda_pin;
+ HAL_GPIO_Init(i2c->sda_port, &gpio_init_struct);
+
+ for(uint8_t i = 0; i < num_clocks; i++){
+ // 3. Check SCL and SDA High level in GPIOx_IDR.
+ if(send_stop_bits){
+ HAL_GPIO_WritePin(i2c->sda_port, i2c->sda_pin, GPIO_PIN_SET);
+ }
+ HAL_GPIO_WritePin(i2c->scl_port, i2c->scl_pin, GPIO_PIN_SET);
+
+ wait_for_gpio_state_timeout(i2c->scl_port, i2c->scl_pin, GPIO_PIN_SET, timeout);
+ if(send_stop_bits){
+ wait_for_gpio_state_timeout(i2c->sda_port, i2c->sda_pin, GPIO_PIN_SET, timeout);
+ }
+
+ // 4. Configure the SDA I/O as General Purpose Output Open-Drain, Low level (Write 0 to GPIOx_ODR).
+ if(send_stop_bits){
+ HAL_GPIO_WritePin(i2c->sda_port, i2c->sda_pin, GPIO_PIN_RESET);
+ wait_for_gpio_state_timeout(i2c->sda_port, i2c->sda_pin, GPIO_PIN_RESET, timeout); // 5. Check SDA Low level in GPIOx_IDR
+ }
+
+ // 6. Configure the SCL I/O as General Purpose Output Open-Drain, Low level (Write 0 to GPIOx_ODR).
+ HAL_GPIO_WritePin(i2c->scl_port, i2c->scl_pin, GPIO_PIN_RESET);
+ wait_for_gpio_state_timeout(i2c->scl_port, i2c->scl_pin, GPIO_PIN_RESET, timeout); // 7. Check SCL Low level in GPIOx_IDR.
+
+ // 8. Configure the SCL I/O as General Purpose Output Open-Drain, High level (Write 1 to GPIOx_ODR).
+ HAL_GPIO_WritePin(i2c->scl_port, i2c->scl_pin, GPIO_PIN_SET);
+ wait_for_gpio_state_timeout(i2c->scl_port, i2c->scl_pin, GPIO_PIN_SET, timeout); // 9. Check SCL High level in GPIOx_IDR.
+
+ // 10. Configure the SDA I/O as General Purpose Output Open-Drain , High level (Write 1 to GPIOx_ODR).
+ if(send_stop_bits){
+ HAL_GPIO_WritePin(i2c->sda_port, i2c->sda_pin, GPIO_PIN_SET);
+ wait_for_gpio_state_timeout(i2c->sda_port, i2c->sda_pin, GPIO_PIN_SET, timeout); // 11. Check SDA High level in GPIOx_IDR.
+ }
+ }
+
+ // 12. Configure the SCL and SDA I/Os as Alternate function Open-Drain.
+ gpio_init_struct.Mode = GPIO_MODE_AF_OD;
+ gpio_init_struct.Alternate = GPIO_AF4_I2C1;
+
+ gpio_init_struct.Pin = i2c->scl_pin;
+ HAL_GPIO_Init(i2c->scl_port, &gpio_init_struct);
+
+ gpio_init_struct.Pin = i2c->sda_pin;
+ HAL_GPIO_Init(i2c->sda_port, &gpio_init_struct);
+
+ // 13. Set SWRST bit in I2Cx_CR1 register.
+ SET_BIT(handler->Instance->CR1, I2C_CR1_SWRST);
+ asm("nop");
+
+ /* 14. Clear SWRST bit in I2Cx_CR1 register. */
+ CLEAR_BIT(handler->Instance->CR1, I2C_CR1_SWRST);
+ asm("nop");
+
+ /* 15. Enable the I2C peripheral by setting the PE bit in I2Cx_CR1 register */
+ SET_BIT(handler->Instance->CR1, I2C_CR1_PE);
+ asm("nop");
+
+ HAL_I2C_Init(handler);
+}
+
+// --------------------------- END OF WORKAROUND FUNCTIONS -------------------------------------------------
diff --git a/soccer_firmware/Core/Src/dynamixel_p2.c b/soccer_firmware/Core/Src/dynamixel_p2.c
index 9608db58d..25e638d88 100644
--- a/soccer_firmware/Core/Src/dynamixel_p2.c
+++ b/soccer_firmware/Core/Src/dynamixel_p2.c
@@ -19,6 +19,19 @@ void write_goal_position_p2(MotorPort *port, uint8_t id, uint16_t angle) {
_motor_write_p2(port, id, addr, data, dataLen);
}
+void write_min_position_limit_p2(MotorPort *port, uint8_t id, uint32_t limit) {
+ uint8_t data[4] = {limit & 0xff, (limit>>8) & 0xff, 0, 0};
+ uint16_t dataLen = 4;
+ uint16_t addr = 48;
+ _motor_write_p2(port, id, addr, data, dataLen);
+}
+
+void write_max_position_limit_p2(MotorPort *port, uint8_t id, uint32_t limit) {
+ uint8_t data[4] = {limit & 0xff, (limit>>8) & 0xff, 0, 0};
+ uint16_t dataLen = 4;
+ uint16_t addr = 52;
+ _motor_write_p2(port, id, addr, data, dataLen);
+}
/*
* Dynamixel 2.0
*/
diff --git a/soccer_firmware/Core/Src/main.c b/soccer_firmware/Core/Src/main.c
index 7bd46a852..d27b60318 100644
--- a/soccer_firmware/Core/Src/main.c
+++ b/soccer_firmware/Core/Src/main.c
@@ -18,17 +18,11 @@
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
-
-#include "bringup_tests.h"
-#include "dynamixel_p1.h"
-#include "dynamixel_p2.h"
-#include "MPU6050.h"
-#include "update_loop.h"
#include "usb_device.h"
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
-
+#include "BMI088.h"
/* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/
@@ -97,6 +91,7 @@ static void MX_I2C1_Init(void);
/* USER CODE BEGIN 0 */
// test various functionalities
void init_ports() {
+ // port1 => UART1 ==> J2 on new PCB
port1 = (MotorPort){
.huart = &huart1,
.hdma_uart_tx = &hdma_usart1_tx,
@@ -105,11 +100,12 @@ void init_ports() {
.dirPinNum = USART1_DIR_Pin,
.dmaDoneReading = false,
.currMotor = 0,
- .numMotors = 2,
- .motorIds = {16, 17},
- .protocol = {1, 1}
+ .numMotors = 3,
+ .motorIds = {10, 11, 12}, //{16, 17},
+ .protocol = {2, 2, 2} //{1, 1}
};
+ // port2 => UART2 ==> J7 on new PCB
port2 = (MotorPort){
.huart = &huart2,
.hdma_uart_tx = &hdma_usart2_tx,
@@ -118,11 +114,12 @@ void init_ports() {
.dirPinNum = USART2_DIR_Pin,
.dmaDoneReading = false,
.currMotor = 0,
- .numMotors = 4,
- .motorIds = {0, 1, 2, 3},
- .protocol = {2, 2, 2, 2}
+ .numMotors = 3,
+ .motorIds = {10, 11, 12},//{0, 1, 2, 3},
+ .protocol = {2, 2, 2}
};
+ // port3 => UART3 ==> J12 on new PCB
port3 = (MotorPort){
.huart = &huart3,
.hdma_uart_tx = &hdma_usart3_tx,
@@ -131,10 +128,11 @@ void init_ports() {
.dirPinNum = USART3_DIR_Pin,
.dmaDoneReading = false,
.numMotors = 3,
- .motorIds = {13, 14, 15},
- .protocol = {1, 1, 1}
+ .motorIds = {10, 11, 12},//{13, 14, 15},
+ .protocol = {2, 2, 2}//{1, 1, 1}
};
+ // port4 => UART4 ==> J3 on new PCB
port4 = (MotorPort){
.huart = &huart4,
.hdma_uart_tx = &hdma_uart4_tx,
@@ -143,10 +141,11 @@ void init_ports() {
.dirPinNum = USART4_DIR_Pin,
.dmaDoneReading = false,
.numMotors = 3,
- .motorIds = {7, 8, 9},
- .protocol = {1, 1, 1}
+ .motorIds = {10, 11, 12},//7, 8, 9},
+ .protocol = {2, 2, 2} //{1, 1, 1}
};
+ // port5 => UART5 ==> J6 on new PCB
port5 = (MotorPort){
.huart = &huart5,
.hdma_uart_tx = &hdma_uart5_tx,
@@ -155,10 +154,11 @@ void init_ports() {
.dirPinNum = USART5_DIR_Pin,
.dmaDoneReading = false,
.numMotors = 3,
- .motorIds = {4, 5, 6},
- .protocol = {2, 2, 2}
+ .motorIds = {10, 11, 12}, //{4, 5, 6},
+ .protocol = {2,2,2}//{2, 2, 2}
};
+ // port6 => UART6 ==> J11 on new PCB
port6 = (MotorPort){
.huart = &huart6,
.hdma_uart_tx = &hdma_usart6_tx,
@@ -167,8 +167,8 @@ void init_ports() {
.dirPinNum = USART6_DIR_Pin,
.dmaDoneReading = false,
.numMotors = 3,
- .motorIds = {10, 11, 12},
- .protocol = {2, 2, 2}
+ .motorIds = {10, 11, 12}, //{10, 11, 12},
+ .protocol = {2, 2, 2} //{2, 2, 2}
};
motorPorts[0] = &port1;
@@ -249,22 +249,64 @@ int main(void)
MX_I2C1_Init();
/* USER CODE BEGIN 2 */
// give some time for motors to power on and initialize, before we try to talk to them
- HAL_Delay(1000);
+ HAL_Delay(5000);
init_ports();
- MPU6050_init();
+// MPU6050_init();
init_motors();
+ BMI088 imu;
+ BMI088_Init(&imu, &hi2c1);
+
+// uint16_t angle = 0;
+// uint16_t angle_lo = 0;
+// uint16_t angle_hi = 0;
+
/* USER CODE END 2 */
/* Infinite loop */
/* USER CODE BEGIN WHILE */
while (1)
{
- update();
- // test Dynamixel 2.0
-// test_led_p2(&port2, 0x6);
-// test_motor_sweep2(&port2);
+
+ update();
+
+// angle += 1;
+// angle %= 0x3FF;
+// angle_lo = angle & 0xFF;
+// angle_hi = (angle >> 8) & 0xFF;
+//
+// write_goal_position_p2(motorPorts[4], 1, angle);
+// write_goal_position_p2(motorPorts[4], 32, angle);
+// write_goal_position_p2(motorPorts[4], 27, angle);
+//
+// write_goal_position_p2(motorPorts[3], 3, angle);
+// write_goal_position_p2(motorPorts[3], 4, angle); // range 212 - 122
+// write_goal_position_p2(motorPorts[3], 10, angle);
+//
+// write_goal_position_p2(motorPorts[2], 1, angle);
+// write_goal_position_p2(motorPorts[2], 22, angle);
+// write_goal_position_p2(motorPorts[2], 21, angle);
+//
+// write_goal_position_p2(motorPorts[1], 26, angle);
+// write_goal_position_p2(motorPorts[1], 31, angle);
+// write_goal_position_p2(motorPorts[1], 30, angle);
+//
+// write_goal_position_p2(motorPorts[0], 23, angle);
+// write_goal_position_p2(motorPorts[0], 24, angle);
+// write_goal_position_p2(motorPorts[0], 28, angle);
+// write_goal_position_p2(motorPorts[0], 29, angle);
+// write_goal_position_p2(motorPorts[0], 34, angle);
+// write_goal_position_p2(motorPorts[0], 33, angle);
+
+
+// // test Dynamixel 2.0
+// test_led_p2(&port6, 0x1);
+// test_motor_sweep2(&port6);
+// HAL_Delay(9000);
+
+// test_led_p2(&port4, 0x1);
+// test_motor_sweep2(&port4);
// test_ping2(&port2);
// motor_torque_en_p2(&port2, 0xfe, 0);
// update_baud_rate_p2(&port2, 0x1, 3);
@@ -286,8 +328,8 @@ int main(void)
// test Dynamixel 1.0
// test_motor_sweep1(&port2, 13);
// _motor_ping_p1(&port6, 13);
- HAL_GPIO_TogglePin(GPIOA, GREEN_LED_Pin);
- HAL_Delay(100);
+// HAL_GPIO_TogglePin(GPIOA, GREEN_LED_Pin);
+// HAL_Delay(50);
/* USER CODE END WHILE */
/* USER CODE BEGIN 3 */
diff --git a/soccer_firmware/Core/Src/update_loop.c b/soccer_firmware/Core/Src/update_loop.c
index 6d6fe43d9..79469c03d 100644
--- a/soccer_firmware/Core/Src/update_loop.c
+++ b/soccer_firmware/Core/Src/update_loop.c
@@ -11,8 +11,11 @@
#include "main.h"
#include "usbd_cdc_if.h"
#include "MPU6050.h"
+#include "BMI088.h"
-#define UPDATE_PERIOD 3 // milliseconds
+#define UPDATE_PERIOD 1 // milliseconds
+
+BMI088 imu;
void update()
{
@@ -21,18 +24,19 @@ void update()
uint32_t lastTime = HAL_GetTick();
while(1)
{
- if(HAL_GetTick() - lastTime > UPDATE_PERIOD) // send IMU/ANGLES periodically back to main computer
- {
- lastTime = HAL_GetTick();
+ if(HAL_GetTick() - lastTime > UPDATE_PERIOD) // send IMU/ANGLES periodically back to main computer
+ {
+ lastTime = HAL_GetTick();
- // we read want to read 1 motor position from each port
- read_motors(txBuf);
+ // we read want to read 1 motor position from each port
+ read_motors(txBuf);
- // get current linear Acceleration and Rotational speed
- read_imu(txBuf);
+ // get current linear Acceleration and Rotational speed
+ read_imu(txBuf);
- CDC_Transmit_FS((uint8_t *) txBuf, sizeof(txBuf));
- }
+ CDC_Transmit_FS((uint8_t *) txBuf, sizeof(txBuf));
+ HAL_GPIO_TogglePin(GPIOA, GREEN_LED_Pin);
+ }
if (usb_received) // when we receive USB packet, service it right away
{
@@ -45,12 +49,57 @@ void update()
command_motors();
usb_received = false;
- HAL_GPIO_TogglePin(GPIOA, GREEN_LED_Pin);
+// HAL_GPIO_TogglePin(GPIOA, GREEN_LED_Pin);
}
}
}
+void set_robot_motor_limits() {
+ // position limits 0 - 4095 (1 rotation)
+ // min position address => 48
+ // max position address => 52
+
+ // use index as motorID!!
+ // 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19
+ uint32_t minLimits[20] = {1510, 1050, 2000, 2000, 1213, 1536, 655, 760, 1610, 1825, 1100, 1550, 1720, 2057, 920, 1870, 0, 0, 0, 1060};
+ uint32_t maxLimits[20] = {2400, 3100, 2945, 4096, 3155, 2569, 2373, 2070, 3160, 2260, 3060, 2550, 3560, 3300, 2580, 2245, 0, 0, 3000, 4096};
+
+ for (uint16_t i = 0; i < 6; i++) {// reset variables
+ motorPorts[i]->currMotor = 0;
+ }
+
+ while(1)
+ {
+ bool doneWithAllMotors = true;
+ for (uint16_t i = 0; i < 6; i++)
+ {
+ uint8_t idx = motorPorts[i]->currMotor;
+ uint8_t motorId = motorPorts[i]->motorIds[idx];
+// uint8_t protocol = motorPorts[i]->protocol[idx];
+
+ if (idx >= motorPorts[i]->numMotors){ // skip port if all motors already serviced
+ continue;
+ } else {
+ doneWithAllMotors = false;
+ }
+
+ // angle format depends on how Python script. Bit wise OR
+// uint16_t angle = usbRxBuffer[2 + motorId * 2] | (usbRxBuffer[2 + motorId * 2 + 1] << 8);
+
+ write_min_position_limit_p2(motorPorts[i], motorId, minLimits[motorId]);
+ write_max_position_limit_p2(motorPorts[i], motorId, maxLimits[motorId]);
+
+ motorPorts[i]->currMotor = idx + 1;
+ }
+
+ HAL_Delay(1); // delay enough for motors to have time to respond
+
+ if(doneWithAllMotors) return; // all motors serviced, peace out
+ }
+
+}
+
/*
* Send commands in parallel on all ports to reduce latency
* Keep sending angles until all motors have been commanded
@@ -75,7 +124,8 @@ void command_motors() {
doneWithAllMotors = false;
}
- // angle format depends on how Python script
+
+ // angle format depends on how Python script. Bit wise OR
uint16_t angle = usbRxBuffer[2 + motorId * 2] | (usbRxBuffer[2 + motorId * 2 + 1] << 8);
if(protocol == 1) {
@@ -101,13 +151,33 @@ void read_imu(uint8_t *rxBuf) {
uint8_t accBuff[6] = {0};
Read_Accelerometer_IT(accBuff);
- for(uint8_t i = 0; i < 6; i++) {
- rxBuf[2 + 18 * 2 + i] = accBuff[i];
- }
+ int16_t gyroX = 0;
+ int16_t gyroY = 0;
+ int16_t gyroZ = 0;
+ int16_t accX = 0;
+ int16_t accY = 0;
+ int16_t accZ = 0;
+
+
+ BMI088_ReadAccelerometer(&imu, &hi2c1, &accX, &accY, &accZ);
+
+ // Read gyro after accel, otherwise we get HAL_error (not sure why :/)
+ BMI088_ReadGyroscope(&hi2c1, &gyroX, &gyroY, &gyroZ);
+
+ rxBuf[2 + 18 * 2 + 0] = (accX >> 8) & 0xFF; // MSB first means big endian
+ rxBuf[2 + 18 * 2 + 1] = accX & 0xFF;
+ rxBuf[2 + 18 * 2 + 2] = (accY >> 8) & 0xFF;
+ rxBuf[2 + 18 * 2 + 3] = accY & 0xFF;
+ rxBuf[2 + 18 * 2 + 4] = (accZ >> 8) & 0xFF;
+ rxBuf[2 + 18 * 2 + 5] = accZ & 0xFF;
+
+ rxBuf[2 + 18 * 2 + 6 + 0] = (gyroX >> 8) & 0xFF;
+ rxBuf[2 + 18 * 2 + 6 + 1] = gyroX & 0xFF;
+ rxBuf[2 + 18 * 2 + 6 + 2] = (gyroY >> 8) & 0xFF;
+ rxBuf[2 + 18 * 2 + 6 + 3] = gyroY & 0xFF;
+ rxBuf[2 + 18 * 2 + 6 + 4] = (gyroZ >> 8) & 0xFF;
+ rxBuf[2 + 18 * 2 + 6 + 5] = gyroZ & 0xFF;
- for(uint8_t i = 0; i < 6; i++) {
- rxBuf[2 + 18 * 2 + 6 + i] = gyrBuff[i];
- }
}
void read_motors(uint8_t *rxBuf) {
diff --git a/soccer_firmware/soccer_firmware Debug.launch b/soccer_firmware/soccer_firmware Debug.launch
index 82b4070be..cfb474dfd 100644
--- a/soccer_firmware/soccer_firmware Debug.launch
+++ b/soccer_firmware/soccer_firmware Debug.launch
@@ -29,7 +29,7 @@
-
+
@@ -66,7 +66,7 @@
-
+
diff --git a/soccer_firmware/soccer_firmware.ioc b/soccer_firmware/soccer_firmware.ioc
index 3c11c306b..0e88dc842 100644
--- a/soccer_firmware/soccer_firmware.ioc
+++ b/soccer_firmware/soccer_firmware.ioc
@@ -137,6 +137,8 @@ Dma.USART6_TX.11.Priority=DMA_PRIORITY_HIGH
Dma.USART6_TX.11.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode
File.Version=6
GPIO.groupedBy=Group By Peripherals
+I2C1.ClockSpeed=100000
+I2C1.IPParameters=ClockSpeed
KeepUserPlacement=false
Mcu.CPN=STM32F446RCT6
Mcu.Family=STM32F4
@@ -286,7 +288,7 @@ PH0-OSC_IN.Mode=HSE-External-Oscillator
PH0-OSC_IN.Signal=RCC_OSC_IN
PH1-OSC_OUT.Mode=HSE-External-Oscillator
PH1-OSC_OUT.Signal=RCC_OSC_OUT
-PinOutPanel.RotationAngle=0
+PinOutPanel.RotationAngle=-90
ProjectManager.AskForMigrate=true
ProjectManager.BackupPrevious=false
ProjectManager.CompilerOptimize=6
diff --git a/soccer_hardware/soccer_firmware_interface/config/bez2.yaml b/soccer_hardware/soccer_firmware_interface/config/bez2.yaml
index 50f63713b..3eab3e579 100644
--- a/soccer_hardware/soccer_firmware_interface/config/bez2.yaml
+++ b/soccer_hardware/soccer_firmware_interface/config/bez2.yaml
@@ -4,7 +4,6 @@ left_leg_motor_0:
angle_zero: 180
angle_max: 360
angle_min: 0
- initial_state: 0.0
left_leg_motor_1:
id: 5
type: "xm430"
@@ -12,7 +11,6 @@ left_leg_motor_1:
angle_max: 360
angle_min: 0
flipped: "true"
- initial_state: -0.05
left_leg_motor_2:
id: 6
type: "xm430"
@@ -20,21 +18,18 @@ left_leg_motor_2:
angle_max: 360
angle_min: 0
flipped: "true"
- initial_state: 0.25
left_leg_motor_3:
id: 7
type: "mx64"
angle_zero: 180
angle_max: 360
angle_min: 0
- initial_state: -0.4
left_leg_motor_4:
id: 8
type: "mx64"
angle_zero: 180
angle_max: 360
angle_min: 0
- initial_state: 0.2
left_leg_motor_5:
id: 9
type: "mx64"
@@ -42,7 +37,6 @@ left_leg_motor_5:
angle_max: 360
angle_min: 0
flipped: "true"
- initial_state: 0.0
right_leg_motor_0:
id: 10
type: "xm430"
@@ -50,21 +44,18 @@ right_leg_motor_0:
angle_max: 360
angle_min: 0
flipped: "true"
- initial_state: 0.0
right_leg_motor_1:
id: 11
type: "xm430"
angle_zero: 180
angle_max: 360
angle_min: 0
- initial_state: -0.05
right_leg_motor_2:
id: 12
type: "xm430"
angle_zero: 180
angle_max: 360
angle_min: 0
- initial_state: 0.25
right_leg_motor_3:
id: 13
type: "mx64"
@@ -72,14 +63,12 @@ right_leg_motor_3:
angle_max: 360
angle_min: 0
flipped: "true"
- initial_state: -0.4
right_leg_motor_4:
id: 14
type: "mx64"
angle_zero: 180
angle_max: 360
angle_min: 0
- initial_state: 0.2
flipped: "true"
right_leg_motor_5:
id: 15
@@ -87,49 +76,55 @@ right_leg_motor_5:
angle_zero: 180
angle_max: 360
angle_min: 0
- initial_state: 0.0
left_arm_motor_0:
- id: 2
- type: "xm430"
+ id: 0
+ type: "mx28"
angle_zero: 180
angle_max: 360
angle_min: 0
flipped: "true"
- initial_state: 0.2
left_arm_motor_1:
- id: 3
- type: "xm430"
+ id: 1
+ type: "mx28"
+ angle_zero: 180
+ angle_max: 360
+ angle_min: 0
+left_arm_motor_2:
+ id: 18
+ type: "mx28"
angle_zero: 180
angle_max: 360
angle_min: 0
- initial_state: 1.5
right_arm_motor_0: # Checked
- id: 0
- type: "xm430"
+ id: 19
+ type: "mx28"
angle_zero: 180
angle_max: 360
angle_min: 0
- initial_state: 0.2
right_arm_motor_1:
- id: 1
- type: "xm430"
+ id: 2
+ type: "mx28"
angle_zero: 180
angle_max: 360
angle_min: 0
flipped: "true"
- initial_state: 1.5
+ right_arm_motor_2:
+ id: 3
+ type: "mx28"
+ angle_zero: 180
+ angle_max: 360
+ angle_min: 0
+ flipped: "true"
head_motor_0:
id: 16
- type: "ax12"
+ type: "xl430"
angle_zero: 150
angle_max: 300
angle_min: 0
flipped: "true"
- initial_state: 0.0
head_motor_1:
id: 17
- type: "ax12"
+ type: "xl430"
angle_zero: 150
angle_max: 300
angle_min: 0
- initial_state: 0.0
diff --git a/soccer_hardware/soccer_firmware_interface/src/soccer_firmware_interface/firmware_interface.py b/soccer_hardware/soccer_firmware_interface/src/soccer_firmware_interface/firmware_interface.py
index 9a3f988f7..a30cd913d 100644
--- a/soccer_hardware/soccer_firmware_interface/src/soccer_firmware_interface/firmware_interface.py
+++ b/soccer_hardware/soccer_firmware_interface/src/soccer_firmware_interface/firmware_interface.py
@@ -34,21 +34,20 @@ def __init__(self):
self.last_motor_publish_time = rospy.Time.now()
self.last_motor_publish_time_real = rospy.Time.now()
- self._IMU_FILT_B = np.array(
- [0.030738841, 0.048424201, 0.083829062, 0.11125669, 0.13424691, 0.14013315, 0.13424691, 0.11125669, 0.083829062, 0.048424201, 0.030738841]
- ) # from embedded code
- self._imu_filt_zi = np.zeros((len(self._IMU_FILT_B) - 1, 3))
+ # self._IMU_FILT_B = np.array(
+ # [0.030738841, 0.048424201, 0.083829062, 0.11125669, 0.13424691, 0.14013315, 0.13424691, 0.11125669, 0.083829062, 0.048424201, 0.030738841]
+ # ) # from embedded code
+ # self._imu_filt_zi = np.zeros((len(self._IMU_FILT_B) - 1, 3))
# Start the thread
serial_thread = threading.Thread(target=self.firmware_update_loop)
serial_thread.start()
- pass
-
def reconnect_serial_port(self):
if self.serial is None:
# todo: loop through ACMs see which one connects
- for i in range(10):
+ # for debug assume ACM0 is STLINK and ACM1 is our PCB
+ for i in range(0, 10):
rospy.loginfo_throttle(10, f"Trying connection to /dev/ttyACM{i}")
if os.path.exists(f"/dev/ttyACM{i}"):
self.serial = serial.Serial(f"/dev/ttyACM{i}")
@@ -113,32 +112,32 @@ def firmware_update_loop(self):
imu_data = data[2 + 2 * 18 : 2 + 2 * 18 + 12]
- ACC_RANGE = 16384.0
- IMU_GY_RANGE = 131
+ # https://www.mouser.com/datasheet/2/783/BST_BMI088_DS001-1509549.pdf
+ ACC_RANGE = 32768.0 / 2.0 / 1.5 # page 27 datasheet bmi088
+ IMU_GY_RANGE = 32768.0 / 1000.0 # page 39 datasheet bmi088
+ # ACC_RANGE = 32768.0 / 2.0 # page 22 datasheet bmi085
+ # IMU_GY_RANGE = 32768.0 / 1000.0 # page 27 datasheet bmi085
G = 9.81
- ax = int.from_bytes(imu_data[0:2], byteorder="big", signed=True)
- ay = int.from_bytes(imu_data[2:4], byteorder="big", signed=True)
- az = int.from_bytes(imu_data[4:6], byteorder="big", signed=True)
+ ax = int.from_bytes(imu_data[0:2], byteorder="big", signed=True) / ACC_RANGE * G
+ ay = int.from_bytes(imu_data[2:4], byteorder="big", signed=True) / ACC_RANGE * G
+ az = int.from_bytes(imu_data[4:6], byteorder="big", signed=True) / ACC_RANGE * G
- imu.linear_acceleration.x = -(ax) * G / ACC_RANGE
- imu.linear_acceleration.y = -(ay) * G / ACC_RANGE
- imu.linear_acceleration.z = -(az) * G / ACC_RANGE
+ imu.linear_acceleration.x = ax
+ imu.linear_acceleration.y = ay
+ imu.linear_acceleration.z = az
vx = int.from_bytes(imu_data[6:8], byteorder="big", signed=True) / IMU_GY_RANGE
vy = int.from_bytes(imu_data[8:10], byteorder="big", signed=True) / IMU_GY_RANGE
vz = int.from_bytes(imu_data[10:12], byteorder="big", signed=True) / IMU_GY_RANGE
- # implement low-pass filter from embedded system
- v_filt, self._imu_filt_zi = scipy.signal.lfilter(
- self._IMU_FILT_B, [1], [[vx, vy, vz]], axis=0, zi=self._imu_filt_zi
- ) # note: expected to be shape (1, 3), double nested list is intentional
- imu.angular_velocity = Vector3(*v_filt[0])
+ print(f"a {ax} {ay} {az}")
+ print(f"v {vx:10.3f} {vy:10.3f} {vz:10.3f}")
+
+ imu.angular_velocity.x = vx
+ imu.angular_velocity.y = vy
+ imu.angular_velocity.z = vz
- # imu.orientation.x = 0
- # imu.orientation.y = 0
- # imu.orientation.z = 0
- # imu.orientation.w = 0
self.imu_publisher.publish(imu)
except Exception as ex:
@@ -147,19 +146,18 @@ def firmware_update_loop(self):
def joint_command_callback(self, joint_state: JointState):
try:
-
- if self.last_motor_publish_time is not None:
- time_diff_message_in = joint_state.header.stamp - self.last_motor_publish_time
- time_diff_real = rospy.Time.now() - self.last_motor_publish_time_real
-
- time_lag = rospy.Time.now() - joint_state.header.stamp
- if time_lag > time_diff_message_in:
- print(f"Message Skipped Time Lag {time_lag}")
- self.last_motor_publish_time = joint_state.header.stamp
- return
-
- if time_diff_real < time_diff_message_in:
- rospy.sleep(time_diff_message_in - time_diff_real)
+ # if self.last_motor_publish_time is not None:
+ # time_diff_message_in = joint_state.header.stamp - self.last_motor_publish_time
+ # time_diff_real = rospy.Time.now() - self.last_motor_publish_time_real
+ #
+ # time_lag = rospy.Time.now() - joint_state.header.stamp
+ # if time_lag > time_diff_message_in:
+ # print(f"Message Skipped Time Lag {time_lag}")
+ # self.last_motor_publish_time = joint_state.header.stamp
+ # return
+ #
+ # if time_diff_real < time_diff_message_in:
+ # rospy.sleep(time_diff_message_in - time_diff_real)
t1 = time.time()
diff --git a/soccer_hardware/soccer_firmware_interface/src/soccer_firmware_interface/test_firmware_interface.py b/soccer_hardware/soccer_firmware_interface/src/soccer_firmware_interface/test_firmware_interface.py
index 6349f3dae..64698dd19 100644
--- a/soccer_hardware/soccer_firmware_interface/src/soccer_firmware_interface/test_firmware_interface.py
+++ b/soccer_hardware/soccer_firmware_interface/src/soccer_firmware_interface/test_firmware_interface.py
@@ -26,29 +26,31 @@ def test_send_command():
angle %= 0x3FF
angle_lo = angle & 0xFF
angle_hi = (angle >> 8) & 0xFF
- # print('angle:', angle)
- s.write([0xFF, 0xFF, angle_lo, angle_hi, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18])
-
- for i in range(20):
- res = s.read()
- print((res))
+ print("angle:", angle)
+ goalPositionsAllMotors = []
+ goalPositionsAllMotors = [0xFF, 0xFF]
+ for motorID in range(30):
+ goalPositionsAllMotors.append(angle_lo)
+ goalPositionsAllMotors.append(angle_hi)
+
+ # 2 header + 18 in the array below
+ # header header motorID#0 motorID#1 motorID#2 motorID#3 motorID#4 motorID#5 motorID#6 motorID#7
+ # Index: [0] [1] [2] [3] [4] [5] [6] [7] [8] [9] [10] [11] [12] [13] [14] [15] [16] [17]
+ # s.write([0xFF, 0xFF, angle_lo, angle_hi, angle_lo, angle_hi, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18])
+ result = s.write(goalPositionsAllMotors)
+ time.sleep(0.01)
+ # for i in range(20):
+ # res = s.read()
+ # print((res))
count_loop += 1
print(count_loop / (time.time() - t_start))
def test_firmware_interface():
- rospy.init_node("test")
-
- importlib.reload(logging)
-
- with open((os.path.dirname(os.path.abspath(__file__)) + "/../../config/motor_types.yaml")) as f:
- param_info = yaml.safe_load(f)
- rosparam.upload_params("motor_types", param_info)
-
- with open((os.path.dirname(os.path.abspath(__file__)) + "/../../config/bez2.yaml")) as f:
- param_info = yaml.safe_load(f)
- rosparam.upload_params("motor_mapping", param_info)
+ rospy.init_node("firmware_interface")
+ rospy.set_param("motor_mapping", os.path.dirname(os.path.realpath(__file__)) + "/../../config/bez2.yaml")
+ rospy.set_param("motor_types", os.path.dirname(os.path.realpath(__file__)) + "/../../config/motor_types.yaml")
f = FirmwareInterface()
@@ -75,14 +77,14 @@ def test_firmware_interface():
"right_leg_motor_5",
]
# j.position = [math.sin(i / 180 * math.pi) * 0.1, math.cos(i / 180 * math.pi) * 0.1]
- # ang = math.sin(i / 180 * math.pi) * 0.2
- ang = 0.0
+ ang = math.sin(i / 30 * math.pi) * 0.2
+ # ang = 0.0
j.position = [ang] * 18
j.header.stamp = rospy.Time.now()
f.joint_command_callback(j)
- # time.sleep(0.01)
+ time.sleep(0.01)
pass