diff --git a/.cproject b/.cproject
index 91bd2e0..99e2a62 100644
--- a/.cproject
+++ b/.cproject
@@ -55,6 +55,7 @@
 									<listOptionValue builtIn="false" value="../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F"/>
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/CAN-Demo/Libs/rtos2-utils/include}&quot;"/>
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Libs/TACOS/include}&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Libs/isotp/include}&quot;"/>
 								</option>
 								<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.input.c.1002361924" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.input.c"/>
 							</tool>
@@ -75,6 +76,7 @@
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/CAN-Demo/App/Inc/sta}&quot;"/>
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/CAN-Demo/Libs/rtos2-utils/include}&quot;"/>
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Libs/TACOS/include}&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Libs/isotp/include}&quot;"/>
 								</option>
 								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.definedsymbols.2087867384" name="Define symbols (-D)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.definedsymbols" useByScannerDiscovery="false" valueType="definedSymbols">
 									<listOptionValue builtIn="false" value="DEBUG"/>
diff --git a/.mxproject b/.mxproject
index 4925fef..26e39bc 100644
--- a/.mxproject
+++ b/.mxproject
@@ -2,7 +2,7 @@
 LibFiles=Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_can.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_rcc.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_rcc_ex.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_bus.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_rcc.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_system.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_utils.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_flash.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_flash_ex.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_flash_ramfunc.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_gpio.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_gpio_ex.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_gpio.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_dma_ex.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_dma.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_dma.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_dmamux.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_pwr.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_pwr_ex.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_pwr.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_cortex.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_cortex.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal.h;Drivers\STM32F4xx_HAL_Driver\Inc\Legacy\stm32_hal_legacy.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_def.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_exti.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_exti.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_tim.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_tim_ex.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_uart.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_usart.h;Middlewares\Third_Party\FreeRTOS\Source\include\croutine.h;Middlewares\Third_Party\FreeRTOS\Source\include\deprecated_definitions.h;Middlewares\Third_Party\FreeRTOS\Source\include\event_groups.h;Middlewares\Third_Party\FreeRTOS\Source\include\FreeRTOS.h;Middlewares\Third_Party\FreeRTOS\Source\include\list.h;Middlewares\Third_Party\FreeRTOS\Source\include\message_buffer.h;Middlewares\Third_Party\FreeRTOS\Source\include\mpu_prototypes.h;Middlewares\Third_Party\FreeRTOS\Source\include\mpu_wrappers.h;Middlewares\Third_Party\FreeRTOS\Source\include\portable.h;Middlewares\Third_Party\FreeRTOS\Source\include\projdefs.h;Middlewares\Third_Party\FreeRTOS\Source\include\queue.h;Middlewares\Third_Party\FreeRTOS\Source\include\semphr.h;Middlewares\Third_Party\FreeRTOS\Source\include\stack_macros.h;Middlewares\Third_Party\FreeRTOS\Source\include\StackMacros.h;Middlewares\Third_Party\FreeRTOS\Source\include\stream_buffer.h;Middlewares\Third_Party\FreeRTOS\Source\include\task.h;Middlewares\Third_Party\FreeRTOS\Source\include\timers.h;Middlewares\Third_Party\FreeRTOS\Source\include\atomic.h;Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.h;Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os.h;Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\freertos_mpool.h;Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\freertos_os2.h;Middlewares\Third_Party\FreeRTOS\Source\portable\GCC\ARM_CM4F\portmacro.h;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_can.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_rcc.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_rcc_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_flash.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_flash_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_flash_ramfunc.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_gpio.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_dma_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_dma.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_pwr.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_pwr_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_cortex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_exti.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_tim.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_tim_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_uart.c;Middlewares\Third_Party\FreeRTOS\Source\croutine.c;Middlewares\Third_Party\FreeRTOS\Source\event_groups.c;Middlewares\Third_Party\FreeRTOS\Source\list.c;Middlewares\Third_Party\FreeRTOS\Source\queue.c;Middlewares\Third_Party\FreeRTOS\Source\stream_buffer.c;Middlewares\Third_Party\FreeRTOS\Source\tasks.c;Middlewares\Third_Party\FreeRTOS\Source\timers.c;Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.c;Middlewares\Third_Party\FreeRTOS\Source\portable\MemMang\heap_4.c;Middlewares\Third_Party\FreeRTOS\Source\portable\GCC\ARM_CM4F\port.c;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_can.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_rcc.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_rcc_ex.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_bus.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_rcc.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_system.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_utils.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_flash.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_flash_ex.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_flash_ramfunc.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_gpio.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_gpio_ex.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_gpio.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_dma_ex.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_dma.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_dma.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_dmamux.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_pwr.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_pwr_ex.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_pwr.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_cortex.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_cortex.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal.h;Drivers\STM32F4xx_HAL_Driver\Inc\Legacy\stm32_hal_legacy.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_def.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_exti.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_exti.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_tim.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_tim_ex.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_uart.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_usart.h;Middlewares\Third_Party\FreeRTOS\Source\include\croutine.h;Middlewares\Third_Party\FreeRTOS\Source\include\deprecated_definitions.h;Middlewares\Third_Party\FreeRTOS\Source\include\event_groups.h;Middlewares\Third_Party\FreeRTOS\Source\include\FreeRTOS.h;Middlewares\Third_Party\FreeRTOS\Source\include\list.h;Middlewares\Third_Party\FreeRTOS\Source\include\message_buffer.h;Middlewares\Third_Party\FreeRTOS\Source\include\mpu_prototypes.h;Middlewares\Third_Party\FreeRTOS\Source\include\mpu_wrappers.h;Middlewares\Third_Party\FreeRTOS\Source\include\portable.h;Middlewares\Third_Party\FreeRTOS\Source\include\projdefs.h;Middlewares\Third_Party\FreeRTOS\Source\include\queue.h;Middlewares\Third_Party\FreeRTOS\Source\include\semphr.h;Middlewares\Third_Party\FreeRTOS\Source\include\stack_macros.h;Middlewares\Third_Party\FreeRTOS\Source\include\StackMacros.h;Middlewares\Third_Party\FreeRTOS\Source\include\stream_buffer.h;Middlewares\Third_Party\FreeRTOS\Source\include\task.h;Middlewares\Third_Party\FreeRTOS\Source\include\timers.h;Middlewares\Third_Party\FreeRTOS\Source\include\atomic.h;Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.h;Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os.h;Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\freertos_mpool.h;Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\freertos_os2.h;Middlewares\Third_Party\FreeRTOS\Source\portable\GCC\ARM_CM4F\portmacro.h;Drivers\CMSIS\Device\ST\STM32F4xx\Include\stm32f407xx.h;Drivers\CMSIS\Device\ST\STM32F4xx\Include\stm32f4xx.h;Drivers\CMSIS\Device\ST\STM32F4xx\Include\system_stm32f4xx.h;Drivers\CMSIS\Device\ST\STM32F4xx\Source\Templates\system_stm32f4xx.c;Drivers\CMSIS\Include\cmsis_armcc.h;Drivers\CMSIS\Include\cmsis_armclang.h;Drivers\CMSIS\Include\cmsis_compiler.h;Drivers\CMSIS\Include\cmsis_gcc.h;Drivers\CMSIS\Include\cmsis_iccarm.h;Drivers\CMSIS\Include\cmsis_version.h;Drivers\CMSIS\Include\core_armv8mbl.h;Drivers\CMSIS\Include\core_armv8mml.h;Drivers\CMSIS\Include\core_cm0.h;Drivers\CMSIS\Include\core_cm0plus.h;Drivers\CMSIS\Include\core_cm1.h;Drivers\CMSIS\Include\core_cm23.h;Drivers\CMSIS\Include\core_cm3.h;Drivers\CMSIS\Include\core_cm33.h;Drivers\CMSIS\Include\core_cm4.h;Drivers\CMSIS\Include\core_cm7.h;Drivers\CMSIS\Include\core_sc000.h;Drivers\CMSIS\Include\core_sc300.h;Drivers\CMSIS\Include\mpu_armv7.h;Drivers\CMSIS\Include\mpu_armv8.h;Drivers\CMSIS\Include\tz_context.h;
 
 [PreviousUsedCubeIDEFiles]
-SourceFiles=Core\Src\main.c;Core\Src\gpio.c;Core\Src\freertos.c;Core\Src\can.c;Core\Src\usart.c;Core\Src\stm32f4xx_it.c;Core\Src\stm32f4xx_hal_msp.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_can.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_rcc.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_rcc_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_flash.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_flash_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_flash_ramfunc.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_gpio.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_dma_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_dma.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_pwr.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_pwr_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_cortex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_exti.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_tim.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_tim_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_uart.c;Middlewares\Third_Party\FreeRTOS\Source\croutine.c;Middlewares\Third_Party\FreeRTOS\Source\event_groups.c;Middlewares\Third_Party\FreeRTOS\Source\list.c;Middlewares\Third_Party\FreeRTOS\Source\queue.c;Middlewares\Third_Party\FreeRTOS\Source\stream_buffer.c;Middlewares\Third_Party\FreeRTOS\Source\tasks.c;Middlewares\Third_Party\FreeRTOS\Source\timers.c;Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.c;Middlewares\Third_Party\FreeRTOS\Source\portable\MemMang\heap_4.c;Middlewares\Third_Party\FreeRTOS\Source\portable\GCC\ARM_CM4F\port.c;Drivers\CMSIS\Device\ST\STM32F4xx\Source\Templates\system_stm32f4xx.c;Core\Src\system_stm32f4xx.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_can.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_rcc.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_rcc_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_flash.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_flash_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_flash_ramfunc.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_gpio.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_dma_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_dma.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_pwr.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_pwr_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_cortex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_exti.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_tim.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_tim_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_uart.c;Middlewares\Third_Party\FreeRTOS\Source\croutine.c;Middlewares\Third_Party\FreeRTOS\Source\event_groups.c;Middlewares\Third_Party\FreeRTOS\Source\list.c;Middlewares\Third_Party\FreeRTOS\Source\queue.c;Middlewares\Third_Party\FreeRTOS\Source\stream_buffer.c;Middlewares\Third_Party\FreeRTOS\Source\tasks.c;Middlewares\Third_Party\FreeRTOS\Source\timers.c;Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.c;Middlewares\Third_Party\FreeRTOS\Source\portable\MemMang\heap_4.c;Middlewares\Third_Party\FreeRTOS\Source\portable\GCC\ARM_CM4F\port.c;Drivers\CMSIS\Device\ST\STM32F4xx\Source\Templates\system_stm32f4xx.c;Core\Src\system_stm32f4xx.c;;;Middlewares\Third_Party\FreeRTOS\Source\croutine.c;Middlewares\Third_Party\FreeRTOS\Source\event_groups.c;Middlewares\Third_Party\FreeRTOS\Source\list.c;Middlewares\Third_Party\FreeRTOS\Source\queue.c;Middlewares\Third_Party\FreeRTOS\Source\stream_buffer.c;Middlewares\Third_Party\FreeRTOS\Source\tasks.c;Middlewares\Third_Party\FreeRTOS\Source\timers.c;Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.c;Middlewares\Third_Party\FreeRTOS\Source\portable\MemMang\heap_4.c;Middlewares\Third_Party\FreeRTOS\Source\portable\GCC\ARM_CM4F\port.c;
+SourceFiles=Core\Src\main.c;Core\Src\gpio.c;Core\Src\freertos.c;Core\Src\can.c;Core\Src\usart.c;Core\Src\stm32f4xx_it.c;Core\Src\stm32f4xx_hal_msp.c;Core\Src\stm32f4xx_hal_timebase_tim.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_can.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_rcc.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_rcc_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_flash.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_flash_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_flash_ramfunc.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_gpio.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_dma_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_dma.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_pwr.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_pwr_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_cortex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_exti.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_tim.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_tim_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_uart.c;Middlewares\Third_Party\FreeRTOS\Source\croutine.c;Middlewares\Third_Party\FreeRTOS\Source\event_groups.c;Middlewares\Third_Party\FreeRTOS\Source\list.c;Middlewares\Third_Party\FreeRTOS\Source\queue.c;Middlewares\Third_Party\FreeRTOS\Source\stream_buffer.c;Middlewares\Third_Party\FreeRTOS\Source\tasks.c;Middlewares\Third_Party\FreeRTOS\Source\timers.c;Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.c;Middlewares\Third_Party\FreeRTOS\Source\portable\MemMang\heap_4.c;Middlewares\Third_Party\FreeRTOS\Source\portable\GCC\ARM_CM4F\port.c;Drivers\CMSIS\Device\ST\STM32F4xx\Source\Templates\system_stm32f4xx.c;Core\Src\system_stm32f4xx.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_can.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_rcc.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_rcc_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_flash.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_flash_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_flash_ramfunc.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_gpio.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_dma_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_dma.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_pwr.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_pwr_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_cortex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_exti.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_tim.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_tim_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_uart.c;Middlewares\Third_Party\FreeRTOS\Source\croutine.c;Middlewares\Third_Party\FreeRTOS\Source\event_groups.c;Middlewares\Third_Party\FreeRTOS\Source\list.c;Middlewares\Third_Party\FreeRTOS\Source\queue.c;Middlewares\Third_Party\FreeRTOS\Source\stream_buffer.c;Middlewares\Third_Party\FreeRTOS\Source\tasks.c;Middlewares\Third_Party\FreeRTOS\Source\timers.c;Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.c;Middlewares\Third_Party\FreeRTOS\Source\portable\MemMang\heap_4.c;Middlewares\Third_Party\FreeRTOS\Source\portable\GCC\ARM_CM4F\port.c;Drivers\CMSIS\Device\ST\STM32F4xx\Source\Templates\system_stm32f4xx.c;Core\Src\system_stm32f4xx.c;;;Middlewares\Third_Party\FreeRTOS\Source\croutine.c;Middlewares\Third_Party\FreeRTOS\Source\event_groups.c;Middlewares\Third_Party\FreeRTOS\Source\list.c;Middlewares\Third_Party\FreeRTOS\Source\queue.c;Middlewares\Third_Party\FreeRTOS\Source\stream_buffer.c;Middlewares\Third_Party\FreeRTOS\Source\tasks.c;Middlewares\Third_Party\FreeRTOS\Source\timers.c;Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.c;Middlewares\Third_Party\FreeRTOS\Source\portable\MemMang\heap_4.c;Middlewares\Third_Party\FreeRTOS\Source\portable\GCC\ARM_CM4F\port.c;
 HeaderPath=Drivers\STM32F4xx_HAL_Driver\Inc;Drivers\STM32F4xx_HAL_Driver\Inc\Legacy;Middlewares\Third_Party\FreeRTOS\Source\include;Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2;Middlewares\Third_Party\FreeRTOS\Source\portable\GCC\ARM_CM4F;Drivers\CMSIS\Device\ST\STM32F4xx\Include;Drivers\CMSIS\Include;Core\Inc;
 CDefines=USE_HAL_DRIVER;STM32F407xx;USE_HAL_DRIVER;USE_HAL_DRIVER;
 
@@ -19,14 +19,15 @@ HeaderFiles#6=..\Core\Inc\main.h
 HeaderFolderListSize=1
 HeaderPath#0=..\Core\Inc
 HeaderFiles=;
-SourceFileListSize=7
+SourceFileListSize=8
 SourceFiles#0=..\Core\Src\gpio.c
 SourceFiles#1=..\Core\Src\freertos.c
 SourceFiles#2=..\Core\Src\can.c
 SourceFiles#3=..\Core\Src\usart.c
 SourceFiles#4=..\Core\Src\stm32f4xx_it.c
 SourceFiles#5=..\Core\Src\stm32f4xx_hal_msp.c
-SourceFiles#6=..\Core\Src\main.c
+SourceFiles#6=..\Core\Src\stm32f4xx_hal_timebase_tim.c
+SourceFiles#7=..\Core\Src\main.c
 SourceFolderListSize=1
 SourcePath#0=..\Core\Src
 SourceFiles=;
diff --git a/.settings/language.settings.xml b/.settings/language.settings.xml
index 96f5bd0..ff2d585 100644
--- a/.settings/language.settings.xml
+++ b/.settings/language.settings.xml
@@ -5,7 +5,7 @@
 			<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="-1080687771005896909" 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="-614993993494983921" 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 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="-1080687771005896909" 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="-614993993494983921" 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>
diff --git a/.settings/stm32cubeide.project.prefs b/.settings/stm32cubeide.project.prefs
index 48449af..0d7d288 100644
--- a/.settings/stm32cubeide.project.prefs
+++ b/.settings/stm32cubeide.project.prefs
@@ -1,5 +1,5 @@
 635E684B79701B039C64EA45C3F84D30=44DBEC1DE4EBA9B0485911FBE58BFC80
 66BE74F758C12D739921AEA421D593D3=3
-8DF89ED150041C4CBC7CB9A9CAA90856=84EF3C290D485BA5E95DA5C0811FD966
-DC22A860405A8BF2F2C095E5B6529F12=B12B632EDB5BE446F20C032575C0758A
+8DF89ED150041C4CBC7CB9A9CAA90856=7B5B92F1162457C6ECACD202698061B0
+DC22A860405A8BF2F2C095E5B6529F12=7B5B92F1162457C6ECACD202698061B0
 eclipse.preferences.version=1
diff --git a/App/Inc/sta/config.hpp b/App/Inc/sta/config.hpp
index 06a096a..1c00977 100644
--- a/App/Inc/sta/config.hpp
+++ b/App/Inc/sta/config.hpp
@@ -29,14 +29,10 @@
 #define STA_RTOS_SYSTEM_EVENTS_ENABLE
 // #define STA_RTOS_SYSTEM_WATCHDOG_ENABLE
 // #define STA_RTOS_WATCHDOG_ENABLE
+// #define STA_CAN_BUS_ENABLE
 
-
-// Settings for TACOS
-#define STA_TACOS_MANAGER_PRIORITY osPriorityNormal
-#define STA_TACOS_STATEMACHINE_PRIORITY osPriorityNormal
-
-// Statemachine settings. Here, we only have a single state which is also the initial state.
+// Uses the default configuration for TACOS.
+#include<sta/tacos/configs/default.hpp>
 #define STA_TACOS_NUM_STATES 3
-#define STA_TACOS_INITIAL_STATE 0
 
 #endif /* INC_STA_CONFIG_HPP_ */
diff --git a/App/Src/tasks/can_task.cpp b/App/Src/tasks/can_task.cpp
index 84bd5a3..947108d 100644
--- a/App/Src/tasks/can_task.cpp
+++ b/App/Src/tasks/can_task.cpp
@@ -46,18 +46,55 @@ namespace demo
 
 		STA_DEBUG_PRINTLN("WAITING");
 		HAL_CAN_GetRxMessage(&hcan1, CAN_RX_FIFO0, &rxHeader, canRX);
-		STA_DEBUG_PRINTLN("RECEIVED");
 
 		uint8_t type_id, sensor_ID, value, include;
 
 		unpackValues(canRX[0], &type_id, &sensor_ID, &value, &include);
 
-		if (type_id == 0 && sensor_ID == 1){
-			if (value == 0){
-				HAL_GPIO_WritePin(GPIOD,GPIO_PIN_8, GPIO_PIN_RESET);
-			}else{
-				HAL_GPIO_WritePin(GPIOD,GPIO_PIN_8, GPIO_PIN_SET);
+		if (type_id == 0){
+			if (sensor_ID == 1){
+
+				STA_DEBUG_PRINTLN("Relay 1 triggered");
+				if(value == 1){
+					HAL_GPIO_WritePin(GPIOA,GPIO_PIN_2, GPIO_PIN_SET);
+				} else if(value == 0){
+					HAL_GPIO_WritePin(GPIOA,GPIO_PIN_2, GPIO_PIN_RESET);
+				}
+
+			}else if (sensor_ID == 2){
+
+				STA_DEBUG_PRINTLN("Relay 2 triggered");
+				if(value == 1){
+					HAL_GPIO_WritePin(GPIOA,GPIO_PIN_1, GPIO_PIN_SET);
+				} else if(value == 0){
+					HAL_GPIO_WritePin(GPIOA,GPIO_PIN_1, GPIO_PIN_RESET);
+				}
+			}else if (sensor_ID == 3){
+
+				STA_DEBUG_PRINTLN("Relay 3 triggered");
+				if(value == 1){
+					HAL_GPIO_WritePin(GPIOA,GPIO_PIN_0, GPIO_PIN_SET);
+				} else if(value == 0){
+					HAL_GPIO_WritePin(GPIOA,GPIO_PIN_0, GPIO_PIN_RESET);
+				}
+			}else if (sensor_ID == 4){
+
+				STA_DEBUG_PRINTLN("Relay 4 triggered");
+				if(value == 1){
+					HAL_GPIO_WritePin(GPIOA,GPIO_PIN_3, GPIO_PIN_SET);
+				} else if(value == 0){
+					HAL_GPIO_WritePin(GPIOA,GPIO_PIN_3, GPIO_PIN_RESET);
+				}
+			}else if (sensor_ID == 5){
+
+				STA_DEBUG_PRINTLN("Relay 5 triggered");
+				if(value == 1){
+					HAL_GPIO_WritePin(GPIOA,GPIO_PIN_6, GPIO_PIN_SET);
+				} else if(value == 0){
+					HAL_GPIO_WritePin(GPIOA,GPIO_PIN_6, GPIO_PIN_RESET);
+				}
 			}
+
 		}
 
 		HAL_Delay(1000);
diff --git a/CAN-Demo.ioc b/CAN-Demo.ioc
index 7d425d1..9a592f0 100644
--- a/CAN-Demo.ioc
+++ b/CAN-Demo.ioc
@@ -33,32 +33,35 @@ Mcu.Name=STM32F407Z(E-G)Tx
 Mcu.Package=LQFP144
 Mcu.Pin0=PH0-OSC_IN
 Mcu.Pin1=PH1-OSC_OUT
-Mcu.Pin10=PA13
-Mcu.Pin11=PA14
-Mcu.Pin12=PA15
-Mcu.Pin13=PC10
-Mcu.Pin14=PC11
-Mcu.Pin15=PC12
-Mcu.Pin16=PB3
-Mcu.Pin17=VP_FREERTOS_VS_CMSIS_V2
-Mcu.Pin18=VP_SYS_VS_Systick
-Mcu.Pin2=PB12
-Mcu.Pin3=PB13
-Mcu.Pin4=PD8
-Mcu.Pin5=PD9
-Mcu.Pin6=PA9
-Mcu.Pin7=PA10
-Mcu.Pin8=PA11
-Mcu.Pin9=PA12
-Mcu.PinsNb=19
+Mcu.Pin10=PD9
+Mcu.Pin11=PA9
+Mcu.Pin12=PA10
+Mcu.Pin13=PA11
+Mcu.Pin14=PA12
+Mcu.Pin15=PA13
+Mcu.Pin16=PA14
+Mcu.Pin17=PA15
+Mcu.Pin18=PC10
+Mcu.Pin19=PC11
+Mcu.Pin2=PA0-WKUP
+Mcu.Pin20=PC12
+Mcu.Pin21=PB3
+Mcu.Pin22=VP_FREERTOS_VS_CMSIS_V2
+Mcu.Pin23=VP_SYS_VS_tim1
+Mcu.Pin3=PA1
+Mcu.Pin4=PA2
+Mcu.Pin5=PA3
+Mcu.Pin6=PA6
+Mcu.Pin7=PB12
+Mcu.Pin8=PB13
+Mcu.Pin9=PD8
+Mcu.PinsNb=24
 Mcu.ThirdPartyNb=0
 Mcu.UserConstants=
 Mcu.UserName=STM32F407ZGTx
 MxCube.Version=6.10.0
 MxDb.Version=DB.6.0.100
 NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
-NVIC.CAN1_RX0_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
-NVIC.CAN1_RX1_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
 NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
 NVIC.ForceEnableDMAVector=true
 NVIC.HardFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
@@ -70,8 +73,15 @@ NVIC.SVCall_IRQn=true\:0\:0\:false\:false\:false\:false\:false\:false\:false
 NVIC.SavedPendsvIrqHandlerGenerated=true
 NVIC.SavedSvcallIrqHandlerGenerated=true
 NVIC.SavedSystickIrqHandlerGenerated=true
-NVIC.SysTick_IRQn=true\:15\:0\:false\:false\:true\:true\:false\:true\:false
+NVIC.SysTick_IRQn=true\:15\:0\:false\:false\:false\:true\:false\:true\:false
+NVIC.TIM1_UP_TIM10_IRQn=true\:15\:0\:false\:false\:true\:false\:false\:true\:true
+NVIC.TimeBase=TIM1_UP_TIM10_IRQn
+NVIC.TimeBaseIP=TIM1
 NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
+PA0-WKUP.Locked=true
+PA0-WKUP.Signal=GPIO_Output
+PA1.Locked=true
+PA1.Signal=GPIO_Output
 PA10.Mode=Asynchronous
 PA10.Signal=USART1_RX
 PA11.Locked=true
@@ -86,6 +96,12 @@ PA14.Locked=true
 PA14.Signal=SYS_JTCK-SWCLK
 PA15.Locked=true
 PA15.Signal=SYS_JTDI
+PA2.Locked=true
+PA2.Signal=GPIO_Output
+PA3.Locked=true
+PA3.Signal=GPIO_Output
+PA6.Locked=true
+PA6.Signal=GPIO_Output
 PA9.Mode=Asynchronous
 PA9.Signal=USART1_TX
 PB12.GPIOParameters=GPIO_Speed,PinState
@@ -162,7 +178,8 @@ RCC.HCLKFreq_Value=16000000
 RCC.HSE_VALUE=8000000
 RCC.HSI_VALUE=16000000
 RCC.I2SClocksFreq_Value=192000000
-RCC.IPParameters=48MHZClocksFreq_Value,AHBCLKDivider,AHBFreq_Value,APB1Freq_Value,APB1TimFreq_Value,APB2Freq_Value,APB2TimFreq_Value,CortexFreq_Value,EthernetFreq_Value,FCLKCortexFreq_Value,FamilyName,HCLKFreq_Value,HSE_VALUE,HSI_VALUE,I2SClocksFreq_Value,LSI_VALUE,MCO2PinFreq_Value,PLLCLKFreq_Value,PLLM,PLLN,PLLQCLKFreq_Value,PLLSourceVirtual,RTCFreq_Value,RTCHSEDivFreq_Value,SYSCLKFreq_VALUE,SYSCLKSource,VCOI2SOutputFreq_Value,VCOInputFreq_Value,VCOOutputFreq_Value,VcooutputI2S
+RCC.IPParameters=48MHZClocksFreq_Value,AHBCLKDivider,AHBFreq_Value,APB1Freq_Value,APB1TimFreq_Value,APB2Freq_Value,APB2TimFreq_Value,CortexFreq_Value,EthernetFreq_Value,FCLKCortexFreq_Value,FamilyName,HCLKFreq_Value,HSE_VALUE,HSI_VALUE,I2SClocksFreq_Value,LSE_VALUE,LSI_VALUE,MCO2PinFreq_Value,PLLCLKFreq_Value,PLLM,PLLN,PLLQCLKFreq_Value,PLLSourceVirtual,RTCFreq_Value,RTCHSEDivFreq_Value,SYSCLKFreq_VALUE,SYSCLKSource,VCOI2SOutputFreq_Value,VCOInputFreq_Value,VCOOutputFreq_Value,VcooutputI2S
+RCC.LSE_VALUE=32768
 RCC.LSI_VALUE=32000
 RCC.MCO2PinFreq_Value=64000000
 RCC.PLLCLKFreq_Value=64000000
@@ -183,8 +200,8 @@ USART1.IPParameters=VirtualMode,BaudRate
 USART1.VirtualMode=VM_ASYNC
 VP_FREERTOS_VS_CMSIS_V2.Mode=CMSIS_V2
 VP_FREERTOS_VS_CMSIS_V2.Signal=FREERTOS_VS_CMSIS_V2
-VP_SYS_VS_Systick.Mode=SysTick
-VP_SYS_VS_Systick.Signal=SYS_VS_Systick
+VP_SYS_VS_tim1.Mode=TIM1
+VP_SYS_VS_tim1.Signal=SYS_VS_tim1
 board=custom
 rtos.0.ip=FREERTOS
 isbadioc=false
diff --git a/Core/Inc/FreeRTOSConfig.h b/Core/Inc/FreeRTOSConfig.h
index 3a0535d..adf0f92 100644
--- a/Core/Inc/FreeRTOSConfig.h
+++ b/Core/Inc/FreeRTOSConfig.h
@@ -51,7 +51,6 @@
 #if defined(__ICCARM__) || defined(__CC_ARM) || defined(__GNUC__)
   #include <stdint.h>
   extern uint32_t SystemCoreClock;
-  void xPortSysTickHandler(void);
 #endif
 #ifndef CMSIS_device_header
 #define CMSIS_device_header "stm32f4xx.h"
@@ -166,7 +165,7 @@ standard names. */
 
 /* IMPORTANT: After 10.3.1 update, Systick_Handler comes from NVIC (if SYS timebase = systick), otherwise from cmsis_os2.c */
 
-#define USE_CUSTOM_SYSTICK_HANDLER_IMPLEMENTATION 1
+#define USE_CUSTOM_SYSTICK_HANDLER_IMPLEMENTATION 0
 
 /* USER CODE BEGIN Defines */
 /* Section where parameter definitions can be added (for instance, to override default ones in FreeRTOS.h) */
diff --git a/Core/Inc/stm32f4xx_hal_conf.h b/Core/Inc/stm32f4xx_hal_conf.h
index c026ca8..e891e65 100644
--- a/Core/Inc/stm32f4xx_hal_conf.h
+++ b/Core/Inc/stm32f4xx_hal_conf.h
@@ -63,7 +63,7 @@
 /* #define HAL_SD_MODULE_ENABLED */
 /* #define HAL_MMC_MODULE_ENABLED */
 /* #define HAL_SPI_MODULE_ENABLED */
-/* #define HAL_TIM_MODULE_ENABLED */
+#define HAL_TIM_MODULE_ENABLED
 #define HAL_UART_MODULE_ENABLED
 /* #define HAL_USART_MODULE_ENABLED */
 /* #define HAL_IRDA_MODULE_ENABLED */
diff --git a/Core/Inc/stm32f4xx_it.h b/Core/Inc/stm32f4xx_it.h
index f877f08..aebd577 100644
--- a/Core/Inc/stm32f4xx_it.h
+++ b/Core/Inc/stm32f4xx_it.h
@@ -52,9 +52,7 @@ void MemManage_Handler(void);
 void BusFault_Handler(void);
 void UsageFault_Handler(void);
 void DebugMon_Handler(void);
-void SysTick_Handler(void);
-void CAN1_RX0_IRQHandler(void);
-void CAN1_RX1_IRQHandler(void);
+void TIM1_UP_TIM10_IRQHandler(void);
 /* USER CODE BEGIN EFP */
 
 /* USER CODE END EFP */
diff --git a/Core/Src/can.c b/Core/Src/can.c
index 4a952bc..dd79b62 100644
--- a/Core/Src/can.c
+++ b/Core/Src/can.c
@@ -83,11 +83,6 @@ void HAL_CAN_MspInit(CAN_HandleTypeDef* canHandle)
     GPIO_InitStruct.Alternate = GPIO_AF9_CAN1;
     HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
 
-    /* CAN1 interrupt Init */
-    HAL_NVIC_SetPriority(CAN1_RX0_IRQn, 5, 0);
-    HAL_NVIC_EnableIRQ(CAN1_RX0_IRQn);
-    HAL_NVIC_SetPriority(CAN1_RX1_IRQn, 5, 0);
-    HAL_NVIC_EnableIRQ(CAN1_RX1_IRQn);
   /* USER CODE BEGIN CAN1_MspInit 1 */
 
   /* USER CODE END CAN1_MspInit 1 */
@@ -111,9 +106,6 @@ void HAL_CAN_MspDeInit(CAN_HandleTypeDef* canHandle)
     */
     HAL_GPIO_DeInit(GPIOA, GPIO_PIN_11|GPIO_PIN_12);
 
-    /* CAN1 interrupt Deinit */
-    HAL_NVIC_DisableIRQ(CAN1_RX0_IRQn);
-    HAL_NVIC_DisableIRQ(CAN1_RX1_IRQn);
   /* USER CODE BEGIN CAN1_MspDeInit 1 */
 
   /* USER CODE END CAN1_MspDeInit 1 */
diff --git a/Core/Src/gpio.c b/Core/Src/gpio.c
index 3dded7a..64ca0ed 100644
--- a/Core/Src/gpio.c
+++ b/Core/Src/gpio.c
@@ -49,17 +49,30 @@ void MX_GPIO_Init(void)
 
   /* GPIO Ports Clock Enable */
   __HAL_RCC_GPIOH_CLK_ENABLE();
+  __HAL_RCC_GPIOA_CLK_ENABLE();
   __HAL_RCC_GPIOB_CLK_ENABLE();
   __HAL_RCC_GPIOD_CLK_ENABLE();
-  __HAL_RCC_GPIOA_CLK_ENABLE();
   __HAL_RCC_GPIOC_CLK_ENABLE();
 
+  /*Configure GPIO pin Output Level */
+  HAL_GPIO_WritePin(GPIOA, GPIO_PIN_0|GPIO_PIN_1|GPIO_PIN_2|GPIO_PIN_3
+                          |GPIO_PIN_6, GPIO_PIN_RESET);
+
   /*Configure GPIO pin Output Level */
   HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12|GPIO_PIN_13, GPIO_PIN_RESET);
 
   /*Configure GPIO pin Output Level */
   HAL_GPIO_WritePin(GPIOD, GPIO_PIN_8|GPIO_PIN_9, GPIO_PIN_RESET);
 
+  /*Configure GPIO pins : PA0 PA1 PA2 PA3
+                           PA6 */
+  GPIO_InitStruct.Pin = GPIO_PIN_0|GPIO_PIN_1|GPIO_PIN_2|GPIO_PIN_3
+                          |GPIO_PIN_6;
+  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
+  GPIO_InitStruct.Pull = GPIO_NOPULL;
+  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+  HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
+
   /*Configure GPIO pins : PB12 PB13 */
   GPIO_InitStruct.Pin = GPIO_PIN_12|GPIO_PIN_13;
   GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
diff --git a/Core/Src/main.c b/Core/Src/main.c
index f8a7cc3..b627079 100644
--- a/Core/Src/main.c
+++ b/Core/Src/main.c
@@ -217,7 +217,7 @@ void unpackValues(uint8_t packedByte, uint8_t* type_id, uint8_t* sensor_ID, uint
 void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan1)
 {
 
-	HAL_CAN_GetRxMessage(hcan1, CAN_RX_FIFO0, &rxHeader, canRX);
+	/*HAL_CAN_GetRxMessage(hcan1, CAN_RX_FIFO0, &rxHeader, canRX);
 
 	uint8_t type_id, sensor_ID, value, include;
 
@@ -225,17 +225,38 @@ void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan1)
 
 	if (type_id == 0 && sensor_ID == 1){
 		if (value == 0){
-			HAL_GPIO_WritePin(GPIOD,GPIO_PIN_8, GPIO_PIN_RESET);
+			HAL_GPIO_WritePin(GPIOD, GPIO_PIN_8, GPIO_PIN_SET);
 		}else{
 			HAL_GPIO_WritePin(GPIOD,GPIO_PIN_8, GPIO_PIN_SET);
 		}
-	}
+	}*/
 
 
 }
 
 /* USER CODE END 4 */
 
+/**
+  * @brief  Period elapsed callback in non blocking mode
+  * @note   This function is called  when TIM1 interrupt took place, inside
+  * HAL_TIM_IRQHandler(). It makes a direct call to HAL_IncTick() to increment
+  * a global variable "uwTick" used as application time base.
+  * @param  htim : TIM handle
+  * @retval None
+  */
+void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
+{
+  /* USER CODE BEGIN Callback 0 */
+
+  /* USER CODE END Callback 0 */
+  if (htim->Instance == TIM1) {
+    HAL_IncTick();
+  }
+  /* USER CODE BEGIN Callback 1 */
+
+  /* USER CODE END Callback 1 */
+}
+
 /**
   * @brief  This function is executed in case of error occurrence.
   * @retval None
diff --git a/Core/Src/stm32f4xx_hal_timebase_tim.c b/Core/Src/stm32f4xx_hal_timebase_tim.c
new file mode 100644
index 0000000..aa6ba4f
--- /dev/null
+++ b/Core/Src/stm32f4xx_hal_timebase_tim.c
@@ -0,0 +1,128 @@
+/* USER CODE BEGIN Header */
+/**
+  ******************************************************************************
+  * @file    stm32f4xx_hal_timebase_tim.c
+  * @brief   HAL time base based on the hardware TIM.
+  ******************************************************************************
+  * @attention
+  *
+  * Copyright (c) 2024 STMicroelectronics.
+  * All rights reserved.
+  *
+  * This software is licensed under terms that can be found in the LICENSE file
+  * in the root directory of this software component.
+  * If no LICENSE file comes with this software, it is provided AS-IS.
+  *
+  ******************************************************************************
+  */
+/* USER CODE END Header */
+
+/* Includes ------------------------------------------------------------------*/
+#include "stm32f4xx_hal.h"
+#include "stm32f4xx_hal_tim.h"
+
+/* Private typedef -----------------------------------------------------------*/
+/* Private define ------------------------------------------------------------*/
+/* Private macro -------------------------------------------------------------*/
+/* Private variables ---------------------------------------------------------*/
+TIM_HandleTypeDef        htim1;
+/* Private function prototypes -----------------------------------------------*/
+/* Private functions ---------------------------------------------------------*/
+
+/**
+  * @brief  This function configures the TIM1 as a time base source.
+  *         The time source is configured  to have 1ms time base with a dedicated
+  *         Tick interrupt priority.
+  * @note   This function is called  automatically at the beginning of program after
+  *         reset by HAL_Init() or at any time when clock is configured, by HAL_RCC_ClockConfig().
+  * @param  TickPriority: Tick interrupt priority.
+  * @retval HAL status
+  */
+HAL_StatusTypeDef HAL_InitTick(uint32_t TickPriority)
+{
+  RCC_ClkInitTypeDef    clkconfig;
+  uint32_t              uwTimclock = 0U;
+
+  uint32_t              uwPrescalerValue = 0U;
+  uint32_t              pFLatency;
+  HAL_StatusTypeDef     status;
+
+  /* Enable TIM1 clock */
+  __HAL_RCC_TIM1_CLK_ENABLE();
+
+  /* Get clock configuration */
+  HAL_RCC_GetClockConfig(&clkconfig, &pFLatency);
+
+  /* Compute TIM1 clock */
+      uwTimclock = HAL_RCC_GetPCLK2Freq();
+
+  /* Compute the prescaler value to have TIM1 counter clock equal to 1MHz */
+  uwPrescalerValue = (uint32_t) ((uwTimclock / 1000000U) - 1U);
+
+  /* Initialize TIM1 */
+  htim1.Instance = TIM1;
+
+  /* Initialize TIMx peripheral as follow:
+
+  + Period = [(TIM1CLK/1000) - 1]. to have a (1/1000) s time base.
+  + Prescaler = (uwTimclock/1000000 - 1) to have a 1MHz counter clock.
+  + ClockDivision = 0
+  + Counter direction = Up
+  */
+  htim1.Init.Period = (1000000U / 1000U) - 1U;
+  htim1.Init.Prescaler = uwPrescalerValue;
+  htim1.Init.ClockDivision = 0;
+  htim1.Init.CounterMode = TIM_COUNTERMODE_UP;
+  htim1.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
+
+  status = HAL_TIM_Base_Init(&htim1);
+  if (status == HAL_OK)
+  {
+    /* Start the TIM time Base generation in interrupt mode */
+    status = HAL_TIM_Base_Start_IT(&htim1);
+    if (status == HAL_OK)
+    {
+    /* Enable the TIM1 global Interrupt */
+        HAL_NVIC_EnableIRQ(TIM1_UP_TIM10_IRQn);
+      /* Configure the SysTick IRQ priority */
+      if (TickPriority < (1UL << __NVIC_PRIO_BITS))
+      {
+        /* Configure the TIM IRQ priority */
+        HAL_NVIC_SetPriority(TIM1_UP_TIM10_IRQn, TickPriority, 0U);
+        uwTickPrio = TickPriority;
+      }
+      else
+      {
+        status = HAL_ERROR;
+      }
+    }
+  }
+
+ /* Return function status */
+  return status;
+}
+
+/**
+  * @brief  Suspend Tick increment.
+  * @note   Disable the tick increment by disabling TIM1 update interrupt.
+  * @param  None
+  * @retval None
+  */
+void HAL_SuspendTick(void)
+{
+  /* Disable TIM1 update Interrupt */
+  __HAL_TIM_DISABLE_IT(&htim1, TIM_IT_UPDATE);
+}
+
+/**
+  * @brief  Resume Tick increment.
+  * @note   Enable the tick increment by Enabling TIM1 update interrupt.
+  * @param  None
+  * @retval None
+  */
+void HAL_ResumeTick(void)
+{
+  /* Enable TIM1 Update interrupt */
+  __HAL_TIM_ENABLE_IT(&htim1, TIM_IT_UPDATE);
+}
+
diff --git a/Core/Src/stm32f4xx_it.c b/Core/Src/stm32f4xx_it.c
index d6230e6..8e1e9e4 100644
--- a/Core/Src/stm32f4xx_it.c
+++ b/Core/Src/stm32f4xx_it.c
@@ -20,8 +20,6 @@
 /* Includes ------------------------------------------------------------------*/
 #include "main.h"
 #include "stm32f4xx_it.h"
-#include "FreeRTOS.h"
-#include "task.h"
 /* Private includes ----------------------------------------------------------*/
 /* USER CODE BEGIN Includes */
 /* USER CODE END Includes */
@@ -57,7 +55,8 @@
 /* USER CODE END 0 */
 
 /* External variables --------------------------------------------------------*/
-extern CAN_HandleTypeDef hcan1;
+extern TIM_HandleTypeDef htim1;
+
 /* USER CODE BEGIN EV */
 
 /* USER CODE END EV */
@@ -153,28 +152,6 @@ void DebugMon_Handler(void)
   /* USER CODE END DebugMonitor_IRQn 1 */
 }
 
-/**
-  * @brief This function handles System tick timer.
-  */
-void SysTick_Handler(void)
-{
-  /* USER CODE BEGIN SysTick_IRQn 0 */
-
-  /* USER CODE END SysTick_IRQn 0 */
-  HAL_IncTick();
-#if (INCLUDE_xTaskGetSchedulerState == 1 )
-  if (xTaskGetSchedulerState() != taskSCHEDULER_NOT_STARTED)
-  {
-#endif /* INCLUDE_xTaskGetSchedulerState */
-  xPortSysTickHandler();
-#if (INCLUDE_xTaskGetSchedulerState == 1 )
-  }
-#endif /* INCLUDE_xTaskGetSchedulerState */
-  /* USER CODE BEGIN SysTick_IRQn 1 */
-
-  /* USER CODE END SysTick_IRQn 1 */
-}
-
 /******************************************************************************/
 /* STM32F4xx Peripheral Interrupt Handlers                                    */
 /* Add here the Interrupt Handlers for the used peripherals.                  */
@@ -183,31 +160,17 @@ void SysTick_Handler(void)
 /******************************************************************************/
 
 /**
-  * @brief This function handles CAN1 RX0 interrupts.
+  * @brief This function handles TIM1 update interrupt and TIM10 global interrupt.
   */
-void CAN1_RX0_IRQHandler(void)
+void TIM1_UP_TIM10_IRQHandler(void)
 {
-  /* USER CODE BEGIN CAN1_RX0_IRQn 0 */
+  /* USER CODE BEGIN TIM1_UP_TIM10_IRQn 0 */
 
-  /* USER CODE END CAN1_RX0_IRQn 0 */
-  HAL_CAN_IRQHandler(&hcan1);
-  /* USER CODE BEGIN CAN1_RX0_IRQn 1 */
+  /* USER CODE END TIM1_UP_TIM10_IRQn 0 */
+  HAL_TIM_IRQHandler(&htim1);
+  /* USER CODE BEGIN TIM1_UP_TIM10_IRQn 1 */
 
-  /* USER CODE END CAN1_RX0_IRQn 1 */
-}
-
-/**
-  * @brief This function handles CAN1 RX1 interrupt.
-  */
-void CAN1_RX1_IRQHandler(void)
-{
-  /* USER CODE BEGIN CAN1_RX1_IRQn 0 */
-
-  /* USER CODE END CAN1_RX1_IRQn 0 */
-  HAL_CAN_IRQHandler(&hcan1);
-  /* USER CODE BEGIN CAN1_RX1_IRQn 1 */
-
-  /* USER CODE END CAN1_RX1_IRQn 1 */
+  /* USER CODE END TIM1_UP_TIM10_IRQn 1 */
 }
 
 /* USER CODE BEGIN 1 */
diff --git a/Libs/TACOS b/Libs/TACOS
index 93a4112..16b2ed5 160000
--- a/Libs/TACOS
+++ b/Libs/TACOS
@@ -1 +1 @@
-Subproject commit 93a4112f595ca9e13bead8c2920a1bf302dc189c
+Subproject commit 16b2ed5a20df1c04b9bbcf5e12101d5e822e45d2
diff --git a/Libs/rtos2-utils b/Libs/rtos2-utils
index 9433b59..1925a16 160000
--- a/Libs/rtos2-utils
+++ b/Libs/rtos2-utils
@@ -1 +1 @@
-Subproject commit 9433b5990d155862454d5d7f99cf86e89a369ef0
+Subproject commit 1925a1668e847e048423499f523f3293ea9d923a
diff --git a/Libs/sta-core b/Libs/sta-core
index 62e275f..8901abd 160000
--- a/Libs/sta-core
+++ b/Libs/sta-core
@@ -1 +1 @@
-Subproject commit 62e275f667092645c5ad487a36ad7d00bd43cf52
+Subproject commit 8901abdb9cc393b3e7f2de295e75ba4205d6de39