From 4c38bfe344bf4d6731d70cba9be24c1820439ea5 Mon Sep 17 00:00:00 2001
From: Nam Nguyen <lkunelk@gmail.com>
Date: Mon, 15 Jul 2024 07:38:39 -0400
Subject: [PATCH] Fix imu code (#842)

* revert build configs

* imu working

* add back try catch block
---
 soccer_firmware/.cproject                     |  202 +++-
 soccer_firmware/.project                      |    1 +
 .../.settings/language.settings.xml           |   14 +-
 soccer_firmware/Core/Inc/stm32f4xx_hal_conf.h |    8 +-
 soccer_firmware/Core/Src/dynamixel_p1.c       |    1 -
 soccer_firmware/Core/Src/dynamixel_p2.c       |    2 +-
 soccer_firmware/Core/Src/main.c               |    2 +-
 soccer_firmware/Core/Src/stm32f4xx_hal_msp.c  |    1 -
 soccer_firmware/Core/Src/update_loop.c        |    8 -
 .../Core/Startup/startup_stm32f446rctx.s      | 1070 ++++++++---------
 soccer_firmware/STM32F446RCTX_FLASH.ld        |    6 +-
 soccer_firmware/STM32F446RCTX_RAM.ld          |    6 +-
 soccer_firmware/soccer_firmware Debug.launch  |  142 +--
 soccer_firmware/soccer_firmware.ioc           |    9 +-
 .../config/bez2.yaml                          |   33 +-
 .../config/motor_types.yaml                   |    3 +
 .../firmware_interface.py                     |    1 +
 17 files changed, 858 insertions(+), 651 deletions(-)

diff --git a/soccer_firmware/.cproject b/soccer_firmware/.cproject
index c8fb513aa..d341b2e02 100644
--- a/soccer_firmware/.cproject
+++ b/soccer_firmware/.cproject
@@ -76,7 +76,12 @@
 								</option>
 								<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.input.cpp.402515729" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.input.cpp"/>
 							</tool>
-							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.1333709486" name="MCU GCC Linker" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker"/>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.1333709486" name="MCU GCC Linker" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker">
+								<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.input.33682271" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.input">
+									<additionalInput kind="additionalinputdependency" paths="$(USER_OBJS)"/>
+									<additionalInput kind="additionalinput" paths="$(LIBS)"/>
+								</inputType>
+							</tool>
 							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.469078976" name="MCU G++ Linker" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker">
 								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.option.script.1447570877" name="Linker Script (-T)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.option.script" value="${workspace_loc:/${ProjName}/STM32F446RCTX_FLASH.ld}" valueType="string"/>
 								<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.input.1451308528" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.input">
@@ -174,7 +179,12 @@
 								</option>
 								<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.input.cpp.370155548" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.input.cpp"/>
 							</tool>
-							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.944698730" name="MCU GCC Linker" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker"/>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.944698730" name="MCU GCC Linker" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker">
+								<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.input.1360941167" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.input">
+									<additionalInput kind="additionalinputdependency" paths="$(USER_OBJS)"/>
+									<additionalInput kind="additionalinput" paths="$(LIBS)"/>
+								</inputType>
+							</tool>
 							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.281455922" name="MCU G++ Linker" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker">
 								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.option.script.1297975947" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.option.script" value="${workspace_loc:/${ProjName}/STM32F446RCTX_FLASH.ld}" valueType="string"/>
 								<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.input.763717818" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.input">
@@ -202,6 +212,170 @@
 			</storageModule>
 			<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
 		</cconfiguration>
+		<cconfiguration id="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug.1206544251">
+			<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug.1206544251" moduleId="org.eclipse.cdt.core.settings" name="Debug">
+				<externalSettings/>
+				<extensions>
+					<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
+					<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+				</extensions>
+			</storageModule>
+			<storageModule moduleId="cdtBuildSystem" version="4.0.0">
+				<configuration artifactExtension="elf" artifactName="${ProjName}" buildArtefactType="org.eclipse.cdt.build.core.buildArtefactType.exe" buildProperties="org.eclipse.cdt.build.core.buildArtefactType=org.eclipse.cdt.build.core.buildArtefactType.exe,org.eclipse.cdt.build.core.buildType=org.eclipse.cdt.build.core.buildType.debug" cleanCommand="rm -rf" description="" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug.1206544251" name="Debug" parent="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug">
+					<folderInfo id="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug.1206544251." name="/" resourcePath="">
+						<toolChain id="com.st.stm32cube.ide.mcu.gnu.managedbuild.toolchain.exe.debug.1504879823" name="MCU ARM GCC" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.toolchain.exe.debug">
+							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_mcu.1507568330" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_mcu" useByScannerDiscovery="true" value="STM32F446RCTx" valueType="string"/>
+							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_cpuid.1150861636" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_cpuid" useByScannerDiscovery="false" value="0" valueType="string"/>
+							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_coreid.811814404" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_coreid" useByScannerDiscovery="false" value="0" valueType="string"/>
+							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.fpu.1094368107" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.fpu" useByScannerDiscovery="true" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.fpu.value.fpv4-sp-d16" valueType="enumerated"/>
+							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.floatabi.45529993" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.floatabi" useByScannerDiscovery="true" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.floatabi.value.hard" valueType="enumerated"/>
+							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_board.1261510242" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_board" useByScannerDiscovery="false" value="genericBoard" valueType="string"/>
+							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.defaults.1429059007" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.defaults" useByScannerDiscovery="false" value="com.st.stm32cube.ide.common.services.build.inputs.revA.1.0.6 || Debug || true || Executable || com.st.stm32cube.ide.mcu.gnu.managedbuild.option.toolchain.value.workspace || STM32F446RCTx || 0 || 0 || arm-none-eabi- || ${gnu_tools_for_stm32_compiler_path} || ../USB_DEVICE/App | ../USB_DEVICE/Target | ../Core/Inc | ../Drivers/STM32F4xx_HAL_Driver/Inc | ../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy | ../Middlewares/ST/STM32_USB_Device_Library/Core/Inc | ../Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc | ../Drivers/CMSIS/Device/ST/STM32F4xx/Include | ../Drivers/CMSIS/Include ||  ||  || USE_HAL_DRIVER | STM32F446xx ||  || Drivers | Core/Startup | Middlewares | Core | USB_DEVICE ||  ||  || ${workspace_loc:/${ProjName}/STM32F446RCTX_FLASH.ld} || true || NonSecure ||  || secure_nsclib.o ||  || None ||  ||  || " valueType="string"/>
+							<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.targetplatform.804643560" isAbstract="false" osList="all" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.targetplatform"/>
+							<builder buildPath="${workspace_loc:/soccer_firmware}/Debug" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.builder.1203747850" managedBuildOn="true" name="Gnu Make Builder.Debug" parallelBuildOn="true" parallelizationNumber="optimal" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.builder"/>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.99267941" name="MCU GCC Assembler" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler">
+								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.debuglevel.1170510629" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.debuglevel" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.debuglevel.value.g3" valueType="enumerated"/>
+								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.definedsymbols.101575766" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.definedsymbols" valueType="definedSymbols">
+									<listOptionValue builtIn="false" value="DEBUG"/>
+								</option>
+								<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.input.1623119034" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.input"/>
+							</tool>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.1622745735" name="MCU GCC Compiler" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler">
+								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.debuglevel.1750835666" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.debuglevel" useByScannerDiscovery="false" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.debuglevel.value.g3" valueType="enumerated"/>
+								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.optimization.level.1727836686" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.optimization.level" useByScannerDiscovery="false"/>
+								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.definedsymbols.1481740569" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.definedsymbols" useByScannerDiscovery="false" valueType="definedSymbols">
+									<listOptionValue builtIn="false" value="DEBUG"/>
+									<listOptionValue builtIn="false" value="USE_HAL_DRIVER"/>
+									<listOptionValue builtIn="false" value="STM32F446xx"/>
+								</option>
+								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.includepaths.528464354" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.includepaths" useByScannerDiscovery="false" valueType="includePath">
+									<listOptionValue builtIn="false" value="../USB_DEVICE/App"/>
+									<listOptionValue builtIn="false" value="../USB_DEVICE/Target"/>
+									<listOptionValue builtIn="false" value="../Core/Inc"/>
+									<listOptionValue builtIn="false" value="../Drivers/STM32F4xx_HAL_Driver/Inc"/>
+									<listOptionValue builtIn="false" value="../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy"/>
+									<listOptionValue builtIn="false" value="../Middlewares/ST/STM32_USB_Device_Library/Core/Inc"/>
+									<listOptionValue builtIn="false" value="../Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc"/>
+									<listOptionValue builtIn="false" value="../Drivers/CMSIS/Device/ST/STM32F4xx/Include"/>
+									<listOptionValue builtIn="false" value="../Drivers/CMSIS/Include"/>
+								</option>
+								<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.input.c.416437019" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.input.c"/>
+							</tool>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.2145909960" name="MCU G++ Compiler" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler">
+								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.debuglevel.2037698129" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.debuglevel" useByScannerDiscovery="false" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.debuglevel.value.g3" valueType="enumerated"/>
+								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.optimization.level.641320443" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.optimization.level" useByScannerDiscovery="false"/>
+							</tool>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.156005146" name="MCU GCC Linker" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker">
+								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.option.script.484151510" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.option.script" value="${workspace_loc:/${ProjName}/STM32F446RCTX_FLASH.ld}" valueType="string"/>
+								<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.input.905788596" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.input">
+									<additionalInput kind="additionalinputdependency" paths="$(USER_OBJS)"/>
+									<additionalInput kind="additionalinput" paths="$(LIBS)"/>
+								</inputType>
+							</tool>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.1145585942" name="MCU G++ Linker" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker"/>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.archiver.590962184" name="MCU GCC Archiver" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.archiver"/>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.size.2052614907" name="MCU Size" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.size"/>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objdump.listfile.1255938945" name="MCU Output Converter list file" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objdump.listfile"/>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.hex.436229644" name="MCU Output Converter Hex" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.hex"/>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.binary.1103717450" name="MCU Output Converter Binary" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.binary"/>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.verilog.1158446397" name="MCU Output Converter Verilog" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.verilog"/>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.srec.1532042514" name="MCU Output Converter Motorola S-rec" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.srec"/>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.symbolsrec.1967459526" name="MCU Output Converter Motorola S-rec with symbols" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.symbolsrec"/>
+						</toolChain>
+					</folderInfo>
+					<sourceEntries>
+						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Core"/>
+						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Middlewares"/>
+						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Drivers"/>
+						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="USB_DEVICE"/>
+					</sourceEntries>
+				</configuration>
+			</storageModule>
+		</cconfiguration>
+		<cconfiguration id="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.release.976303107">
+			<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.release.976303107" moduleId="org.eclipse.cdt.core.settings" name="Release">
+				<externalSettings/>
+				<extensions>
+					<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
+					<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+				</extensions>
+			</storageModule>
+			<storageModule moduleId="cdtBuildSystem" version="4.0.0">
+				<configuration artifactExtension="elf" artifactName="${ProjName}" buildArtefactType="org.eclipse.cdt.build.core.buildArtefactType.exe" buildProperties="org.eclipse.cdt.build.core.buildArtefactType=org.eclipse.cdt.build.core.buildArtefactType.exe,org.eclipse.cdt.build.core.buildType=org.eclipse.cdt.build.core.buildType.release" cleanCommand="rm -rf" description="" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.release.976303107" name="Release" parent="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.release">
+					<folderInfo id="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.release.976303107." name="/" resourcePath="">
+						<toolChain id="com.st.stm32cube.ide.mcu.gnu.managedbuild.toolchain.exe.release.3276894" name="MCU ARM GCC" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.toolchain.exe.release">
+							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_mcu.345369864" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_mcu" useByScannerDiscovery="true" value="STM32F446RCTx" valueType="string"/>
+							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_cpuid.843053658" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_cpuid" useByScannerDiscovery="false" value="0" valueType="string"/>
+							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_coreid.295771043" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_coreid" useByScannerDiscovery="false" value="0" valueType="string"/>
+							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.fpu.341838799" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.fpu" useByScannerDiscovery="true" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.fpu.value.fpv4-sp-d16" valueType="enumerated"/>
+							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.floatabi.491369348" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.floatabi" useByScannerDiscovery="true" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.floatabi.value.hard" valueType="enumerated"/>
+							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_board.1035814544" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_board" useByScannerDiscovery="false" value="genericBoard" valueType="string"/>
+							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.defaults.1319827053" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.defaults" useByScannerDiscovery="false" value="com.st.stm32cube.ide.common.services.build.inputs.revA.1.0.6 || Release || false || Executable || com.st.stm32cube.ide.mcu.gnu.managedbuild.option.toolchain.value.workspace || STM32F446RCTx || 0 || 0 || arm-none-eabi- || ${gnu_tools_for_stm32_compiler_path} || ../USB_DEVICE/App | ../USB_DEVICE/Target | ../Core/Inc | ../Drivers/STM32F4xx_HAL_Driver/Inc | ../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy | ../Middlewares/ST/STM32_USB_Device_Library/Core/Inc | ../Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc | ../Drivers/CMSIS/Device/ST/STM32F4xx/Include | ../Drivers/CMSIS/Include ||  ||  || USE_HAL_DRIVER | STM32F446xx ||  || Drivers | Core/Startup | Middlewares | Core | USB_DEVICE ||  ||  || ${workspace_loc:/${ProjName}/STM32F446RCTX_FLASH.ld} || true || NonSecure ||  || secure_nsclib.o ||  || None ||  ||  || " valueType="string"/>
+							<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.targetplatform.1275815241" isAbstract="false" osList="all" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.targetplatform"/>
+							<builder buildPath="${workspace_loc:/soccer_firmware}/Release" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.builder.419542566" managedBuildOn="true" name="Gnu Make Builder.Release" parallelBuildOn="true" parallelizationNumber="optimal" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.builder"/>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.2083855866" name="MCU GCC Assembler" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler">
+								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.debuglevel.1488222273" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.debuglevel" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.debuglevel.value.g0" valueType="enumerated"/>
+								<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.input.861242225" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.input"/>
+							</tool>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.1624578333" name="MCU GCC Compiler" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler">
+								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.debuglevel.612261277" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.debuglevel" useByScannerDiscovery="false" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.debuglevel.value.g0" valueType="enumerated"/>
+								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.optimization.level.1293553770" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.optimization.level" useByScannerDiscovery="false" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.optimization.level.value.os" valueType="enumerated"/>
+								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.definedsymbols.1698401717" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.definedsymbols" useByScannerDiscovery="false" valueType="definedSymbols">
+									<listOptionValue builtIn="false" value="USE_HAL_DRIVER"/>
+									<listOptionValue builtIn="false" value="STM32F446xx"/>
+								</option>
+								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.includepaths.889365987" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.includepaths" useByScannerDiscovery="false" valueType="includePath">
+									<listOptionValue builtIn="false" value="../USB_DEVICE/App"/>
+									<listOptionValue builtIn="false" value="../USB_DEVICE/Target"/>
+									<listOptionValue builtIn="false" value="../Core/Inc"/>
+									<listOptionValue builtIn="false" value="../Drivers/STM32F4xx_HAL_Driver/Inc"/>
+									<listOptionValue builtIn="false" value="../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy"/>
+									<listOptionValue builtIn="false" value="../Middlewares/ST/STM32_USB_Device_Library/Core/Inc"/>
+									<listOptionValue builtIn="false" value="../Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc"/>
+									<listOptionValue builtIn="false" value="../Drivers/CMSIS/Device/ST/STM32F4xx/Include"/>
+									<listOptionValue builtIn="false" value="../Drivers/CMSIS/Include"/>
+								</option>
+								<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.input.c.1859769067" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.input.c"/>
+							</tool>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.962265918" name="MCU G++ Compiler" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler">
+								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.debuglevel.865605295" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.debuglevel" useByScannerDiscovery="false" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.debuglevel.value.g0" valueType="enumerated"/>
+								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.optimization.level.1446397763" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.optimization.level" useByScannerDiscovery="false" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.optimization.level.value.os" valueType="enumerated"/>
+							</tool>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.1855528563" name="MCU GCC Linker" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker">
+								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.option.script.359546705" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.option.script" value="${workspace_loc:/${ProjName}/STM32F446RCTX_FLASH.ld}" valueType="string"/>
+								<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.input.520709163" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.input">
+									<additionalInput kind="additionalinputdependency" paths="$(USER_OBJS)"/>
+									<additionalInput kind="additionalinput" paths="$(LIBS)"/>
+								</inputType>
+							</tool>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker.370082265" name="MCU G++ Linker" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.linker"/>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.archiver.1355613949" name="MCU GCC Archiver" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.archiver"/>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.size.1638896371" name="MCU Size" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.size"/>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objdump.listfile.2093191329" name="MCU Output Converter list file" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objdump.listfile"/>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.hex.505340111" name="MCU Output Converter Hex" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.hex"/>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.binary.96035328" name="MCU Output Converter Binary" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.binary"/>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.verilog.518893620" name="MCU Output Converter Verilog" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.verilog"/>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.srec.1326498388" name="MCU Output Converter Motorola S-rec" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.srec"/>
+							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.symbolsrec.1713353981" name="MCU Output Converter Motorola S-rec with symbols" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.symbolsrec"/>
+						</toolChain>
+					</folderInfo>
+					<sourceEntries>
+						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Core"/>
+						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Middlewares"/>
+						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Drivers"/>
+						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="USB_DEVICE"/>
+					</sourceEntries>
+				</configuration>
+			</storageModule>
+		</cconfiguration>
 	</storageModule>
 	<storageModule moduleId="org.eclipse.cdt.core.pathentry"/>
 	<storageModule moduleId="cdtBuildSystem" version="4.0.0">
@@ -210,5 +384,25 @@
 	<storageModule moduleId="org.eclipse.cdt.core.LanguageSettingsProviders"/>
 	<storageModule moduleId="org.eclipse.cdt.make.core.buildtargets"/>
 	<storageModule moduleId="refreshScope"/>
-	<storageModule moduleId="scannerConfiguration"/>
-</cproject>
+	<storageModule moduleId="scannerConfiguration">
+		<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
+		<scannerConfigBuildInfo instanceId="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.release.1071476856;com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.release.1071476856.;com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.907636321;com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.input.c.507996537">
+			<autodiscovery enabled="false" problemReportingEnabled="true" selectedProfileId=""/>
+		</scannerConfigBuildInfo>
+		<scannerConfigBuildInfo instanceId="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.release.1071476856;com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.release.1071476856.;com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.1434796339;com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.input.cpp.370155548">
+			<autodiscovery enabled="false" problemReportingEnabled="true" selectedProfileId=""/>
+		</scannerConfigBuildInfo>
+		<scannerConfigBuildInfo instanceId="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug.683473154;com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug.683473154.;com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.30071470;com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.input.c.1233310560">
+			<autodiscovery enabled="false" problemReportingEnabled="true" selectedProfileId=""/>
+		</scannerConfigBuildInfo>
+		<scannerConfigBuildInfo instanceId="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug.683473154;com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug.683473154.;com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.1389884261;com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.input.cpp.402515729">
+			<autodiscovery enabled="false" problemReportingEnabled="true" selectedProfileId=""/>
+		</scannerConfigBuildInfo>
+		<scannerConfigBuildInfo instanceId="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug.1206544251;com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug.1206544251.;com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.1622745735;com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.input.c.416437019">
+			<autodiscovery enabled="false" problemReportingEnabled="true" selectedProfileId=""/>
+		</scannerConfigBuildInfo>
+		<scannerConfigBuildInfo instanceId="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.release.976303107;com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.release.976303107.;com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.1624578333;com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.input.c.1859769067">
+			<autodiscovery enabled="false" problemReportingEnabled="true" selectedProfileId=""/>
+		</scannerConfigBuildInfo>
+	</storageModule>
+</cproject>
\ No newline at end of file
diff --git a/soccer_firmware/.project b/soccer_firmware/.project
index d3af63d12..b09b4c428 100644
--- a/soccer_firmware/.project
+++ b/soccer_firmware/.project
@@ -22,6 +22,7 @@
 		<nature>com.st.stm32cube.ide.mcu.MCUProjectNature</nature>
 		<nature>com.st.stm32cube.ide.mcu.MCUCubeProjectNature</nature>
 		<nature>org.eclipse.cdt.core.cnature</nature>
+		<nature>org.eclipse.cdt.core.ccnature</nature>
 		<nature>com.st.stm32cube.ide.mcu.MCUCubeIdeServicesRevAev2ProjectNature</nature>
 		<nature>com.st.stm32cube.ide.mcu.MCUAdvancedStructureProjectNature</nature>
 		<nature>com.st.stm32cube.ide.mcu.MCUSingleCpuProjectNature</nature>
diff --git a/soccer_firmware/.settings/language.settings.xml b/soccer_firmware/.settings/language.settings.xml
index ee9d118df..2b38d7304 100644
--- a/soccer_firmware/.settings/language.settings.xml
+++ b/soccer_firmware/.settings/language.settings.xml
@@ -5,7 +5,7 @@
 			<provider-reference id="org.eclipse.cdt.ui.UserLanguageSettingsProvider" ref="shared-provider"/>
 			<provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
 			<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/>
-			<provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="-1784325041921134830" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
+			<provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="-260604961516797221" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
 				<language-scope id="org.eclipse.cdt.core.gcc"/>
 				<language-scope id="org.eclipse.cdt.core.g++"/>
 			</provider>
@@ -16,7 +16,7 @@
 			<provider-reference id="org.eclipse.cdt.ui.UserLanguageSettingsProvider" ref="shared-provider"/>
 			<provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
 			<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/>
-			<provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="-1784325041921134830" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
+			<provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="-260604961516797221" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
 				<language-scope id="org.eclipse.cdt.core.gcc"/>
 				<language-scope id="org.eclipse.cdt.core.g++"/>
 			</provider>
@@ -24,10 +24,10 @@
 	</configuration>
 	<configuration id="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug.1206544251" name="Debug">
 		<extension point="org.eclipse.cdt.core.LanguageSettingsProvider">
-			<provider-reference id="org.eclipse.cdt.ui.UserLanguageSettingsProvider" ref="shared-provider"/>
+			<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/>
 			<provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
 			<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/>
-			<provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="-1784325041921134830" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
+			<provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="-260604961516797221" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
 				<language-scope id="org.eclipse.cdt.core.gcc"/>
 				<language-scope id="org.eclipse.cdt.core.g++"/>
 			</provider>
@@ -35,13 +35,13 @@
 	</configuration>
 	<configuration id="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.release.976303107" name="Release">
 		<extension point="org.eclipse.cdt.core.LanguageSettingsProvider">
-			<provider-reference id="org.eclipse.cdt.ui.UserLanguageSettingsProvider" ref="shared-provider"/>
+			<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/>
 			<provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
 			<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/>
-			<provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="-1784325041921134830" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
+			<provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="-260604961516797221" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
 				<language-scope id="org.eclipse.cdt.core.gcc"/>
 				<language-scope id="org.eclipse.cdt.core.g++"/>
 			</provider>
 		</extension>
 	</configuration>
-</project>
+</project>
\ No newline at end of file
diff --git a/soccer_firmware/Core/Inc/stm32f4xx_hal_conf.h b/soccer_firmware/Core/Inc/stm32f4xx_hal_conf.h
index d67017b92..6baa6bd9b 100644
--- a/soccer_firmware/Core/Inc/stm32f4xx_hal_conf.h
+++ b/soccer_firmware/Core/Inc/stm32f4xx_hal_conf.h
@@ -222,7 +222,7 @@
 /* Section 2: PHY configuration section */
 
 /* DP83848_PHY_ADDRESS Address*/
-#define DP83848_PHY_ADDRESS
+#define DP83848_PHY_ADDRESS           0x01U
 /* PHY Reset delay these values are based on a 1 ms Systick interrupt*/
 #define PHY_RESET_DELAY                 0x000000FFU
 /* PHY Configuration delay */
@@ -252,10 +252,10 @@
 #define PHY_JABBER_DETECTION            ((uint16_t)0x0002U)  /*!< Jabber condition detected            */
 
 /* Section 4: Extended PHY Registers */
-#define PHY_SR                          ((uint16_t))    /*!< PHY status register Offset                      */
+#define PHY_SR                          ((uint16_t)0x10U)    /*!< PHY status register Offset                      */
 
-#define PHY_SPEED_STATUS                ((uint16_t))  /*!< PHY Speed mask                                  */
-#define PHY_DUPLEX_STATUS               ((uint16_t))  /*!< PHY Duplex mask                                 */
+#define PHY_SPEED_STATUS                ((uint16_t)0x0002U)  /*!< PHY Speed mask                                  */
+#define PHY_DUPLEX_STATUS               ((uint16_t)0x0004U)  /*!< PHY Duplex mask                                 */
 
 /* ################## SPI peripheral configuration ########################## */
 
diff --git a/soccer_firmware/Core/Src/dynamixel_p1.c b/soccer_firmware/Core/Src/dynamixel_p1.c
index 2f4936c0e..7331a0b42 100644
--- a/soccer_firmware/Core/Src/dynamixel_p1.c
+++ b/soccer_firmware/Core/Src/dynamixel_p1.c
@@ -5,7 +5,6 @@
  */
 
 #include "dynamixel_p1.h"
-#include "main.h"
 
 /*
  * Dynamixel 1.0
diff --git a/soccer_firmware/Core/Src/dynamixel_p2.c b/soccer_firmware/Core/Src/dynamixel_p2.c
index 25e638d88..084390837 100644
--- a/soccer_firmware/Core/Src/dynamixel_p2.c
+++ b/soccer_firmware/Core/Src/dynamixel_p2.c
@@ -6,7 +6,7 @@
  */
 
 #include "dynamixel_p2.h"
-#include "main.h"
+//#include "main.h"
 
 
 /*
diff --git a/soccer_firmware/Core/Src/main.c b/soccer_firmware/Core/Src/main.c
index d27b60318..1f0d6af9a 100644
--- a/soccer_firmware/Core/Src/main.c
+++ b/soccer_firmware/Core/Src/main.c
@@ -249,7 +249,7 @@ 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(5000);
+  HAL_Delay(1000);
 
   init_ports();
 //  MPU6050_init();
diff --git a/soccer_firmware/Core/Src/stm32f4xx_hal_msp.c b/soccer_firmware/Core/Src/stm32f4xx_hal_msp.c
index 923262c9c..a0b588209 100644
--- a/soccer_firmware/Core/Src/stm32f4xx_hal_msp.c
+++ b/soccer_firmware/Core/Src/stm32f4xx_hal_msp.c
@@ -20,7 +20,6 @@
 
 /* Includes ------------------------------------------------------------------*/
 #include "main.h"
-
 /* USER CODE BEGIN Includes */
 
 /* USER CODE END Includes */
diff --git a/soccer_firmware/Core/Src/update_loop.c b/soccer_firmware/Core/Src/update_loop.c
index 79469c03d..17cb1098a 100644
--- a/soccer_firmware/Core/Src/update_loop.c
+++ b/soccer_firmware/Core/Src/update_loop.c
@@ -144,13 +144,6 @@ void command_motors() {
 }
 
 void read_imu(uint8_t *rxBuf) {
-  uint8_t gyrBuff[6] = {0};
-  Read_Gyroscope(gyrBuff);
-  HAL_Delay(1); // wait some time before sending next request
-
-  uint8_t accBuff[6] = {0};
-  Read_Accelerometer_IT(accBuff);
-
   int16_t gyroX = 0;
   int16_t gyroY = 0;
   int16_t gyroZ = 0;
@@ -158,7 +151,6 @@ void read_imu(uint8_t *rxBuf) {
   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 :/)
diff --git a/soccer_firmware/Core/Startup/startup_stm32f446rctx.s b/soccer_firmware/Core/Startup/startup_stm32f446rctx.s
index bcb58a059..41fb9f455 100644
--- a/soccer_firmware/Core/Startup/startup_stm32f446rctx.s
+++ b/soccer_firmware/Core/Startup/startup_stm32f446rctx.s
@@ -1,535 +1,535 @@
-/**
-  ******************************************************************************
-  * @file      startup_stm32f446xx.s
-  * @author    MCD Application Team
-  * @brief     STM32F446xx Devices vector table for GCC based toolchains.
-  *            This module performs:
-  *                - Set the initial SP
-  *                - Set the initial PC == Reset_Handler,
-  *                - Set the vector table entries with the exceptions ISR address
-  *                - Branches to main in the C library (which eventually
-  *                  calls main()).
-  *            After Reset the Cortex-M4 processor is in Thread mode,
-  *            priority is Privileged, and the Stack is set to Main.
-  ******************************************************************************
-  * @attention
-  *
-  * Copyright (c) 2017 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.
-  *
-  ******************************************************************************
-  */
-
-  .syntax unified
-  .cpu cortex-m4
-  .fpu softvfp
-  .thumb
-
-.global  g_pfnVectors
-.global  Default_Handler
-
-/* start address for the initialization values of the .data section.
-defined in linker script */
-.word  _sidata
-/* start address for the .data section. defined in linker script */
-.word  _sdata
-/* end address for the .data section. defined in linker script */
-.word  _edata
-/* start address for the .bss section. defined in linker script */
-.word  _sbss
-/* end address for the .bss section. defined in linker script */
-.word  _ebss
-/* stack used for SystemInit_ExtMemCtl; always internal RAM used */
-
-/**
- * @brief  This is the code that gets called when the processor first
- *          starts execution following a reset event. Only the absolutely
- *          necessary set is performed, after which the application
- *          supplied main() routine is called.
- * @param  None
- * @retval : None
-*/
-
-    .section  .text.Reset_Handler
-  .weak  Reset_Handler
-  .type  Reset_Handler, %function
-Reset_Handler:
-  ldr   sp, =_estack      /* set stack pointer */
-
-/* Copy the data segment initializers from flash to SRAM */
-  ldr r0, =_sdata
-  ldr r1, =_edata
-  ldr r2, =_sidata
-  movs r3, #0
-  b LoopCopyDataInit
-
-CopyDataInit:
-  ldr r4, [r2, r3]
-  str r4, [r0, r3]
-  adds r3, r3, #4
-
-LoopCopyDataInit:
-  adds r4, r0, r3
-  cmp r4, r1
-  bcc CopyDataInit
-
-/* Zero fill the bss segment. */
-  ldr r2, =_sbss
-  ldr r4, =_ebss
-  movs r3, #0
-  b LoopFillZerobss
-
-FillZerobss:
-  str  r3, [r2]
-  adds r2, r2, #4
-
-LoopFillZerobss:
-  cmp r2, r4
-  bcc FillZerobss
-
-/* Call the clock system initialization function.*/
-  bl  SystemInit
-/* Call static constructors */
-    bl __libc_init_array
-/* Call the application's entry point.*/
-  bl  main
-  bx  lr
-.size  Reset_Handler, .-Reset_Handler
-
-/**
- * @brief  This is the code that gets called when the processor receives an
- *         unexpected interrupt.  This simply enters an infinite loop, preserving
- *         the system state for examination by a debugger.
- * @param  None
- * @retval None
-*/
-    .section  .text.Default_Handler,"ax",%progbits
-Default_Handler:
-Infinite_Loop:
-  b  Infinite_Loop
-  .size  Default_Handler, .-Default_Handler
-/******************************************************************************
-*
-* The minimal vector table for a Cortex M3. Note that the proper constructs
-* must be placed on this to ensure that it ends up at physical address
-* 0x0000.0000.
-*
-*******************************************************************************/
-   .section  .isr_vector,"a",%progbits
-  .type  g_pfnVectors, %object
-  .size  g_pfnVectors, .-g_pfnVectors
-
-
-g_pfnVectors:
-  .word  _estack
-  .word  Reset_Handler
-
-  .word  NMI_Handler
-  .word  HardFault_Handler
-  .word  MemManage_Handler
-  .word  BusFault_Handler
-  .word  UsageFault_Handler
-  .word  0
-  .word  0
-  .word  0
-  .word  0
-  .word  SVC_Handler
-  .word  DebugMon_Handler
-  .word  0
-  .word  PendSV_Handler
-  .word  SysTick_Handler
-
-  /* External Interrupts */
-  .word     WWDG_IRQHandler                   /* Window WatchDog              */
-  .word     PVD_IRQHandler                    /* PVD through EXTI Line detection */
-  .word     TAMP_STAMP_IRQHandler             /* Tamper and TimeStamps through the EXTI line */
-  .word     RTC_WKUP_IRQHandler               /* RTC Wakeup through the EXTI line */
-  .word     FLASH_IRQHandler                  /* FLASH                        */
-  .word     RCC_IRQHandler                    /* RCC                          */
-  .word     EXTI0_IRQHandler                  /* EXTI Line0                   */
-  .word     EXTI1_IRQHandler                  /* EXTI Line1                   */
-  .word     EXTI2_IRQHandler                  /* EXTI Line2                   */
-  .word     EXTI3_IRQHandler                  /* EXTI Line3                   */
-  .word     EXTI4_IRQHandler                  /* EXTI Line4                   */
-  .word     DMA1_Stream0_IRQHandler           /* DMA1 Stream 0                */
-  .word     DMA1_Stream1_IRQHandler           /* DMA1 Stream 1                */
-  .word     DMA1_Stream2_IRQHandler           /* DMA1 Stream 2                */
-  .word     DMA1_Stream3_IRQHandler           /* DMA1 Stream 3                */
-  .word     DMA1_Stream4_IRQHandler           /* DMA1 Stream 4                */
-  .word     DMA1_Stream5_IRQHandler           /* DMA1 Stream 5                */
-  .word     DMA1_Stream6_IRQHandler           /* DMA1 Stream 6                */
-  .word     ADC_IRQHandler                    /* ADC1, ADC2 and ADC3s         */
-  .word     CAN1_TX_IRQHandler                /* CAN1 TX                      */
-  .word     CAN1_RX0_IRQHandler               /* CAN1 RX0                     */
-  .word     CAN1_RX1_IRQHandler               /* CAN1 RX1                     */
-  .word     CAN1_SCE_IRQHandler               /* CAN1 SCE                     */
-  .word     EXTI9_5_IRQHandler                /* External Line[9:5]s          */
-  .word     TIM1_BRK_TIM9_IRQHandler          /* TIM1 Break and TIM9          */
-  .word     TIM1_UP_TIM10_IRQHandler          /* TIM1 Update and TIM10        */
-  .word     TIM1_TRG_COM_TIM11_IRQHandler     /* TIM1 Trigger and Commutation and TIM11 */
-  .word     TIM1_CC_IRQHandler                /* TIM1 Capture Compare         */
-  .word     TIM2_IRQHandler                   /* TIM2                         */
-  .word     TIM3_IRQHandler                   /* TIM3                         */
-  .word     TIM4_IRQHandler                   /* TIM4                         */
-  .word     I2C1_EV_IRQHandler                /* I2C1 Event                   */
-  .word     I2C1_ER_IRQHandler                /* I2C1 Error                   */
-  .word     I2C2_EV_IRQHandler                /* I2C2 Event                   */
-  .word     I2C2_ER_IRQHandler                /* I2C2 Error                   */
-  .word     SPI1_IRQHandler                   /* SPI1                         */
-  .word     SPI2_IRQHandler                   /* SPI2                         */
-  .word     USART1_IRQHandler                 /* USART1                       */
-  .word     USART2_IRQHandler                 /* USART2                       */
-  .word     USART3_IRQHandler                 /* USART3                       */
-  .word     EXTI15_10_IRQHandler              /* External Line[15:10]s        */
-  .word     RTC_Alarm_IRQHandler              /* RTC Alarm (A and B) through EXTI Line */
-  .word     OTG_FS_WKUP_IRQHandler            /* USB OTG FS Wakeup through EXTI line */
-  .word     TIM8_BRK_TIM12_IRQHandler         /* TIM8 Break and TIM12         */
-  .word     TIM8_UP_TIM13_IRQHandler          /* TIM8 Update and TIM13        */
-  .word     TIM8_TRG_COM_TIM14_IRQHandler     /* TIM8 Trigger and Commutation and TIM14 */
-  .word     TIM8_CC_IRQHandler                /* TIM8 Capture Compare         */
-  .word     DMA1_Stream7_IRQHandler           /* DMA1 Stream7                 */
-  .word     FMC_IRQHandler                    /* FMC                          */
-  .word     SDIO_IRQHandler                   /* SDIO                         */
-  .word     TIM5_IRQHandler                   /* TIM5                         */
-  .word     SPI3_IRQHandler                   /* SPI3                         */
-  .word     UART4_IRQHandler                  /* UART4                        */
-  .word     UART5_IRQHandler                  /* UART5                        */
-  .word     TIM6_DAC_IRQHandler               /* TIM6 and DAC1&2 underrun errors */
-  .word     TIM7_IRQHandler                   /* TIM7                         */
-  .word     DMA2_Stream0_IRQHandler           /* DMA2 Stream 0                */
-  .word     DMA2_Stream1_IRQHandler           /* DMA2 Stream 1                */
-  .word     DMA2_Stream2_IRQHandler           /* DMA2 Stream 2                */
-  .word     DMA2_Stream3_IRQHandler           /* DMA2 Stream 3                */
-  .word     DMA2_Stream4_IRQHandler           /* DMA2 Stream 4                */
-  .word     0                                 /* Reserved                     */
-  .word     0                                 /* Reserved                     */
-  .word     CAN2_TX_IRQHandler                /* CAN2 TX                      */
-  .word     CAN2_RX0_IRQHandler               /* CAN2 RX0                     */
-  .word     CAN2_RX1_IRQHandler               /* CAN2 RX1                     */
-  .word     CAN2_SCE_IRQHandler               /* CAN2 SCE                     */
-  .word     OTG_FS_IRQHandler                 /* USB OTG FS                   */
-  .word     DMA2_Stream5_IRQHandler           /* DMA2 Stream 5                */
-  .word     DMA2_Stream6_IRQHandler           /* DMA2 Stream 6                */
-  .word     DMA2_Stream7_IRQHandler           /* DMA2 Stream 7                */
-  .word     USART6_IRQHandler                 /* USART6                       */
-  .word     I2C3_EV_IRQHandler                /* I2C3 event                   */
-  .word     I2C3_ER_IRQHandler                /* I2C3 error                   */
-  .word     OTG_HS_EP1_OUT_IRQHandler         /* USB OTG HS End Point 1 Out   */
-  .word     OTG_HS_EP1_IN_IRQHandler          /* USB OTG HS End Point 1 In    */
-  .word     OTG_HS_WKUP_IRQHandler            /* USB OTG HS Wakeup through EXTI */
-  .word     OTG_HS_IRQHandler                 /* USB OTG HS                   */
-  .word     DCMI_IRQHandler                   /* DCMI                         */
-  .word     0                                 /* Reserved                     */
-  .word     0                                 /* Reserved                     */
-  .word     FPU_IRQHandler                    /* FPU                          */
-  .word     0                                 /* Reserved                     */
-  .word     0                                 /* Reserved                     */
-  .word     SPI4_IRQHandler                   /* SPI4                         */
-  .word     0                                 /* Reserved                     */
-  .word     0                                 /* Reserved                     */
-  .word     SAI1_IRQHandler                   /* SAI1                         */
-  .word     0                                 /* Reserved                     */
-  .word     0                                 /* Reserved                     */
-  .word     0                                 /* Reserved                     */
-  .word     SAI2_IRQHandler                   /* SAI2                         */
-  .word     QUADSPI_IRQHandler                /* QuadSPI                      */
-  .word     CEC_IRQHandler                    /* CEC                          */
-  .word     SPDIF_RX_IRQHandler               /* SPDIF RX                     */
-  .word     FMPI2C1_EV_IRQHandler          /* FMPI2C 1 Event               */
-  .word     FMPI2C1_ER_IRQHandler          /* FMPI2C 1 Error               */
-
-/*******************************************************************************
-*
-* Provide weak aliases for each Exception handler to the Default_Handler.
-* As they are weak aliases, any function with the same name will override
-* this definition.
-*
-*******************************************************************************/
-   .weak      NMI_Handler
-   .thumb_set NMI_Handler,Default_Handler
-
-   .weak      HardFault_Handler
-   .thumb_set HardFault_Handler,Default_Handler
-
-   .weak      MemManage_Handler
-   .thumb_set MemManage_Handler,Default_Handler
-
-   .weak      BusFault_Handler
-   .thumb_set BusFault_Handler,Default_Handler
-
-   .weak      UsageFault_Handler
-   .thumb_set UsageFault_Handler,Default_Handler
-
-   .weak      SVC_Handler
-   .thumb_set SVC_Handler,Default_Handler
-
-   .weak      DebugMon_Handler
-   .thumb_set DebugMon_Handler,Default_Handler
-
-   .weak      PendSV_Handler
-   .thumb_set PendSV_Handler,Default_Handler
-
-   .weak      SysTick_Handler
-   .thumb_set SysTick_Handler,Default_Handler
-
-   .weak      WWDG_IRQHandler
-   .thumb_set WWDG_IRQHandler,Default_Handler
-
-   .weak      PVD_IRQHandler
-   .thumb_set PVD_IRQHandler,Default_Handler
-
-   .weak      TAMP_STAMP_IRQHandler
-   .thumb_set TAMP_STAMP_IRQHandler,Default_Handler
-
-   .weak      RTC_WKUP_IRQHandler
-   .thumb_set RTC_WKUP_IRQHandler,Default_Handler
-
-   .weak      FLASH_IRQHandler
-   .thumb_set FLASH_IRQHandler,Default_Handler
-
-   .weak      RCC_IRQHandler
-   .thumb_set RCC_IRQHandler,Default_Handler
-
-   .weak      EXTI0_IRQHandler
-   .thumb_set EXTI0_IRQHandler,Default_Handler
-
-   .weak      EXTI1_IRQHandler
-   .thumb_set EXTI1_IRQHandler,Default_Handler
-
-   .weak      EXTI2_IRQHandler
-   .thumb_set EXTI2_IRQHandler,Default_Handler
-
-   .weak      EXTI3_IRQHandler
-   .thumb_set EXTI3_IRQHandler,Default_Handler
-
-   .weak      EXTI4_IRQHandler
-   .thumb_set EXTI4_IRQHandler,Default_Handler
-
-   .weak      DMA1_Stream0_IRQHandler
-   .thumb_set DMA1_Stream0_IRQHandler,Default_Handler
-
-   .weak      DMA1_Stream1_IRQHandler
-   .thumb_set DMA1_Stream1_IRQHandler,Default_Handler
-
-   .weak      DMA1_Stream2_IRQHandler
-   .thumb_set DMA1_Stream2_IRQHandler,Default_Handler
-
-   .weak      DMA1_Stream3_IRQHandler
-   .thumb_set DMA1_Stream3_IRQHandler,Default_Handler
-
-   .weak      DMA1_Stream4_IRQHandler
-   .thumb_set DMA1_Stream4_IRQHandler,Default_Handler
-
-   .weak      DMA1_Stream5_IRQHandler
-   .thumb_set DMA1_Stream5_IRQHandler,Default_Handler
-
-   .weak      DMA1_Stream6_IRQHandler
-   .thumb_set DMA1_Stream6_IRQHandler,Default_Handler
-
-   .weak      ADC_IRQHandler
-   .thumb_set ADC_IRQHandler,Default_Handler
-
-   .weak      CAN1_TX_IRQHandler
-   .thumb_set CAN1_TX_IRQHandler,Default_Handler
-
-   .weak      CAN1_RX0_IRQHandler
-   .thumb_set CAN1_RX0_IRQHandler,Default_Handler
-
-   .weak      CAN1_RX1_IRQHandler
-   .thumb_set CAN1_RX1_IRQHandler,Default_Handler
-
-   .weak      CAN1_SCE_IRQHandler
-   .thumb_set CAN1_SCE_IRQHandler,Default_Handler
-
-   .weak      EXTI9_5_IRQHandler
-   .thumb_set EXTI9_5_IRQHandler,Default_Handler
-
-   .weak      TIM1_BRK_TIM9_IRQHandler
-   .thumb_set TIM1_BRK_TIM9_IRQHandler,Default_Handler
-
-   .weak      TIM1_UP_TIM10_IRQHandler
-   .thumb_set TIM1_UP_TIM10_IRQHandler,Default_Handler
-
-   .weak      TIM1_TRG_COM_TIM11_IRQHandler
-   .thumb_set TIM1_TRG_COM_TIM11_IRQHandler,Default_Handler
-
-   .weak      TIM1_CC_IRQHandler
-   .thumb_set TIM1_CC_IRQHandler,Default_Handler
-
-   .weak      TIM2_IRQHandler
-   .thumb_set TIM2_IRQHandler,Default_Handler
-
-   .weak      TIM3_IRQHandler
-   .thumb_set TIM3_IRQHandler,Default_Handler
-
-   .weak      TIM4_IRQHandler
-   .thumb_set TIM4_IRQHandler,Default_Handler
-
-   .weak      I2C1_EV_IRQHandler
-   .thumb_set I2C1_EV_IRQHandler,Default_Handler
-
-   .weak      I2C1_ER_IRQHandler
-   .thumb_set I2C1_ER_IRQHandler,Default_Handler
-
-   .weak      I2C2_EV_IRQHandler
-   .thumb_set I2C2_EV_IRQHandler,Default_Handler
-
-   .weak      I2C2_ER_IRQHandler
-   .thumb_set I2C2_ER_IRQHandler,Default_Handler
-
-   .weak      SPI1_IRQHandler
-   .thumb_set SPI1_IRQHandler,Default_Handler
-
-   .weak      SPI2_IRQHandler
-   .thumb_set SPI2_IRQHandler,Default_Handler
-
-   .weak      USART1_IRQHandler
-   .thumb_set USART1_IRQHandler,Default_Handler
-
-   .weak      USART2_IRQHandler
-   .thumb_set USART2_IRQHandler,Default_Handler
-
-   .weak      USART3_IRQHandler
-   .thumb_set USART3_IRQHandler,Default_Handler
-
-   .weak      EXTI15_10_IRQHandler
-   .thumb_set EXTI15_10_IRQHandler,Default_Handler
-
-   .weak      RTC_Alarm_IRQHandler
-   .thumb_set RTC_Alarm_IRQHandler,Default_Handler
-
-   .weak      OTG_FS_WKUP_IRQHandler
-   .thumb_set OTG_FS_WKUP_IRQHandler,Default_Handler
-
-   .weak      TIM8_BRK_TIM12_IRQHandler
-   .thumb_set TIM8_BRK_TIM12_IRQHandler,Default_Handler
-
-   .weak      TIM8_UP_TIM13_IRQHandler
-   .thumb_set TIM8_UP_TIM13_IRQHandler,Default_Handler
-
-   .weak      TIM8_TRG_COM_TIM14_IRQHandler
-   .thumb_set TIM8_TRG_COM_TIM14_IRQHandler,Default_Handler
-
-   .weak      TIM8_CC_IRQHandler
-   .thumb_set TIM8_CC_IRQHandler,Default_Handler
-
-   .weak      DMA1_Stream7_IRQHandler
-   .thumb_set DMA1_Stream7_IRQHandler,Default_Handler
-
-   .weak      FMC_IRQHandler
-   .thumb_set FMC_IRQHandler,Default_Handler
-
-   .weak      SDIO_IRQHandler
-   .thumb_set SDIO_IRQHandler,Default_Handler
-
-   .weak      TIM5_IRQHandler
-   .thumb_set TIM5_IRQHandler,Default_Handler
-
-   .weak      SPI3_IRQHandler
-   .thumb_set SPI3_IRQHandler,Default_Handler
-
-   .weak      UART4_IRQHandler
-   .thumb_set UART4_IRQHandler,Default_Handler
-
-   .weak      UART5_IRQHandler
-   .thumb_set UART5_IRQHandler,Default_Handler
-
-   .weak      TIM6_DAC_IRQHandler
-   .thumb_set TIM6_DAC_IRQHandler,Default_Handler
-
-   .weak      TIM7_IRQHandler
-   .thumb_set TIM7_IRQHandler,Default_Handler
-
-   .weak      DMA2_Stream0_IRQHandler
-   .thumb_set DMA2_Stream0_IRQHandler,Default_Handler
-
-   .weak      DMA2_Stream1_IRQHandler
-   .thumb_set DMA2_Stream1_IRQHandler,Default_Handler
-
-   .weak      DMA2_Stream2_IRQHandler
-   .thumb_set DMA2_Stream2_IRQHandler,Default_Handler
-
-   .weak      DMA2_Stream3_IRQHandler
-   .thumb_set DMA2_Stream3_IRQHandler,Default_Handler
-
-   .weak      DMA2_Stream4_IRQHandler
-   .thumb_set DMA2_Stream4_IRQHandler,Default_Handler
-
-   .weak      CAN2_TX_IRQHandler
-   .thumb_set CAN2_TX_IRQHandler,Default_Handler
-
-   .weak      CAN2_RX0_IRQHandler
-   .thumb_set CAN2_RX0_IRQHandler,Default_Handler
-
-   .weak      CAN2_RX1_IRQHandler
-   .thumb_set CAN2_RX1_IRQHandler,Default_Handler
-
-   .weak      CAN2_SCE_IRQHandler
-   .thumb_set CAN2_SCE_IRQHandler,Default_Handler
-
-   .weak      OTG_FS_IRQHandler
-   .thumb_set OTG_FS_IRQHandler,Default_Handler
-
-   .weak      DMA2_Stream5_IRQHandler
-   .thumb_set DMA2_Stream5_IRQHandler,Default_Handler
-
-   .weak      DMA2_Stream6_IRQHandler
-   .thumb_set DMA2_Stream6_IRQHandler,Default_Handler
-
-   .weak      DMA2_Stream7_IRQHandler
-   .thumb_set DMA2_Stream7_IRQHandler,Default_Handler
-
-   .weak      USART6_IRQHandler
-   .thumb_set USART6_IRQHandler,Default_Handler
-
-   .weak      I2C3_EV_IRQHandler
-   .thumb_set I2C3_EV_IRQHandler,Default_Handler
-
-   .weak      I2C3_ER_IRQHandler
-   .thumb_set I2C3_ER_IRQHandler,Default_Handler
-
-   .weak      OTG_HS_EP1_OUT_IRQHandler
-   .thumb_set OTG_HS_EP1_OUT_IRQHandler,Default_Handler
-
-   .weak      OTG_HS_EP1_IN_IRQHandler
-   .thumb_set OTG_HS_EP1_IN_IRQHandler,Default_Handler
-
-   .weak      OTG_HS_WKUP_IRQHandler
-   .thumb_set OTG_HS_WKUP_IRQHandler,Default_Handler
-
-   .weak      OTG_HS_IRQHandler
-   .thumb_set OTG_HS_IRQHandler,Default_Handler
-
-   .weak      DCMI_IRQHandler
-   .thumb_set DCMI_IRQHandler,Default_Handler
-
-   .weak      FPU_IRQHandler
-   .thumb_set FPU_IRQHandler,Default_Handler
-
-   .weak      SPI4_IRQHandler
-   .thumb_set SPI4_IRQHandler,Default_Handler
-
-   .weak      SAI1_IRQHandler
-   .thumb_set SAI1_IRQHandler,Default_Handler
-
-   .weak      SAI2_IRQHandler
-   .thumb_set SAI2_IRQHandler,Default_Handler
-
-   .weak      QUADSPI_IRQHandler
-   .thumb_set QUADSPI_IRQHandler,Default_Handler
-
-   .weak      CEC_IRQHandler
-   .thumb_set CEC_IRQHandler,Default_Handler
-
-   .weak      SPDIF_RX_IRQHandler
-   .thumb_set SPDIF_RX_IRQHandler,Default_Handler
-
-   .weak      FMPI2C1_EV_IRQHandler
-   .thumb_set FMPI2C1_EV_IRQHandler,Default_Handler
-
-   .weak      FMPI2C1_ER_IRQHandler
-   .thumb_set FMPI2C1_ER_IRQHandler,Default_Handler
+/**
+  ******************************************************************************
+  * @file      startup_stm32f446xx.s
+  * @author    MCD Application Team
+  * @brief     STM32F446xx Devices vector table for GCC based toolchains.
+  *            This module performs:
+  *                - Set the initial SP
+  *                - Set the initial PC == Reset_Handler,
+  *                - Set the vector table entries with the exceptions ISR address
+  *                - Branches to main in the C library (which eventually
+  *                  calls main()).
+  *            After Reset the Cortex-M4 processor is in Thread mode,
+  *            priority is Privileged, and the Stack is set to Main.
+  ******************************************************************************
+  * @attention
+  *
+  * Copyright (c) 2017 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.
+  *
+  ******************************************************************************
+  */
+
+  .syntax unified
+  .cpu cortex-m4
+  .fpu softvfp
+  .thumb
+
+.global  g_pfnVectors
+.global  Default_Handler
+
+/* start address for the initialization values of the .data section.
+defined in linker script */
+.word  _sidata
+/* start address for the .data section. defined in linker script */
+.word  _sdata
+/* end address for the .data section. defined in linker script */
+.word  _edata
+/* start address for the .bss section. defined in linker script */
+.word  _sbss
+/* end address for the .bss section. defined in linker script */
+.word  _ebss
+/* stack used for SystemInit_ExtMemCtl; always internal RAM used */
+
+/**
+ * @brief  This is the code that gets called when the processor first
+ *          starts execution following a reset event. Only the absolutely
+ *          necessary set is performed, after which the application
+ *          supplied main() routine is called.
+ * @param  None
+ * @retval : None
+*/
+
+    .section  .text.Reset_Handler
+  .weak  Reset_Handler
+  .type  Reset_Handler, %function
+Reset_Handler:
+  ldr   sp, =_estack      /* set stack pointer */
+
+/* Copy the data segment initializers from flash to SRAM */
+  ldr r0, =_sdata
+  ldr r1, =_edata
+  ldr r2, =_sidata
+  movs r3, #0
+  b LoopCopyDataInit
+
+CopyDataInit:
+  ldr r4, [r2, r3]
+  str r4, [r0, r3]
+  adds r3, r3, #4
+
+LoopCopyDataInit:
+  adds r4, r0, r3
+  cmp r4, r1
+  bcc CopyDataInit
+
+/* Zero fill the bss segment. */
+  ldr r2, =_sbss
+  ldr r4, =_ebss
+  movs r3, #0
+  b LoopFillZerobss
+
+FillZerobss:
+  str  r3, [r2]
+  adds r2, r2, #4
+
+LoopFillZerobss:
+  cmp r2, r4
+  bcc FillZerobss
+
+/* Call the clock system initialization function.*/
+  bl  SystemInit
+/* Call static constructors */
+    bl __libc_init_array
+/* Call the application's entry point.*/
+  bl  main
+  bx  lr
+.size  Reset_Handler, .-Reset_Handler
+
+/**
+ * @brief  This is the code that gets called when the processor receives an
+ *         unexpected interrupt.  This simply enters an infinite loop, preserving
+ *         the system state for examination by a debugger.
+ * @param  None
+ * @retval None
+*/
+    .section  .text.Default_Handler,"ax",%progbits
+Default_Handler:
+Infinite_Loop:
+  b  Infinite_Loop
+  .size  Default_Handler, .-Default_Handler
+/******************************************************************************
+*
+* The minimal vector table for a Cortex M3. Note that the proper constructs
+* must be placed on this to ensure that it ends up at physical address
+* 0x0000.0000.
+*
+*******************************************************************************/
+   .section  .isr_vector,"a",%progbits
+  .type  g_pfnVectors, %object
+  .size  g_pfnVectors, .-g_pfnVectors
+
+
+g_pfnVectors:
+  .word  _estack
+  .word  Reset_Handler
+
+  .word  NMI_Handler
+  .word  HardFault_Handler
+  .word  MemManage_Handler
+  .word  BusFault_Handler
+  .word  UsageFault_Handler
+  .word  0
+  .word  0
+  .word  0
+  .word  0
+  .word  SVC_Handler
+  .word  DebugMon_Handler
+  .word  0
+  .word  PendSV_Handler
+  .word  SysTick_Handler
+
+  /* External Interrupts */
+  .word     WWDG_IRQHandler                   /* Window WatchDog              */
+  .word     PVD_IRQHandler                    /* PVD through EXTI Line detection */
+  .word     TAMP_STAMP_IRQHandler             /* Tamper and TimeStamps through the EXTI line */
+  .word     RTC_WKUP_IRQHandler               /* RTC Wakeup through the EXTI line */
+  .word     FLASH_IRQHandler                  /* FLASH                        */
+  .word     RCC_IRQHandler                    /* RCC                          */
+  .word     EXTI0_IRQHandler                  /* EXTI Line0                   */
+  .word     EXTI1_IRQHandler                  /* EXTI Line1                   */
+  .word     EXTI2_IRQHandler                  /* EXTI Line2                   */
+  .word     EXTI3_IRQHandler                  /* EXTI Line3                   */
+  .word     EXTI4_IRQHandler                  /* EXTI Line4                   */
+  .word     DMA1_Stream0_IRQHandler           /* DMA1 Stream 0                */
+  .word     DMA1_Stream1_IRQHandler           /* DMA1 Stream 1                */
+  .word     DMA1_Stream2_IRQHandler           /* DMA1 Stream 2                */
+  .word     DMA1_Stream3_IRQHandler           /* DMA1 Stream 3                */
+  .word     DMA1_Stream4_IRQHandler           /* DMA1 Stream 4                */
+  .word     DMA1_Stream5_IRQHandler           /* DMA1 Stream 5                */
+  .word     DMA1_Stream6_IRQHandler           /* DMA1 Stream 6                */
+  .word     ADC_IRQHandler                    /* ADC1, ADC2 and ADC3s         */
+  .word     CAN1_TX_IRQHandler                /* CAN1 TX                      */
+  .word     CAN1_RX0_IRQHandler               /* CAN1 RX0                     */
+  .word     CAN1_RX1_IRQHandler               /* CAN1 RX1                     */
+  .word     CAN1_SCE_IRQHandler               /* CAN1 SCE                     */
+  .word     EXTI9_5_IRQHandler                /* External Line[9:5]s          */
+  .word     TIM1_BRK_TIM9_IRQHandler          /* TIM1 Break and TIM9          */
+  .word     TIM1_UP_TIM10_IRQHandler          /* TIM1 Update and TIM10        */
+  .word     TIM1_TRG_COM_TIM11_IRQHandler     /* TIM1 Trigger and Commutation and TIM11 */
+  .word     TIM1_CC_IRQHandler                /* TIM1 Capture Compare         */
+  .word     TIM2_IRQHandler                   /* TIM2                         */
+  .word     TIM3_IRQHandler                   /* TIM3                         */
+  .word     TIM4_IRQHandler                   /* TIM4                         */
+  .word     I2C1_EV_IRQHandler                /* I2C1 Event                   */
+  .word     I2C1_ER_IRQHandler                /* I2C1 Error                   */
+  .word     I2C2_EV_IRQHandler                /* I2C2 Event                   */
+  .word     I2C2_ER_IRQHandler                /* I2C2 Error                   */
+  .word     SPI1_IRQHandler                   /* SPI1                         */
+  .word     SPI2_IRQHandler                   /* SPI2                         */
+  .word     USART1_IRQHandler                 /* USART1                       */
+  .word     USART2_IRQHandler                 /* USART2                       */
+  .word     USART3_IRQHandler                 /* USART3                       */
+  .word     EXTI15_10_IRQHandler              /* External Line[15:10]s        */
+  .word     RTC_Alarm_IRQHandler              /* RTC Alarm (A and B) through EXTI Line */
+  .word     OTG_FS_WKUP_IRQHandler            /* USB OTG FS Wakeup through EXTI line */
+  .word     TIM8_BRK_TIM12_IRQHandler         /* TIM8 Break and TIM12         */
+  .word     TIM8_UP_TIM13_IRQHandler          /* TIM8 Update and TIM13        */
+  .word     TIM8_TRG_COM_TIM14_IRQHandler     /* TIM8 Trigger and Commutation and TIM14 */
+  .word     TIM8_CC_IRQHandler                /* TIM8 Capture Compare         */
+  .word     DMA1_Stream7_IRQHandler           /* DMA1 Stream7                 */
+  .word     FMC_IRQHandler                    /* FMC                          */
+  .word     SDIO_IRQHandler                   /* SDIO                         */
+  .word     TIM5_IRQHandler                   /* TIM5                         */
+  .word     SPI3_IRQHandler                   /* SPI3                         */
+  .word     UART4_IRQHandler                  /* UART4                        */
+  .word     UART5_IRQHandler                  /* UART5                        */
+  .word     TIM6_DAC_IRQHandler               /* TIM6 and DAC1&2 underrun errors */
+  .word     TIM7_IRQHandler                   /* TIM7                         */
+  .word     DMA2_Stream0_IRQHandler           /* DMA2 Stream 0                */
+  .word     DMA2_Stream1_IRQHandler           /* DMA2 Stream 1                */
+  .word     DMA2_Stream2_IRQHandler           /* DMA2 Stream 2                */
+  .word     DMA2_Stream3_IRQHandler           /* DMA2 Stream 3                */
+  .word     DMA2_Stream4_IRQHandler           /* DMA2 Stream 4                */
+  .word     0                                 /* Reserved                     */
+  .word     0                                 /* Reserved                     */
+  .word     CAN2_TX_IRQHandler                /* CAN2 TX                      */
+  .word     CAN2_RX0_IRQHandler               /* CAN2 RX0                     */
+  .word     CAN2_RX1_IRQHandler               /* CAN2 RX1                     */
+  .word     CAN2_SCE_IRQHandler               /* CAN2 SCE                     */
+  .word     OTG_FS_IRQHandler                 /* USB OTG FS                   */
+  .word     DMA2_Stream5_IRQHandler           /* DMA2 Stream 5                */
+  .word     DMA2_Stream6_IRQHandler           /* DMA2 Stream 6                */
+  .word     DMA2_Stream7_IRQHandler           /* DMA2 Stream 7                */
+  .word     USART6_IRQHandler                 /* USART6                       */
+  .word     I2C3_EV_IRQHandler                /* I2C3 event                   */
+  .word     I2C3_ER_IRQHandler                /* I2C3 error                   */
+  .word     OTG_HS_EP1_OUT_IRQHandler         /* USB OTG HS End Point 1 Out   */
+  .word     OTG_HS_EP1_IN_IRQHandler          /* USB OTG HS End Point 1 In    */
+  .word     OTG_HS_WKUP_IRQHandler            /* USB OTG HS Wakeup through EXTI */
+  .word     OTG_HS_IRQHandler                 /* USB OTG HS                   */
+  .word     DCMI_IRQHandler                   /* DCMI                         */
+  .word     0                                 /* Reserved                     */
+  .word     0                                 /* Reserved                     */
+  .word     FPU_IRQHandler                    /* FPU                          */
+  .word     0                                 /* Reserved                     */
+  .word     0                                 /* Reserved                     */
+  .word     SPI4_IRQHandler                   /* SPI4                         */
+  .word     0                                 /* Reserved                     */
+  .word     0                                 /* Reserved                     */
+  .word     SAI1_IRQHandler                   /* SAI1                         */
+  .word     0                                 /* Reserved                     */
+  .word     0                                 /* Reserved                     */
+  .word     0                                 /* Reserved                     */
+  .word     SAI2_IRQHandler                   /* SAI2                         */
+  .word     QUADSPI_IRQHandler                /* QuadSPI                      */
+  .word     CEC_IRQHandler                    /* CEC                          */
+  .word     SPDIF_RX_IRQHandler               /* SPDIF RX                     */
+  .word     FMPI2C1_EV_IRQHandler          /* FMPI2C 1 Event               */
+  .word     FMPI2C1_ER_IRQHandler          /* FMPI2C 1 Error               */
+
+/*******************************************************************************
+*
+* Provide weak aliases for each Exception handler to the Default_Handler.
+* As they are weak aliases, any function with the same name will override
+* this definition.
+*
+*******************************************************************************/
+   .weak      NMI_Handler
+   .thumb_set NMI_Handler,Default_Handler
+
+   .weak      HardFault_Handler
+   .thumb_set HardFault_Handler,Default_Handler
+
+   .weak      MemManage_Handler
+   .thumb_set MemManage_Handler,Default_Handler
+
+   .weak      BusFault_Handler
+   .thumb_set BusFault_Handler,Default_Handler
+
+   .weak      UsageFault_Handler
+   .thumb_set UsageFault_Handler,Default_Handler
+
+   .weak      SVC_Handler
+   .thumb_set SVC_Handler,Default_Handler
+
+   .weak      DebugMon_Handler
+   .thumb_set DebugMon_Handler,Default_Handler
+
+   .weak      PendSV_Handler
+   .thumb_set PendSV_Handler,Default_Handler
+
+   .weak      SysTick_Handler
+   .thumb_set SysTick_Handler,Default_Handler
+
+   .weak      WWDG_IRQHandler
+   .thumb_set WWDG_IRQHandler,Default_Handler
+
+   .weak      PVD_IRQHandler
+   .thumb_set PVD_IRQHandler,Default_Handler
+
+   .weak      TAMP_STAMP_IRQHandler
+   .thumb_set TAMP_STAMP_IRQHandler,Default_Handler
+
+   .weak      RTC_WKUP_IRQHandler
+   .thumb_set RTC_WKUP_IRQHandler,Default_Handler
+
+   .weak      FLASH_IRQHandler
+   .thumb_set FLASH_IRQHandler,Default_Handler
+
+   .weak      RCC_IRQHandler
+   .thumb_set RCC_IRQHandler,Default_Handler
+
+   .weak      EXTI0_IRQHandler
+   .thumb_set EXTI0_IRQHandler,Default_Handler
+
+   .weak      EXTI1_IRQHandler
+   .thumb_set EXTI1_IRQHandler,Default_Handler
+
+   .weak      EXTI2_IRQHandler
+   .thumb_set EXTI2_IRQHandler,Default_Handler
+
+   .weak      EXTI3_IRQHandler
+   .thumb_set EXTI3_IRQHandler,Default_Handler
+
+   .weak      EXTI4_IRQHandler
+   .thumb_set EXTI4_IRQHandler,Default_Handler
+
+   .weak      DMA1_Stream0_IRQHandler
+   .thumb_set DMA1_Stream0_IRQHandler,Default_Handler
+
+   .weak      DMA1_Stream1_IRQHandler
+   .thumb_set DMA1_Stream1_IRQHandler,Default_Handler
+
+   .weak      DMA1_Stream2_IRQHandler
+   .thumb_set DMA1_Stream2_IRQHandler,Default_Handler
+
+   .weak      DMA1_Stream3_IRQHandler
+   .thumb_set DMA1_Stream3_IRQHandler,Default_Handler
+
+   .weak      DMA1_Stream4_IRQHandler
+   .thumb_set DMA1_Stream4_IRQHandler,Default_Handler
+
+   .weak      DMA1_Stream5_IRQHandler
+   .thumb_set DMA1_Stream5_IRQHandler,Default_Handler
+
+   .weak      DMA1_Stream6_IRQHandler
+   .thumb_set DMA1_Stream6_IRQHandler,Default_Handler
+
+   .weak      ADC_IRQHandler
+   .thumb_set ADC_IRQHandler,Default_Handler
+
+   .weak      CAN1_TX_IRQHandler
+   .thumb_set CAN1_TX_IRQHandler,Default_Handler
+
+   .weak      CAN1_RX0_IRQHandler
+   .thumb_set CAN1_RX0_IRQHandler,Default_Handler
+
+   .weak      CAN1_RX1_IRQHandler
+   .thumb_set CAN1_RX1_IRQHandler,Default_Handler
+
+   .weak      CAN1_SCE_IRQHandler
+   .thumb_set CAN1_SCE_IRQHandler,Default_Handler
+
+   .weak      EXTI9_5_IRQHandler
+   .thumb_set EXTI9_5_IRQHandler,Default_Handler
+
+   .weak      TIM1_BRK_TIM9_IRQHandler
+   .thumb_set TIM1_BRK_TIM9_IRQHandler,Default_Handler
+
+   .weak      TIM1_UP_TIM10_IRQHandler
+   .thumb_set TIM1_UP_TIM10_IRQHandler,Default_Handler
+
+   .weak      TIM1_TRG_COM_TIM11_IRQHandler
+   .thumb_set TIM1_TRG_COM_TIM11_IRQHandler,Default_Handler
+
+   .weak      TIM1_CC_IRQHandler
+   .thumb_set TIM1_CC_IRQHandler,Default_Handler
+
+   .weak      TIM2_IRQHandler
+   .thumb_set TIM2_IRQHandler,Default_Handler
+
+   .weak      TIM3_IRQHandler
+   .thumb_set TIM3_IRQHandler,Default_Handler
+
+   .weak      TIM4_IRQHandler
+   .thumb_set TIM4_IRQHandler,Default_Handler
+
+   .weak      I2C1_EV_IRQHandler
+   .thumb_set I2C1_EV_IRQHandler,Default_Handler
+
+   .weak      I2C1_ER_IRQHandler
+   .thumb_set I2C1_ER_IRQHandler,Default_Handler
+
+   .weak      I2C2_EV_IRQHandler
+   .thumb_set I2C2_EV_IRQHandler,Default_Handler
+
+   .weak      I2C2_ER_IRQHandler
+   .thumb_set I2C2_ER_IRQHandler,Default_Handler
+
+   .weak      SPI1_IRQHandler
+   .thumb_set SPI1_IRQHandler,Default_Handler
+
+   .weak      SPI2_IRQHandler
+   .thumb_set SPI2_IRQHandler,Default_Handler
+
+   .weak      USART1_IRQHandler
+   .thumb_set USART1_IRQHandler,Default_Handler
+
+   .weak      USART2_IRQHandler
+   .thumb_set USART2_IRQHandler,Default_Handler
+
+   .weak      USART3_IRQHandler
+   .thumb_set USART3_IRQHandler,Default_Handler
+
+   .weak      EXTI15_10_IRQHandler
+   .thumb_set EXTI15_10_IRQHandler,Default_Handler
+
+   .weak      RTC_Alarm_IRQHandler
+   .thumb_set RTC_Alarm_IRQHandler,Default_Handler
+
+   .weak      OTG_FS_WKUP_IRQHandler
+   .thumb_set OTG_FS_WKUP_IRQHandler,Default_Handler
+
+   .weak      TIM8_BRK_TIM12_IRQHandler
+   .thumb_set TIM8_BRK_TIM12_IRQHandler,Default_Handler
+
+   .weak      TIM8_UP_TIM13_IRQHandler
+   .thumb_set TIM8_UP_TIM13_IRQHandler,Default_Handler
+
+   .weak      TIM8_TRG_COM_TIM14_IRQHandler
+   .thumb_set TIM8_TRG_COM_TIM14_IRQHandler,Default_Handler
+
+   .weak      TIM8_CC_IRQHandler
+   .thumb_set TIM8_CC_IRQHandler,Default_Handler
+
+   .weak      DMA1_Stream7_IRQHandler
+   .thumb_set DMA1_Stream7_IRQHandler,Default_Handler
+
+   .weak      FMC_IRQHandler
+   .thumb_set FMC_IRQHandler,Default_Handler
+
+   .weak      SDIO_IRQHandler
+   .thumb_set SDIO_IRQHandler,Default_Handler
+
+   .weak      TIM5_IRQHandler
+   .thumb_set TIM5_IRQHandler,Default_Handler
+
+   .weak      SPI3_IRQHandler
+   .thumb_set SPI3_IRQHandler,Default_Handler
+
+   .weak      UART4_IRQHandler
+   .thumb_set UART4_IRQHandler,Default_Handler
+
+   .weak      UART5_IRQHandler
+   .thumb_set UART5_IRQHandler,Default_Handler
+
+   .weak      TIM6_DAC_IRQHandler
+   .thumb_set TIM6_DAC_IRQHandler,Default_Handler
+
+   .weak      TIM7_IRQHandler
+   .thumb_set TIM7_IRQHandler,Default_Handler
+
+   .weak      DMA2_Stream0_IRQHandler
+   .thumb_set DMA2_Stream0_IRQHandler,Default_Handler
+
+   .weak      DMA2_Stream1_IRQHandler
+   .thumb_set DMA2_Stream1_IRQHandler,Default_Handler
+
+   .weak      DMA2_Stream2_IRQHandler
+   .thumb_set DMA2_Stream2_IRQHandler,Default_Handler
+
+   .weak      DMA2_Stream3_IRQHandler
+   .thumb_set DMA2_Stream3_IRQHandler,Default_Handler
+
+   .weak      DMA2_Stream4_IRQHandler
+   .thumb_set DMA2_Stream4_IRQHandler,Default_Handler
+
+   .weak      CAN2_TX_IRQHandler
+   .thumb_set CAN2_TX_IRQHandler,Default_Handler
+
+   .weak      CAN2_RX0_IRQHandler
+   .thumb_set CAN2_RX0_IRQHandler,Default_Handler
+
+   .weak      CAN2_RX1_IRQHandler
+   .thumb_set CAN2_RX1_IRQHandler,Default_Handler
+
+   .weak      CAN2_SCE_IRQHandler
+   .thumb_set CAN2_SCE_IRQHandler,Default_Handler
+
+   .weak      OTG_FS_IRQHandler
+   .thumb_set OTG_FS_IRQHandler,Default_Handler
+
+   .weak      DMA2_Stream5_IRQHandler
+   .thumb_set DMA2_Stream5_IRQHandler,Default_Handler
+
+   .weak      DMA2_Stream6_IRQHandler
+   .thumb_set DMA2_Stream6_IRQHandler,Default_Handler
+
+   .weak      DMA2_Stream7_IRQHandler
+   .thumb_set DMA2_Stream7_IRQHandler,Default_Handler
+
+   .weak      USART6_IRQHandler
+   .thumb_set USART6_IRQHandler,Default_Handler
+
+   .weak      I2C3_EV_IRQHandler
+   .thumb_set I2C3_EV_IRQHandler,Default_Handler
+
+   .weak      I2C3_ER_IRQHandler
+   .thumb_set I2C3_ER_IRQHandler,Default_Handler
+
+   .weak      OTG_HS_EP1_OUT_IRQHandler
+   .thumb_set OTG_HS_EP1_OUT_IRQHandler,Default_Handler
+
+   .weak      OTG_HS_EP1_IN_IRQHandler
+   .thumb_set OTG_HS_EP1_IN_IRQHandler,Default_Handler
+
+   .weak      OTG_HS_WKUP_IRQHandler
+   .thumb_set OTG_HS_WKUP_IRQHandler,Default_Handler
+
+   .weak      OTG_HS_IRQHandler
+   .thumb_set OTG_HS_IRQHandler,Default_Handler
+
+   .weak      DCMI_IRQHandler
+   .thumb_set DCMI_IRQHandler,Default_Handler
+
+   .weak      FPU_IRQHandler
+   .thumb_set FPU_IRQHandler,Default_Handler
+
+   .weak      SPI4_IRQHandler
+   .thumb_set SPI4_IRQHandler,Default_Handler
+
+   .weak      SAI1_IRQHandler
+   .thumb_set SAI1_IRQHandler,Default_Handler
+
+   .weak      SAI2_IRQHandler
+   .thumb_set SAI2_IRQHandler,Default_Handler
+
+   .weak      QUADSPI_IRQHandler
+   .thumb_set QUADSPI_IRQHandler,Default_Handler
+
+   .weak      CEC_IRQHandler
+   .thumb_set CEC_IRQHandler,Default_Handler
+
+   .weak      SPDIF_RX_IRQHandler
+   .thumb_set SPDIF_RX_IRQHandler,Default_Handler
+
+   .weak      FMPI2C1_EV_IRQHandler
+   .thumb_set FMPI2C1_EV_IRQHandler,Default_Handler
+
+   .weak      FMPI2C1_ER_IRQHandler
+   .thumb_set FMPI2C1_ER_IRQHandler,Default_Handler
diff --git a/soccer_firmware/STM32F446RCTX_FLASH.ld b/soccer_firmware/STM32F446RCTX_FLASH.ld
index 7eecebdef..1f789b9bd 100644
--- a/soccer_firmware/STM32F446RCTX_FLASH.ld
+++ b/soccer_firmware/STM32F446RCTX_FLASH.ld
@@ -6,8 +6,8 @@
 ** @author      : Auto-generated by STM32CubeIDE
 **
 ** @brief       : Linker script for STM32F446RCTx Device from STM32F4 series
-**                      256KBytes FLASH
-**                      128KBytes RAM
+**                      256Kbytes FLASH
+**                      128Kbytes RAM
 **
 **                Set heap size, stack size and stack location according
 **                to application requirements.
@@ -22,7 +22,7 @@
 ******************************************************************************
 ** @attention
 **
-** Copyright (c) 2024 STMicroelectronics.
+** Copyright (c) 2023 STMicroelectronics.
 ** All rights reserved.
 **
 ** This software is licensed under terms that can be found in the LICENSE file
diff --git a/soccer_firmware/STM32F446RCTX_RAM.ld b/soccer_firmware/STM32F446RCTX_RAM.ld
index db88c3c93..b14d53046 100644
--- a/soccer_firmware/STM32F446RCTX_RAM.ld
+++ b/soccer_firmware/STM32F446RCTX_RAM.ld
@@ -6,8 +6,8 @@
 ** @author      : Auto-generated by STM32CubeIDE
 **
 ** @brief       : Linker script for STM32F446RCTx Device from STM32F4 series
-**                      256KBytes FLASH
-**                      128KBytes RAM
+**                      256Kbytes FLASH
+**                      128Kbytes RAM
 **
 **                Set heap size, stack size and stack location according
 **                to application requirements.
@@ -22,7 +22,7 @@
 ******************************************************************************
 ** @attention
 **
-** Copyright (c) 2024 STMicroelectronics.
+** Copyright (c) 2023 STMicroelectronics.
 ** All rights reserved.
 **
 ** This software is licensed under terms that can be found in the LICENSE file
diff --git a/soccer_firmware/soccer_firmware Debug.launch b/soccer_firmware/soccer_firmware Debug.launch
index cfb474dfd..301892967 100644
--- a/soccer_firmware/soccer_firmware Debug.launch	
+++ b/soccer_firmware/soccer_firmware Debug.launch	
@@ -1,78 +1,78 @@
 <?xml version="1.0" encoding="UTF-8" standalone="no"?>
 <launchConfiguration type="com.st.stm32cube.ide.mcu.debug.launch.launchConfigurationType">
-    <stringAttribute key="com.st.stm32cube.ide.mcu.debug.launch.access_port_id" value="0" />
-    <stringAttribute key="com.st.stm32cube.ide.mcu.debug.launch.cubeprog_external_loaders" value="[]" />
-    <booleanAttribute key="com.st.stm32cube.ide.mcu.debug.launch.enable_live_expr" value="true" />
-    <booleanAttribute key="com.st.stm32cube.ide.mcu.debug.launch.enable_swv" value="false" />
-    <intAttribute key="com.st.stm32cube.ide.mcu.debug.launch.formatVersion" value="2" />
-    <stringAttribute key="com.st.stm32cube.ide.mcu.debug.launch.ip_address_local" value="localhost" />
-    <booleanAttribute key="com.st.stm32cube.ide.mcu.debug.launch.limit_swo_clock.enabled" value="false" />
-    <stringAttribute key="com.st.stm32cube.ide.mcu.debug.launch.limit_swo_clock.value" value="" />
-    <stringAttribute key="com.st.stm32cube.ide.mcu.debug.launch.loadList" value='{"fItems":[{"fIsFromMainTab":true,"fPath":"Debug/soccer_firmware.elf","fProjectName":"soccer_firmware","fPerformBuild":true,"fDownload":true,"fLoadSymbols":true}]}' />
-    <stringAttribute key="com.st.stm32cube.ide.mcu.debug.launch.override_start_address_mode" value="default" />
-    <stringAttribute key="com.st.stm32cube.ide.mcu.debug.launch.remoteCommand" value="target remote" />
-    <booleanAttribute key="com.st.stm32cube.ide.mcu.debug.launch.startServer" value="true" />
-    <booleanAttribute key="com.st.stm32cube.ide.mcu.debug.launch.startuptab.exception.divby0" value="true" />
-    <booleanAttribute key="com.st.stm32cube.ide.mcu.debug.launch.startuptab.exception.unaligned" value="false" />
-    <booleanAttribute key="com.st.stm32cube.ide.mcu.debug.launch.startuptab.haltonexception" value="true" />
-    <booleanAttribute key="com.st.stm32cube.ide.mcu.debug.launch.swd_mode" value="true" />
-    <stringAttribute key="com.st.stm32cube.ide.mcu.debug.launch.swv_port" value="61235" />
-    <stringAttribute key="com.st.stm32cube.ide.mcu.debug.launch.swv_trace_hclk" value="16000000" />
-    <booleanAttribute key="com.st.stm32cube.ide.mcu.debug.launch.useRemoteTarget" value="true" />
-    <stringAttribute key="com.st.stm32cube.ide.mcu.debug.launch.vector_table" value="" />
-    <booleanAttribute key="com.st.stm32cube.ide.mcu.debug.launch.verify_flash_download" value="true" />
-    <booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.cti_allow_halt" value="false" />
-    <booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.cti_signal_halt" value="false" />
-    <booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.enable_logging" value="false" />
-    <booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.enable_max_halt_delay" value="false" />
-    <booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.enable_shared_stlink" value="false" />
-    <stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.frequency" value="0" />
-    <booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.halt_all_on_reset" value="false" />
-    <stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.log_file" value="/home/nam/catkin_ws/src/soccerbot/soccer_firmware/Debug/st-link_gdbserver_log.txt" />
-    <stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.low_power_debug" value="none" />
-    <stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.max_halt_delay" value="2" />
-    <stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.reset_strategy" value="connect_under_reset" />
-    <booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.stlink_check_serial_number" value="false" />
-    <stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.stlink_txt_serial_number" value="" />
-    <stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.watchdog_config" value="none" />
-    <booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlinkenable_rtos" value="false" />
-    <stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlinkrestart_configurations" value='{"fVersion":1,"fItems":[{"fDisplayName":"Reset","fIsSuppressible":false,"fResetAttribute":"Software system reset","fResetStrategies":[{"fDisplayName":"Software system reset","fLaunchAttribute":"system_reset","fGdbCommands":["monitor reset\n"],"fCmdOptions":["-g"]},{"fDisplayName":"Hardware reset","fLaunchAttribute":"hardware_reset","fGdbCommands":["monitor reset hardware\n"],"fCmdOptions":["-g"]},{"fDisplayName":"Core reset","fLaunchAttribute":"core_reset","fGdbCommands":["monitor reset core\n"],"fCmdOptions":["-g"]},{"fDisplayName":"None","fLaunchAttribute":"no_reset","fGdbCommands":[],"fCmdOptions":["-g"]}],"fGdbCommandGroup":{"name":"Additional commands","commands":[]},"fStartApplication":true}]}' />
-    <booleanAttribute key="com.st.stm32cube.ide.mcu.rtosproxy.enableRtosProxy" value="false" />
-    <stringAttribute key="com.st.stm32cube.ide.mcu.rtosproxy.rtosProxyCustomProperties" value="" />
-    <stringAttribute key="com.st.stm32cube.ide.mcu.rtosproxy.rtosProxyDriver" value="threadx" />
-    <booleanAttribute key="com.st.stm32cube.ide.mcu.rtosproxy.rtosProxyDriverAuto" value="false" />
-    <stringAttribute key="com.st.stm32cube.ide.mcu.rtosproxy.rtosProxyDriverPort" value="cortex_m0" />
-    <intAttribute key="com.st.stm32cube.ide.mcu.rtosproxy.rtosProxyPort" value="60000" />
-    <booleanAttribute key="org.eclipse.cdt.debug.gdbjtag.core.doHalt" value="false" />
-    <booleanAttribute key="org.eclipse.cdt.debug.gdbjtag.core.doReset" value="false" />
-    <stringAttribute key="org.eclipse.cdt.debug.gdbjtag.core.initCommands" value="" />
-    <stringAttribute key="org.eclipse.cdt.debug.gdbjtag.core.ipAddress" value="localhost" />
-    <stringAttribute key="org.eclipse.cdt.debug.gdbjtag.core.jtagDeviceId" value="com.st.stm32cube.ide.mcu.debug.stlink" />
-    <stringAttribute key="org.eclipse.cdt.debug.gdbjtag.core.pcRegister" value="" />
-    <intAttribute key="org.eclipse.cdt.debug.gdbjtag.core.portNumber" value="61234" />
-    <stringAttribute key="org.eclipse.cdt.debug.gdbjtag.core.runCommands" value="" />
-    <booleanAttribute key="org.eclipse.cdt.debug.gdbjtag.core.setPcRegister" value="false" />
-    <booleanAttribute key="org.eclipse.cdt.debug.gdbjtag.core.setResume" value="true" />
-    <booleanAttribute key="org.eclipse.cdt.debug.gdbjtag.core.setStopAt" value="true" />
-    <stringAttribute key="org.eclipse.cdt.debug.gdbjtag.core.stopAt" value="main" />
-    <stringAttribute key="org.eclipse.cdt.dsf.gdb.DEBUG_NAME" value="arm-none-eabi-gdb" />
-    <booleanAttribute key="org.eclipse.cdt.dsf.gdb.NON_STOP" value="false" />
-    <booleanAttribute key="org.eclipse.cdt.dsf.gdb.UPDATE_THREADLIST_ON_SUSPEND" value="false" />
-    <intAttribute key="org.eclipse.cdt.launch.ATTR_BUILD_BEFORE_LAUNCH_ATTR" value="2" />
-    <stringAttribute key="org.eclipse.cdt.launch.COREFILE_PATH" value="" />
-    <stringAttribute key="org.eclipse.cdt.launch.DEBUGGER_START_MODE" value="remote" />
-    <booleanAttribute key="org.eclipse.cdt.launch.DEBUGGER_STOP_AT_MAIN" value="true" />
-    <stringAttribute key="org.eclipse.cdt.launch.DEBUGGER_STOP_AT_MAIN_SYMBOL" value="main" />
-    <stringAttribute key="org.eclipse.cdt.launch.PROGRAM_NAME" value="Debug/soccer_firmware.elf" />
-    <stringAttribute key="org.eclipse.cdt.launch.PROJECT_ATTR" value="soccer_firmware" />
-    <booleanAttribute key="org.eclipse.cdt.launch.PROJECT_BUILD_CONFIG_AUTO_ATTR" value="true" />
-    <stringAttribute key="org.eclipse.cdt.launch.PROJECT_BUILD_CONFIG_ID_ATTR" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug.683473154" />
+    <stringAttribute key="com.st.stm32cube.ide.mcu.debug.launch.access_port_id" value="0"/>
+    <stringAttribute key="com.st.stm32cube.ide.mcu.debug.launch.cubeprog_external_loaders" value="[]"/>
+    <booleanAttribute key="com.st.stm32cube.ide.mcu.debug.launch.enable_live_expr" value="true"/>
+    <booleanAttribute key="com.st.stm32cube.ide.mcu.debug.launch.enable_swv" value="false"/>
+    <intAttribute key="com.st.stm32cube.ide.mcu.debug.launch.formatVersion" value="2"/>
+    <stringAttribute key="com.st.stm32cube.ide.mcu.debug.launch.ip_address_local" value="localhost"/>
+    <booleanAttribute key="com.st.stm32cube.ide.mcu.debug.launch.limit_swo_clock.enabled" value="false"/>
+    <stringAttribute key="com.st.stm32cube.ide.mcu.debug.launch.limit_swo_clock.value" value=""/>
+    <stringAttribute key="com.st.stm32cube.ide.mcu.debug.launch.loadList" value="{&quot;fItems&quot;:[{&quot;fIsFromMainTab&quot;:true,&quot;fPath&quot;:&quot;Debug/soccer_firmware.elf&quot;,&quot;fProjectName&quot;:&quot;soccer_firmware&quot;,&quot;fPerformBuild&quot;:true,&quot;fDownload&quot;:true,&quot;fLoadSymbols&quot;:true}]}"/>
+    <stringAttribute key="com.st.stm32cube.ide.mcu.debug.launch.override_start_address_mode" value="default"/>
+    <stringAttribute key="com.st.stm32cube.ide.mcu.debug.launch.remoteCommand" value="target remote"/>
+    <booleanAttribute key="com.st.stm32cube.ide.mcu.debug.launch.startServer" value="true"/>
+    <booleanAttribute key="com.st.stm32cube.ide.mcu.debug.launch.startuptab.exception.divby0" value="true"/>
+    <booleanAttribute key="com.st.stm32cube.ide.mcu.debug.launch.startuptab.exception.unaligned" value="false"/>
+    <booleanAttribute key="com.st.stm32cube.ide.mcu.debug.launch.startuptab.haltonexception" value="true"/>
+    <booleanAttribute key="com.st.stm32cube.ide.mcu.debug.launch.swd_mode" value="true"/>
+    <stringAttribute key="com.st.stm32cube.ide.mcu.debug.launch.swv_port" value="61235"/>
+    <stringAttribute key="com.st.stm32cube.ide.mcu.debug.launch.swv_trace_hclk" value="16000000"/>
+    <booleanAttribute key="com.st.stm32cube.ide.mcu.debug.launch.useRemoteTarget" value="true"/>
+    <stringAttribute key="com.st.stm32cube.ide.mcu.debug.launch.vector_table" value=""/>
+    <booleanAttribute key="com.st.stm32cube.ide.mcu.debug.launch.verify_flash_download" value="true"/>
+    <booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.cti_allow_halt" value="false"/>
+    <booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.cti_signal_halt" value="false"/>
+    <booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.enable_logging" value="false"/>
+    <booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.enable_max_halt_delay" value="false"/>
+    <booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.enable_shared_stlink" value="false"/>
+    <stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.frequency" value="0"/>
+    <booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.halt_all_on_reset" value="false"/>
+    <stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.log_file" value="/home/nam/catkin_ws/src/soccerbot/soccer_firmware/Debug/st-link_gdbserver_log.txt"/>
+    <stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.low_power_debug" value="none"/>
+    <stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.max_halt_delay" value="2"/>
+    <stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.reset_strategy" value="connect_under_reset"/>
+    <booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.stlink_check_serial_number" value="false"/>
+    <stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.stlink_txt_serial_number" value=""/>
+    <stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.watchdog_config" value="none"/>
+    <booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlinkenable_rtos" value="false"/>
+    <stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlinkrestart_configurations" value="{&quot;fVersion&quot;:1,&quot;fItems&quot;:[{&quot;fDisplayName&quot;:&quot;Reset&quot;,&quot;fIsSuppressible&quot;:false,&quot;fResetAttribute&quot;:&quot;Software system reset&quot;,&quot;fResetStrategies&quot;:[{&quot;fDisplayName&quot;:&quot;Software system reset&quot;,&quot;fLaunchAttribute&quot;:&quot;system_reset&quot;,&quot;fGdbCommands&quot;:[&quot;monitor reset\n&quot;],&quot;fCmdOptions&quot;:[&quot;-g&quot;]},{&quot;fDisplayName&quot;:&quot;Hardware reset&quot;,&quot;fLaunchAttribute&quot;:&quot;hardware_reset&quot;,&quot;fGdbCommands&quot;:[&quot;monitor reset hardware\n&quot;],&quot;fCmdOptions&quot;:[&quot;-g&quot;]},{&quot;fDisplayName&quot;:&quot;Core reset&quot;,&quot;fLaunchAttribute&quot;:&quot;core_reset&quot;,&quot;fGdbCommands&quot;:[&quot;monitor reset core\n&quot;],&quot;fCmdOptions&quot;:[&quot;-g&quot;]},{&quot;fDisplayName&quot;:&quot;None&quot;,&quot;fLaunchAttribute&quot;:&quot;no_reset&quot;,&quot;fGdbCommands&quot;:[],&quot;fCmdOptions&quot;:[&quot;-g&quot;]}],&quot;fGdbCommandGroup&quot;:{&quot;name&quot;:&quot;Additional commands&quot;,&quot;commands&quot;:[]},&quot;fStartApplication&quot;:true}]}"/>
+    <booleanAttribute key="com.st.stm32cube.ide.mcu.rtosproxy.enableRtosProxy" value="false"/>
+    <stringAttribute key="com.st.stm32cube.ide.mcu.rtosproxy.rtosProxyCustomProperties" value=""/>
+    <stringAttribute key="com.st.stm32cube.ide.mcu.rtosproxy.rtosProxyDriver" value="threadx"/>
+    <booleanAttribute key="com.st.stm32cube.ide.mcu.rtosproxy.rtosProxyDriverAuto" value="false"/>
+    <stringAttribute key="com.st.stm32cube.ide.mcu.rtosproxy.rtosProxyDriverPort" value="cortex_m0"/>
+    <intAttribute key="com.st.stm32cube.ide.mcu.rtosproxy.rtosProxyPort" value="60000"/>
+    <booleanAttribute key="org.eclipse.cdt.debug.gdbjtag.core.doHalt" value="false"/>
+    <booleanAttribute key="org.eclipse.cdt.debug.gdbjtag.core.doReset" value="false"/>
+    <stringAttribute key="org.eclipse.cdt.debug.gdbjtag.core.initCommands" value=""/>
+    <stringAttribute key="org.eclipse.cdt.debug.gdbjtag.core.ipAddress" value="localhost"/>
+    <stringAttribute key="org.eclipse.cdt.debug.gdbjtag.core.jtagDeviceId" value="com.st.stm32cube.ide.mcu.debug.stlink"/>
+    <stringAttribute key="org.eclipse.cdt.debug.gdbjtag.core.pcRegister" value=""/>
+    <intAttribute key="org.eclipse.cdt.debug.gdbjtag.core.portNumber" value="61234"/>
+    <stringAttribute key="org.eclipse.cdt.debug.gdbjtag.core.runCommands" value=""/>
+    <booleanAttribute key="org.eclipse.cdt.debug.gdbjtag.core.setPcRegister" value="false"/>
+    <booleanAttribute key="org.eclipse.cdt.debug.gdbjtag.core.setResume" value="true"/>
+    <booleanAttribute key="org.eclipse.cdt.debug.gdbjtag.core.setStopAt" value="true"/>
+    <stringAttribute key="org.eclipse.cdt.debug.gdbjtag.core.stopAt" value="main"/>
+    <stringAttribute key="org.eclipse.cdt.dsf.gdb.DEBUG_NAME" value="arm-none-eabi-gdb"/>
+    <booleanAttribute key="org.eclipse.cdt.dsf.gdb.NON_STOP" value="false"/>
+    <booleanAttribute key="org.eclipse.cdt.dsf.gdb.UPDATE_THREADLIST_ON_SUSPEND" value="false"/>
+    <intAttribute key="org.eclipse.cdt.launch.ATTR_BUILD_BEFORE_LAUNCH_ATTR" value="2"/>
+    <stringAttribute key="org.eclipse.cdt.launch.COREFILE_PATH" value=""/>
+    <stringAttribute key="org.eclipse.cdt.launch.DEBUGGER_START_MODE" value="remote"/>
+    <booleanAttribute key="org.eclipse.cdt.launch.DEBUGGER_STOP_AT_MAIN" value="true"/>
+    <stringAttribute key="org.eclipse.cdt.launch.DEBUGGER_STOP_AT_MAIN_SYMBOL" value="main"/>
+    <stringAttribute key="org.eclipse.cdt.launch.PROGRAM_NAME" value="Debug/soccer_firmware.elf"/>
+    <stringAttribute key="org.eclipse.cdt.launch.PROJECT_ATTR" value="soccer_firmware"/>
+    <booleanAttribute key="org.eclipse.cdt.launch.PROJECT_BUILD_CONFIG_AUTO_ATTR" value="true"/>
+    <stringAttribute key="org.eclipse.cdt.launch.PROJECT_BUILD_CONFIG_ID_ATTR" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug.683473154"/>
     <listAttribute key="org.eclipse.debug.core.MAPPED_RESOURCE_PATHS">
-        <listEntry value="/soccer_firmware" />
+        <listEntry value="/soccer_firmware"/>
     </listAttribute>
     <listAttribute key="org.eclipse.debug.core.MAPPED_RESOURCE_TYPES">
-        <listEntry value="4" />
+        <listEntry value="4"/>
     </listAttribute>
-    <stringAttribute key="org.eclipse.dsf.launch.MEMORY_BLOCKS" value='&lt;?xml version="1.0" encoding="UTF-8" standalone="no"?&gt;&lt;memoryBlockExpressionList context="reserved-for-future-use"/&gt;' />
-    <stringAttribute key="process_factory_id" value="com.st.stm32cube.ide.mcu.debug.launch.HardwareDebugProcessFactory" />
+    <stringAttribute key="org.eclipse.dsf.launch.MEMORY_BLOCKS" value="&lt;?xml version=&quot;1.0&quot; encoding=&quot;UTF-8&quot; standalone=&quot;no&quot;?&gt;&lt;memoryBlockExpressionList context=&quot;reserved-for-future-use&quot;/&gt;"/>
+    <stringAttribute key="process_factory_id" value="com.st.stm32cube.ide.mcu.debug.launch.HardwareDebugProcessFactory"/>
 </launchConfiguration>
diff --git a/soccer_firmware/soccer_firmware.ioc b/soccer_firmware/soccer_firmware.ioc
index 0e88dc842..6eee3f781 100644
--- a/soccer_firmware/soccer_firmware.ioc
+++ b/soccer_firmware/soccer_firmware.ioc
@@ -191,8 +191,8 @@ Mcu.PinsNb=29
 Mcu.ThirdPartyNb=0
 Mcu.UserConstants=
 Mcu.UserName=STM32F446RCTx
-MxCube.Version=6.10.0
-MxDb.Version=DB.6.0.100
+MxCube.Version=6.8.1
+MxDb.Version=DB.6.0.81
 NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
 NVIC.DMA1_Stream0_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true
 NVIC.DMA1_Stream1_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true
@@ -307,7 +307,7 @@ ProjectManager.LastFirmware=true
 ProjectManager.LibraryCopy=1
 ProjectManager.MainLocation=Core/Src
 ProjectManager.NoMain=false
-ProjectManager.PreviousToolchain=STM32CubeIDE
+ProjectManager.PreviousToolchain=
 ProjectManager.ProjectBuild=false
 ProjectManager.ProjectFileName=soccer_firmware.ioc
 ProjectManager.ProjectName=soccer_firmware
@@ -316,8 +316,6 @@ ProjectManager.RegisterCallBack=
 ProjectManager.StackSize=0x400
 ProjectManager.TargetToolchain=STM32CubeIDE
 ProjectManager.ToolChainLocation=
-ProjectManager.UAScriptAfterPath=
-ProjectManager.UAScriptBeforePath=
 ProjectManager.UnderRoot=true
 ProjectManager.functionlistsort=1-SystemClock_Config-RCC-false-HAL-false,2-MX_GPIO_Init-GPIO-false-HAL-true,3-MX_DMA_Init-DMA-false-HAL-true,4-MX_USB_DEVICE_Init-USB_DEVICE-false-HAL-false,5-MX_UART4_Init-UART4-false-HAL-true,6-MX_UART5_Init-UART5-false-HAL-true,7-MX_USART1_UART_Init-USART1-false-HAL-true,8-MX_USART2_UART_Init-USART2-false-HAL-true,9-MX_USART3_UART_Init-USART3-false-HAL-true,10-MX_USART6_UART_Init-USART6-false-HAL-true,11-MX_I2C1_Init-I2C1-false-HAL-true
 RCC.AHBFreq_Value=72000000
@@ -393,3 +391,4 @@ VP_SYS_VS_Systick.Signal=SYS_VS_Systick
 VP_USB_DEVICE_VS_USB_DEVICE_CDC_FS.Mode=CDC_FS
 VP_USB_DEVICE_VS_USB_DEVICE_CDC_FS.Signal=USB_DEVICE_VS_USB_DEVICE_CDC_FS
 board=custom
+isbadioc=false
diff --git a/soccer_hardware/soccer_firmware_interface/config/bez2.yaml b/soccer_hardware/soccer_firmware_interface/config/bez2.yaml
index 3eab3e579..ea3f286e3 100644
--- a/soccer_hardware/soccer_firmware_interface/config/bez2.yaml
+++ b/soccer_hardware/soccer_firmware_interface/config/bez2.yaml
@@ -4,6 +4,7 @@ left_leg_motor_0:
   angle_zero: 180
   angle_max: 360
   angle_min: 0
+
 left_leg_motor_1:
   id: 5
   type: "xm430"
@@ -11,6 +12,7 @@ left_leg_motor_1:
   angle_max: 360
   angle_min: 0
   flipped: "true"
+
 left_leg_motor_2:
   id: 6
   type: "xm430"
@@ -18,18 +20,21 @@ left_leg_motor_2:
   angle_max: 360
   angle_min: 0
   flipped: "true"
+
 left_leg_motor_3:
   id: 7
   type: "mx64"
   angle_zero: 180
   angle_max: 360
   angle_min: 0
+
 left_leg_motor_4:
   id: 8
   type: "mx64"
   angle_zero: 180
   angle_max: 360
   angle_min: 0
+
 left_leg_motor_5:
   id: 9
   type: "mx64"
@@ -37,6 +42,7 @@ left_leg_motor_5:
   angle_max: 360
   angle_min: 0
   flipped: "true"
+
 right_leg_motor_0:
   id: 10
   type: "xm430"
@@ -44,18 +50,21 @@ right_leg_motor_0:
   angle_max: 360
   angle_min: 0
   flipped: "true"
+
 right_leg_motor_1:
   id: 11
   type: "xm430"
   angle_zero: 180
   angle_max: 360
   angle_min: 0
+
 right_leg_motor_2:
   id: 12
   type: "xm430"
   angle_zero: 180
   angle_max: 360
   angle_min: 0
+
 right_leg_motor_3:
   id: 13
   type: "mx64"
@@ -63,6 +72,7 @@ right_leg_motor_3:
   angle_max: 360
   angle_min: 0
   flipped: "true"
+
 right_leg_motor_4:
   id: 14
   type: "mx64"
@@ -70,12 +80,14 @@ right_leg_motor_4:
   angle_max: 360
   angle_min: 0
   flipped: "true"
+
 right_leg_motor_5:
   id: 15
   type: "mx64"
   angle_zero: 180
   angle_max: 360
   angle_min: 0
+
 left_arm_motor_0:
   id: 0
   type: "mx28"
@@ -83,24 +95,28 @@ left_arm_motor_0:
   angle_max: 360
   angle_min: 0
   flipped: "true"
+
 left_arm_motor_1:
   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
+
 right_arm_motor_0: # Checked
   id: 19
   type: "mx28"
   angle_zero: 180
   angle_max: 360
   angle_min: 0
+
 right_arm_motor_1:
   id: 2
   type: "mx28"
@@ -108,13 +124,15 @@ right_arm_motor_1:
   angle_max: 360
   angle_min: 0
   flipped: "true"
-  right_arm_motor_2:
-    id: 3
-    type: "mx28"
-    angle_zero: 180
-    angle_max: 360
-    angle_min: 0
-    flipped: "true"
+
+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: "xl430"
@@ -122,6 +140,7 @@ head_motor_0:
   angle_max: 300
   angle_min: 0
   flipped: "true"
+
 head_motor_1:
   id: 17
   type: "xl430"
diff --git a/soccer_hardware/soccer_firmware_interface/config/motor_types.yaml b/soccer_hardware/soccer_firmware_interface/config/motor_types.yaml
index 3199ff548..d7ab3d742 100644
--- a/soccer_hardware/soccer_firmware_interface/config/motor_types.yaml
+++ b/soccer_hardware/soccer_firmware_interface/config/motor_types.yaml
@@ -8,6 +8,9 @@ ax12:
 mx64:
   max_angle_bytes: 0xFFF
   max_angle_degrees: 360
+mx28:
+  max_angle_bytes: 0xFFF
+  max_angle_degrees: 360
 xl430:
   max_angle_bytes: 0xFFF
   max_angle_degrees: 360
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 a30cd913d..73220324c 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
@@ -59,6 +59,7 @@ def firmware_update_loop(self):
         while not rospy.is_shutdown():
             try:
                 self.reconnect_serial_port()
+
                 # data format
                 # FF
                 # (Raw data motor 0)            2 bytes