diff --git a/Q24ECU/.vscode/c_cpp_properties.json b/Q24ECU/.vscode/c_cpp_properties.json
index 8d514fc1..f8712421 100644
--- a/Q24ECU/.vscode/c_cpp_properties.json
+++ b/Q24ECU/.vscode/c_cpp_properties.json
@@ -13,6 +13,7 @@
                 "${workspaceFolder}/core/include",
                 "${workspaceFolder}/core/include",
                 "${workspaceFolder}/vendor/cmsis_f4/Include",
+                "${workspaceFolder}/core/include",
                 "${workspaceFolder}/core/include"
             ],
             "defines": [
diff --git a/Q24ECU/.vscode/settings.json b/Q24ECU/.vscode/settings.json
index 8446ba6d..91189066 100644
--- a/Q24ECU/.vscode/settings.json
+++ b/Q24ECU/.vscode/settings.json
@@ -18,6 +18,8 @@
         "interface_uart.h": "c",
         "task.h": "c",
         "freertosconfig.h": "c",
-        "hal_can.h": "c"
+        "hal_can.h": "c",
+        "uart.h": "c",
+        "tasker.h": "c"
     },
 }
\ No newline at end of file
diff --git a/Q24ECU/core/config/FreeRTOSConfig.h b/Q24ECU/core/config/FreeRTOSConfig.h
index 06d0f3af..457d8471 100644
--- a/Q24ECU/core/config/FreeRTOSConfig.h
+++ b/Q24ECU/core/config/FreeRTOSConfig.h
@@ -64,7 +64,7 @@
 #define configCPU_CLOCK_HZ                       ( 180000000 )
 #define configTICK_RATE_HZ                       ((TickType_t)1000)
 #define configMAX_PRIORITIES                     ( 32 )
-#define configMINIMAL_STACK_SIZE                 ((uint16_t)256)
+#define configMINIMAL_STACK_SIZE                 ((uint16_t)128)
 #define configTOTAL_HEAP_SIZE                    ((size_t)15360)
 #define configMAX_TASK_NAME_LEN                  ( 16 )
 #define configUSE_16_BIT_TICKS                   0
@@ -94,6 +94,7 @@ to exclude the API function. */
 #define INCLUDE_eTaskGetState                1
 
 #define INCLUDE_xTaskDelayUntil              1
+#define INCLUDE_xSemaphoreGetMutexHolder     1
 
 /* Cortex-M specific definitions. */
 #ifdef __NVIC_PRIO_BITS
diff --git a/Q24ECU/core/include/interfaces/interface_adc.h b/Q24ECU/core/include/drivers/adc.h
similarity index 67%
rename from Q24ECU/core/include/interfaces/interface_adc.h
rename to Q24ECU/core/include/drivers/adc.h
index df3e8e1a..af632128 100644
--- a/Q24ECU/core/include/interfaces/interface_adc.h
+++ b/Q24ECU/core/include/drivers/adc.h
@@ -3,7 +3,7 @@
  * @file interface_adc.h
  * @author Jacob Chisholm (https://jchisholm204.github.io/)
  * @brief FreeRTOS ADC Interface for QFSAE VCU
- * @version 0.1
+ * @version 1.1
  * @date 2024-02-24
  * 
  * @copyright Copyright (c) 2024
@@ -14,7 +14,10 @@
 #include "stm32f4xx.h"
 #include "stdbool.h"
 #include "float.h"
+#include "inttypes.h"
 #include "hal/hal_adc.h"
+#include "hal/hal_dma.h"
+#include "hal/hal_gpio.h"
 #include "FreeRTOS.h"
 #include "pins.h"
 
@@ -40,21 +43,18 @@ enum ADC_CHANNEL {
 };
 
 /**
- * @brief OS ADC Setup Handler
+ * @brief ADC Initialization Handler
  * 
- * This function should be called to init the ADC bus and handler interfaces
+ * This function will initialize the ADC and DMA interfaces.
  * This function should not be called more than once
- * ALL CAN bus initialization should happen within this function
- * 
- * Called in main
  * 
  */
-extern void os_adc_setup(void);
+extern void adc_init(void);
 
 /**
- * @brief Fetch the last recorded voltage of an ADC channel
+ * @brief Read the last recorded voltage of an ADC channel
  * 
- * @param channel the channel to draw from
+ * @param channel the channel to read
  * @returns the voltage of the channel
  */
-extern double adc_fetch(enum ADC_CHANNEL channel);
+extern double adc_read(enum ADC_CHANNEL channel);
diff --git a/Q24ECU/core/include/drivers/can.h b/Q24ECU/core/include/drivers/can.h
new file mode 100644
index 00000000..6b929330
--- /dev/null
+++ b/Q24ECU/core/include/drivers/can.h
@@ -0,0 +1,58 @@
+/**
+ * @file can.h
+ * @author Jacob Chisholm (https://jchisholm204.github.io/)
+ * @brief FreeRTOS CAN bus Interface for QFSAE VCU
+ * @version 1.1
+ * @date 2024-03-04
+ * 
+ * Version 1.0
+ * - Created CAN Interface code
+ * Version 1.1
+ * - Refactored as a driver
+ * 
+ * @copyright Copyright (c) 2024
+ * 
+ */
+
+#pragma once
+#include "stm32f4xx.h"
+#include "stdbool.h"
+#include "hal/hal_can.h"
+#include "FreeRTOS.h"
+
+
+/**
+ * @brief CAN Initialization Handler
+ * 
+ * This function will initialize the CAN bus and handler interfaces
+ * This function should not be called more than once
+ * 
+ * @note This Driver initializes a FreeRTOS Task in order to function
+ */
+extern void can_init(void);
+
+
+/**
+ * @brief Get most current copy of a CAN message based on its ID
+ * 
+ * @param CAN The Physical CAN Bus the message was received on 
+ * @param id The CAN ID of the message
+ * @return can_msg_t 
+ */
+extern can_msg_t can_read(CAN_TypeDef *CAN, uint32_t id);
+
+
+/**
+ * @brief Send a message on the CAN Bus
+ * 
+ * @param CAN The Physical CAN bus to use
+ * @param tx_msg The CAN Message to send
+ * @param timeout The Timeout to wait to claim a CAN mailbox
+ * @returns SYS_OK or the associated error
+ */
+extern uint8_t can_write(CAN_TypeDef *CAN, can_msg_t *tx_msg, TickType_t timeout);
+
+/* All IRQ Handlers must be globally accessible for linking purposes */
+
+// CAN1 Receive IRQ Handler
+extern void CAN1_RX0_IRQHandler(void);
\ No newline at end of file
diff --git a/Q24ECU/core/include/interfaces/interface_spi.txt b/Q24ECU/core/include/drivers/spi.txt
similarity index 100%
rename from Q24ECU/core/include/interfaces/interface_spi.txt
rename to Q24ECU/core/include/drivers/spi.txt
diff --git a/Q24ECU/core/include/drivers/uart.h b/Q24ECU/core/include/drivers/uart.h
new file mode 100644
index 00000000..4c6d1b8e
--- /dev/null
+++ b/Q24ECU/core/include/drivers/uart.h
@@ -0,0 +1,120 @@
+/**
+ * @file uart.h
+ * @author Jacob Chisholm (Jchisholm204.github.io)
+ * @brief UART Driver (For FreeRTOS based system)
+ * @version 1.2
+ * @date 2024-03-04
+ * 
+ * Version 1.0
+ * - Created UART Interface code
+ * Version 1.1
+ * - Added RX Interrupt + Handler
+ * Version 1.2
+ * - Refactored as a driver (No task + buffer handling done outside of driver)
+ * 
+ * @copyright Copyright (c) 2024
+ * 
+ */
+
+#pragma once
+
+#include "hal/hal_uart.h"
+#include "FreeRTOS.h"
+#include "semphr.h"
+#include "stream_buffer.h"
+#include "errors.h"
+
+
+typedef struct UART_StreamBuffer {
+    char buffer[64];
+    StaticStreamBuffer_t staticBuffer;
+    StreamBufferHandle_t handle;
+} UART_StreamBuffer_t;
+
+
+/**
+ * @brief UART port typedef
+ * @param port The UART port (CMSIS UART typedef)
+ * @param writeHandler The semaphore controlling write access to the UART port
+ * @param readHandler The semaphore controlling read access to the UART receive buffer
+ * @param pbuffer Pointer to the receive buffer loaded by the UART IRQ Handler
+ * 
+ */
+typedef struct UART_Handle {
+    USART_TypeDef *pUART;
+    StaticSemaphore_t writeSemaphore;
+    xSemaphoreHandle writeHandler;
+    StaticSemaphore_t readSemaphore;
+    xSemaphoreHandle readHandler;
+    StreamBufferHandle_t *pBuffer;
+} UART_Handle_t;
+
+extern UART_Handle_t Serial2, Serial4;
+
+
+/**
+ * @brief UART Initialization Handler
+ * 
+ * This function will initialize all onboard UART interfaces.
+ * This function should not be called more than once.
+ * 
+ */
+extern void uart_init(unsigned long baud);
+
+/**
+ * @brief Open a UART RX Buffer (enables RX Interrupts)
+ * @note Buffer Handling must be done inside the calling function
+ * @param pHandle Pointer to the UART Handle to open
+ * @param stream A pointer to preallocated memory for the stream buffer
+ * @param timeout The amount of ticks to wait for the interface to become available
+ * @return SYS_OK if successful
+ */
+extern enum SYS_ERROR uart_open_buffer(UART_Handle_t *pHandle, UART_StreamBuffer_t *stream, TickType_t timeout);
+
+/**
+ * @brief Close a UART RX Buffer (disables RX Interrupts)
+ * 
+ * @param pHandle Pointer to the Handle to close
+ * @return SYS_OK if successful
+ */
+extern enum SYS_ERROR uart_close_buffer(UART_Handle_t *pHandle);
+
+/**
+ * @brief Write a byte to a UART port
+ * 
+ * @param pHandle Pointer to the Handle for the port to write to
+ * @param byte byte to write
+ * @param timeout The amount of ticks to wait for the interface to become available
+ * @return SYS_OK if successful
+ */
+extern enum SYS_ERROR uart_write_byte(UART_Handle_t *pHandle, char byte, TickType_t timeout);
+
+/**
+ * @brief Write a buffer to a UART port
+ * 
+ * @param pHandle Pointer to the Handle for the port to write to
+ * @param buf A pointer to the buffer to write
+ * @param timeout The amount of ticks to wait for the interface to become available
+ * @return SYS_OK if successful
+ */
+extern enum SYS_ERROR uart_write(UART_Handle_t *pHandle, char* buf, size_t len, TickType_t timeout);
+
+/**
+ * @brief Write a String to a uart port
+ * 
+ * @param pHandle Pointer to the Handle for the port to write to
+ * @param str The String to write
+ * @param timeout The amount of ticks to wait for the interface to become available
+ * @return SYS_OK if successful
+ */
+extern enum SYS_ERROR uart_write_str(UART_Handle_t *pHandle, char* str,  TickType_t timeout);
+
+
+/* All IRQ Handlers must be globally accessible for linking purposes */
+
+// USART2 IQR Handler
+extern void USART2_IRQHandler(void);
+
+// USART2 IQR Handler
+extern void UART4_IRQHandler(void);
+
diff --git a/Q24ECU/core/include/hal/hal_clock.h b/Q24ECU/core/include/hal/hal_clock.h
index e3cdb123..da7ddeca 100644
--- a/Q24ECU/core/include/hal/hal_clock.h
+++ b/Q24ECU/core/include/hal/hal_clock.h
@@ -57,3 +57,7 @@ static inline void clock_init(void){
     while (!(RCC->CFGR & RCC_CFGR_SWS_PLL)) __asm__("nop"); // Wait until done
 }
 
+static inline void spin(volatile uint32_t count) {
+  while (count--) __asm__("nop");
+}
+
diff --git a/Q24ECU/core/include/hal/hal_dma.h b/Q24ECU/core/include/hal/hal_dma.h
index 61b5e413..2b77ce8e 100644
--- a/Q24ECU/core/include/hal/hal_dma.h
+++ b/Q24ECU/core/include/hal/hal_dma.h
@@ -17,7 +17,9 @@
 #pragma once
 
 #include "stm32f4xx.h"
+#include <inttypes.h>
 #include "errors.h"
+#include <stddef.h>
 
 enum DMA_CHANNEL {
     DMA_CHANNEL_0 = 0,
@@ -54,6 +56,8 @@ enum DMA_MEM_SIZE {
  * @param periph_addr Peripheral Address
  * @param mem_addr Memory Address
  */
+
+
 static inline enum SYS_ERROR hal_dma_init(DMA_TypeDef *dma, DMA_Stream_TypeDef *stream, enum DMA_CHANNEL ch, enum DMA_PRIORITY priority, enum DMA_MEM_SIZE mem_size, volatile void *periph_addr, void *mem_addr, size_t num_transfers) {
     // Enable DMA Clock
     if (dma == DMA1) {
diff --git a/Q24ECU/core/include/hal/hal_gpio.h b/Q24ECU/core/include/hal/hal_gpio.h
index 616926d2..c6d7b688 100644
--- a/Q24ECU/core/include/hal/hal_gpio.h
+++ b/Q24ECU/core/include/hal/hal_gpio.h
@@ -4,7 +4,7 @@
 #include <stdbool.h>
 #include <sys/stat.h>
 
-#include <stm32f446xx.h>
+#include "stm32f4xx.h"
 #include "hal_clock.h"
 #include "pins.h"
 
@@ -27,9 +27,9 @@ enum GPIO_AF_MODE { GPIO_AF_SYS = 0, GPIO_AF_TIM1 = 1, GPIO_AF_TIM2 = 1, GPIO_AF
 static inline void gpio_set_mode(uint16_t pin, enum GPIO_MODE_IO mode) {
     GPIO_TypeDef *gpio = GPIO(PINBANK(pin));
     int n = PINNO(pin);
-    RCC->AHB1ENR |= BIT(PINBANK(pin));       // Enable GPIO clock
-    gpio->MODER &= ~(3U << (n*2));
-    gpio->MODER |= (mode & 3U) << (n * 2);   // Set new mode
+    SET_BIT(RCC->AHB1ENR, (uint32_t)(1UL << PINBANK(pin)));       // Enable GPIO clock
+    gpio->MODER &= (uint32_t)~(3U << (n*2));
+    gpio->MODER |= (uint32_t)((mode & 3U) << (n * 2));   // Set new mode
 
 }
 
@@ -42,8 +42,8 @@ static inline void gpio_set_mode(uint16_t pin, enum GPIO_MODE_IO mode) {
 static inline void gpio_set_af(uint16_t pin, enum GPIO_AF_MODE af) {
     GPIO_TypeDef *gpio = GPIO(PINBANK(pin));
     int n = PINNO(pin);
-    gpio->AFR[n >> 3] &= ~(15UL << ((n & 7) * 4));
-    gpio->AFR[n >> 3] |= ((uint32_t) af) << ((n & 7) * 4);
+    gpio->AFR[n >> 3] &= (uint32_t)~(15UL << ((n & 7) * 4));
+    gpio->AFR[n >> 3] |= (uint32_t)((uint32_t) af) << ((n & 7) * 4);
 }
 
 /**
@@ -54,28 +54,28 @@ static inline void gpio_set_af(uint16_t pin, enum GPIO_AF_MODE af) {
  */
 static inline void gpio_write(uint16_t pin, bool value) {
     GPIO_TypeDef *gpio = GPIO(PINBANK(pin));
-    gpio->BSRR = (1U << PINNO(pin)) << (value ? 0 : 16);
+    gpio->BSRR = (uint32_t)((1UL << PINNO(pin)) << (value ? 0 : 16));
 }
 
 static inline void gpio_toggle_pin(uint16_t pin){
     GPIO_TypeDef *gpio = GPIO(PINBANK(pin));
-    gpio->BSRR = (1UL << PINNO(pin)) << (gpio->ODR & (1UL << PINNO(pin)) ? 16 : 0);
+    gpio->BSRR = (uint32_t)((1UL << PINNO(pin)) << (gpio->ODR & (1UL << PINNO(pin)) ? 16 : 0));
 }
 
 static inline bool gpio_read_idr(uint16_t pin) {
     const GPIO_TypeDef *gpio = GPIO(PINBANK(pin));
-    return (gpio->IDR & (1U << PINNO(pin)));
+    return (bool)((gpio->IDR >> PINNO(pin)) & 0x1UL);
 }
 
 static inline bool gpio_read_odr(uint16_t pin){
     const GPIO_TypeDef *gpio = GPIO(PINBANK(pin));
-    return (gpio->ODR & (1U << PINNO(pin)));
+    return (bool)((gpio->ODR >> PINNO(pin)) & 0x1UL);
 }
 
 static inline void gpio_pull(uint16_t pin, enum GPIO_PULL_MODE mode){
     GPIO_TypeDef *gpio = GPIO(PINBANK(pin));
-    gpio->PUPDR &= ~(3U << (PINNO(pin)*2));
-    if(mode!=GPIO_RESET) gpio->PUPDR |= mode << (2*(PINNO(pin)));
+    gpio->PUPDR &= (uint32_t)~(3U << (PINNO(pin)*2));
+    if(mode!=GPIO_RESET) gpio->PUPDR |= (uint32_t)(mode << (2*(PINNO(pin))));
 }
 
 // t: expiration time, prd: period, now: current time. Return true if expired
diff --git a/Q24ECU/core/include/hal/hal_tim_basic.h b/Q24ECU/core/include/hal/hal_tim_basic.h
index a202aa50..f067f1ac 100644
--- a/Q24ECU/core/include/hal/hal_tim_basic.h
+++ b/Q24ECU/core/include/hal/hal_tim_basic.h
@@ -30,8 +30,8 @@ static inline void TIM_basic_Init(TIM_TypeDef *timer, uint16_t prescaler, uint16
     timer->DIER &= ~(BIT(8U) | BIT(0U)); // reset interrupt enable reg
     timer->DIER |= BIT(0); // enable timer interrupt
     // TIM6->EGR |= BIT(0);
-    timer->ARR = (reload_register-1);//(65536);
-    timer->PSC = (prescaler-1);//(APB1_FREQUENCY);
+    timer->ARR = (uint32_t)(reload_register-1UL);//(65536);
+    timer->PSC = (uint32_t)(prescaler-1UL);//(APB1_FREQUENCY);
     timer->CR1 &= ~(TIM_CR1_ARPE | TIM_CR1_OPM | TIM_CR1_URS | TIM_CR1_CEN); // reset timer control register 1
     timer->CR1 |= (TIM_CR1_CEN); // set interrupt source to only overflow | enable timer
 }
diff --git a/Q24ECU/core/include/hal/hal_uart.h b/Q24ECU/core/include/hal/hal_uart.h
index 7c4f2d4e..3c0fedfc 100644
--- a/Q24ECU/core/include/hal/hal_uart.h
+++ b/Q24ECU/core/include/hal/hal_uart.h
@@ -64,9 +64,6 @@ static inline uint8_t hal_uart_read_byte(const USART_TypeDef *uart) {
     return ((uint8_t) (uart->DR & 255));
 }
 
-static inline void spin(volatile uint32_t count) {
-  while (count--) __asm__("nop");
-}
 
 static inline void hal_uart_write_byte(USART_TypeDef * uart, uint8_t byte) {
     uart->DR = byte;
diff --git a/Q24ECU/core/include/interfaces/README.md b/Q24ECU/core/include/interfaces/README.md
new file mode 100644
index 00000000..1f8ecf0b
--- /dev/null
+++ b/Q24ECU/core/include/interfaces/README.md
@@ -0,0 +1,3 @@
+# Interfaces
+
+Interfaces act as abstraction between device drivers (CAN, UART, I2C) and devices on the car. For example, an interface may be used to abstract CAN bus communication between a task and the BMS.
\ No newline at end of file
diff --git a/Q24ECU/core/include/interfaces/cm200dx.h b/Q24ECU/core/include/interfaces/cm200dx.h
new file mode 100644
index 00000000..cc90b987
--- /dev/null
+++ b/Q24ECU/core/include/interfaces/cm200dx.h
@@ -0,0 +1,43 @@
+/**
+ * @file cm200dx.h
+ * @author Jacob Chisholm (https://Jchisholm204.github.io)
+ * @author Matt Dobaj
+ * @brief Cascadia CM200DX Inverter - CAN Bus Interface
+ * @version 0.1
+ * @date 2024-03-14
+ * 
+ * Example code for an Interface
+ *
+ * @copyright Copyright (c) 2024
+ * 
+ */
+
+#pragma once
+
+#include "drivers/can.h"
+
+enum CM200DX {
+    LEFT = 0x100,
+    RIGHT = 0x200
+};
+
+
+static inline uint16_t cm200dx_getRPM(enum CM200DX baseID){
+    can_msg_t msg =  can_read(CAN1, baseID);
+    return (uint16_t)(msg.data[0] | msg.data[1]);
+}
+
+
+static inline uint16_t cm200dx_getPower(enum CM200DX baseID){
+    return (uint16_t)(100U*can_read(CAN1, baseID).data[3]);
+}
+
+static inline void cm200dx_setPower(enum CM200DX baseID, uint8_t power){
+    can_msg_t msg;
+    msg.id =  baseID+1UL;
+    msg.len = 1;
+    msg.data[0] = power;
+    can_write(CAN1, &msg, 10U);
+}
+
+
diff --git a/Q24ECU/core/include/interfaces/interface_can.h b/Q24ECU/core/include/interfaces/interface_can.h
deleted file mode 100644
index 485a0629..00000000
--- a/Q24ECU/core/include/interfaces/interface_can.h
+++ /dev/null
@@ -1,62 +0,0 @@
-/**
- * @file interface_can.h
- * @author Jacob Chisholm (https://jchisholm204.github.io/)
- * @brief FreeRTOS CAN bus Interface for QFSAE VCU
- * @version 0.1
- * @date 2023-12-31
- * 
- * @copyright Copyright (c) 2023
- * 
- */
-#pragma once
-#include "stm32f4xx.h"
-#include "stdbool.h"
-#include "hal/hal_can.h"
-#include "FreeRTOS.h"
-
-
-/**
- * @brief OS CAN Setup Handler
- * 
- * This function should be called to init the CAN bus and handler interfaces
- * This function should not be called more than once
- * ALL CAN bus initialization should happen within this function
- * 
- * Called in main
- * 
- */
-extern void os_can_setup(void);
-
-extern void CAN1_RX0_IRQHandler(void);
-
-/**
- * @brief Get most current copy of a CAN message based on its ID
- * 
- * @param CAN The Physical CAN Bus the message was received on 
- * @param id The CAN ID of the message
- * @return can_msg_t 
- */
-extern can_msg_t can_fetch(CAN_TypeDef *CAN, uint32_t id);
-
-/**
- * @brief Check if a CAN message has gone stale based on its receive time
- * 
- * @param CAN The Physical CAN Bus the message was received on
- * @param id The CAN ID of the message
- * @param maxTicks The maximum time before a message is considered stale.
- * @return true if the message is still good
- * @return false if the message is out of date
- */
-extern bool can_check_timestamp(CAN_TypeDef *CAN, uint32_t id, uint32_t maxTicks);
-
-/**
- * @brief Send a message on the CAN Bus
- * 
- * @param CAN The Physical CAN bus to use
- * @param tx_msg The CAN Message to send
- * @param timeout The Timeout to wait to claim a CAN mailbox
- * @returns SYS_OK or the associated error
- */
-extern uint8_t can_send_msg(CAN_TypeDef *CAN, can_msg_t *tx_msg, TickType_t timeout);
-
-
diff --git a/Q24ECU/core/include/interfaces/interface_sysError.h b/Q24ECU/core/include/interfaces/interface_sysError.h
deleted file mode 100644
index 6ca7c336..00000000
--- a/Q24ECU/core/include/interfaces/interface_sysError.h
+++ /dev/null
@@ -1,79 +0,0 @@
-/**
- * @file interface_sysError.h
- * @author Jacob Chisholm (https://Jchisholm204.github.io)
- * @brief Error reporting interface
- * @version 0.1
- * @date 2024-02-17
- * 
- * @copyright Copyright (c) 2024
- * 
- * Interface for system error reporting to 7 segment displays.
- * Console logging can be enable with the `PRINTF_REPORT_ERR` macro.
- * 
- * *Note: Seven Segment Displays are only available on VCU Hardware >= V2*
- * 
- */
-
-#pragma once
-#include "interfaces/interface_uart.h"
-#include "FreeRTOS.h"
-#include "task.h"
-#include "taskHandlers.h"
-#include "stream_buffer.h"
-#include "errors.h"
-
-//  Enable console logging of errors
-#define PRINTF_REPORT_ERR
-
-/**
- * @brief OS Error Setup Handler
- * 
- * This function should be called to init the error arrays and handler interfaces
- * This function should not be called more than once
- * 
- * Called in main
- * 
- */
-extern void os_sysError_setup(void);
-
-/**
- * @brief Report a system error. Passing `SYS_OK` will do nothing
- * 
- * @param error <enum SYS_ERROR> The error to report
- */
-extern void sysError_report(enum SYS_ERROR error);
-
-/**
- * @brief Report a system error from a task (Suspends the task)
- * 
- * @param error <enum SYS_ERROR> The error to report
- * @param task <TaskHandle_t> The task that reported the error
- */
-extern void sysError_reportTsk(enum SYS_ERROR error, TaskHandle_t *task);
-
-/**
- * @brief Clear a system error
- * 
- * @param error <enum SYS_ERROR> The error to clear
- */
-extern void sysError_clear(enum SYS_ERROR error);
-
-/**
- * @brief Check if an error is currently active
- * 
- * @param error <enum SYS_ERROR> The error to check
- * @return true if the error is active
- * @return false if the error is clean
- */
-extern bool sysError_check(enum SYS_ERROR error);
-
-/**
- * @brief Check if an error has been active since last reset
- * 
- * @param error <enum SYS_ERROR> The error to check
- * @return true if the error was active at any time
- * @return false if the error has never been active
- */
-extern bool sysError_checkRuntime(enum SYS_ERROR error);
-
-
diff --git a/Q24ECU/core/include/interfaces/interface_uart.h b/Q24ECU/core/include/interfaces/interface_uart.h
deleted file mode 100644
index e754d495..00000000
--- a/Q24ECU/core/include/interfaces/interface_uart.h
+++ /dev/null
@@ -1,112 +0,0 @@
-/**
- * @file interface_uart.h
- * @author Jacob Chisholm (Jchisholm204.github.io)
- * @brief uart FreeRTOS Interface with Semaphore Handler
- * @version 0.1
- * @date 2023-11-15
- * 
- * @copyright Copyright (c) 2023
- * 
- */
-
-#pragma once
-
-#include "hal/hal_uart.h"
-#include "FreeRTOS.h"
-#include "semphr.h"
-#include "pins.h"
-#include "stream_buffer.h"
-#include "errors.h"
-
-
-/**
- * @brief UART port typedef
- * @param port The UART port (CMSIS UART typedef)
- * @param semaphore The semaphore controlling the UART port
- * @param rxbuffer The receive buffer loaded by the UART IRQ Handler
- * 
- */
-typedef struct uart_t {
-    USART_TypeDef *port;
-    xSemaphoreHandle semaphore;
-    StreamBufferHandle_t rxbuffer;
-} uart_t;
-
-// UART OS Handlers
-extern uart_t port_uart2;
-extern uart_t port_uart4;
-
-/**
- * @brief OS UART Setup handler
- * 
- * This function should be called to initalized all onboard UART interfaces.
- * This function should not be called more than once.
- * All UART initialization should happen within this function
- * 
- * Called in main
- * 
- */
-extern void os_uart_setup(void);
-
-
-/**
- * @brief Initialize a USART device and its Semaphore
- * 
- * @param port The USART_t typedef handle
- * @param uart the USARTx define from CMSIS headers
- * @param baud USART Baud Rate
- */
-static inline void uart_send_init(uart_t *port, USART_TypeDef *uart, unsigned long baud, uint16_t pin_tx, uint16_t pin_rx){
-    port->semaphore = xSemaphoreCreateBinary();
-    if(port->semaphore == NULL) return;
-    port->rxbuffer = xStreamBufferCreate(64, 1);
-    if(port->rxbuffer == NULL) return;
-    port->port = uart;
-    if(port->port == NULL) return;
-    hal_uart_init(port->port, baud, pin_tx, pin_rx);
-    xSemaphoreGive(port->semaphore);
-}
-
-
-// USART2 IQR Handler
-extern void USART2_IRQHandler(void);
-
-/**
- * @brief Task Blocking command to send a byte over uart
- * 
- * @param port The uart_t port to use (Must be initialized)
- * @param byte The Byte to transmit
- * @param timeout The amount of ticks to wait for the interface to become available
- */
-static inline int uart_send_blocking(uart_t *port, uint8_t byte, TickType_t timeout){
-    if(port == NULL)            return UART_UNINIT_ERR;
-    if(port->port == NULL)      return UART_UNINIT_ERR;
-    if(port->semaphore == NULL) return UART_UNINIT_ERR;
-    if(xSemaphoreTake(port->semaphore, timeout) == pdTRUE){
-        hal_uart_write_byte(port->port, byte);
-        xSemaphoreGive(port->semaphore);
-        return SYS_OK;
-    }
-    return UART_ACC_ERR;
-}
-
-/**
- * @brief Task Blocking command to send a buffer over uart
- * 
- * @param port The uart_t port to use (Must be initialized)
- * @param buf Data Buffer
- * @param len Length of Data Buffer
- * @param timeout The amount of ticks to wait for the interface to become available 
- */
-static inline int uart_send_buf_blocking(uart_t *port, char* buf, size_t len, TickType_t timeout){
-    if(port == NULL)            return UART_UNINIT_ERR;
-    if(port->port == NULL)      return UART_UNINIT_ERR;
-    if(port->semaphore == NULL) return UART_UNINIT_ERR;
-    if(xSemaphoreTake(port->semaphore, timeout) == pdTRUE){
-        hal_uart_write_buf(port->port, buf, len);
-        xSemaphoreGive(port->semaphore);
-        return SYS_OK;
-    }
-    return UART_ACC_ERR;
-}
-
diff --git a/Q24ECU/core/include/interrupts.h b/Q24ECU/core/include/interrupts.h
deleted file mode 100644
index f078bd3c..00000000
--- a/Q24ECU/core/include/interrupts.h
+++ /dev/null
@@ -1,17 +0,0 @@
-/**
- * @file interrupts.h
- * @author Jacob Chisholm (https://jchisholm204.github.io/)
- * @brief Global definitions of all interrupt handler functions
- * @version 0.1
- * @date 2023-11-22
- * 
- * @copyright Copyright (c) 2023
- * 
- * This file MUST be part of the main.h includes list
- * 
- */
-
-#pragma once
-
-extern void TIM6_DAC_IRQHandler(void);
-
diff --git a/Q24ECU/core/include/main.h b/Q24ECU/core/include/main.h
index 93083055..1cdeb2e8 100644
--- a/Q24ECU/core/include/main.h
+++ b/Q24ECU/core/include/main.h
@@ -10,13 +10,19 @@
  */
 
 #include <stm32f4xx.h>
-#include "taskHandlers.h"
+#include "sys_tasks.h"
+#include "sys_state.h"
 #include "nvicConfig.h"
-#include "interrupts.h"
 
 // Comment this out to remove task names from the debug print
 #define DEBUG_PRINTF_TASK_NAME
 
+
+/* Interrupts globally defined for linker */
+
+// Timer 6 IRQn
+extern void TIM6_DAC_IRQHandler(void);
+
 // Located in shutdown.c
 extern void system_critical_shutdown(void);
 
diff --git a/Q24ECU/core/include/sys_state.h b/Q24ECU/core/include/sys_state.h
new file mode 100644
index 00000000..1beb0923
--- /dev/null
+++ b/Q24ECU/core/include/sys_state.h
@@ -0,0 +1,120 @@
+/**
+ * @file core.h
+ * @author Jacob Chisholm (https://Jchisholm204.github.io)
+ * @brief Main System Runtime Header
+ * @version 0.1
+ * @date 2024-03-04
+ * 
+ * This file serves as the header file for the main system runtime
+ * The Core is responsible for system startup, excluding the bootloader, and hardware interfaces
+ * that could prevent the system from starting up properly.
+ * 
+ * The Core's main function is to initialize and maintain the acceleration throttle control (ATC)
+ * tasks including the ATC task itself and the Software Differential Control (SDC) task.
+ * 
+ * @copyright Copyright (c) 2024
+ * 
+ */
+#pragma once
+
+#include "FreeRTOS.h"
+#include "FreeRTOSConfig.h"
+#include "task.h"
+#include "errors.h"
+#include <stdbool.h>
+
+/* BEGIN System State Control Code */
+
+// System State Tracker
+enum SYS_STATE {
+    SYS_STATE_INIT,
+    SYS_STATE_IDLE,
+    SYS_STATE_TEST,
+    SYS_STATE_TS_ACTIVE,
+    SYS_STATE_RTD,
+    SYS_STATE_ERR
+};
+
+extern enum SYS_STATE controller_get_state(void);
+
+/* END System State Control Code */
+
+extern TaskHandle_t xControllerTaskHandle;
+extern void vTask_Controller(void *param);
+
+/* BEGIN System State Functions */
+
+/*  There is no initialization state, Initialization should happen directly within the core
+    function. Idle tasks should be resumed within the vState_Idle function
+
+    States run in the order specified below, and within the core task. Each state is responsible
+    for managing the system during its runtime. Once a state has completed, it must exit to the core task.
+    Upon returning to the core task, the system state will be checked against the error state before the
+    next state starts. States are looping, ie the Ready To Drive state returns to the idle state.
+*/
+
+/**
+ * @brief System IDLE State
+ * 
+ * @param state Pointer to the current system state
+ * @returns When the Tractive System Enable Switch is ON
+ */
+extern void vState_Idle(enum SYS_STATE *state);
+
+/**
+ * @brief Tractive System Start State
+ * 
+ * @param state Pointer to the current system state
+ * 
+ * @returns When Pre-Charge has completed and the driver has completed the steps to enable driving
+ */
+extern void vState_Start(enum SYS_STATE *state);
+
+/**
+ * @brief Ready To Drive State (Runs while the car responds to APPS input)
+ * 
+ * @param state Pointer to the current system state
+ * @returns When the driver shuts down the car, or when an error is encountered
+ */
+extern void vState_RTD(enum SYS_STATE *state);
+
+/**
+ * @brief System Error Checking State
+ * This state function checks to see if an error is present,
+ * and attempts to solve it.
+ * @param state Pointer to the current system state
+ * @returns if the error is resolvable
+ */
+extern void vState_Error(enum SYS_STATE *state);
+
+/* END System State Functions */
+
+/* BEGIN System Error Handling */
+
+/**
+ * @brief Throw an error to the system state controller
+ * 
+ * @param e The SYS_ERROR to throw
+ */
+extern void controller_throw(enum SYS_ERROR e);
+
+/**
+ * @brief Check if an error is present in the system state controller
+ * 
+ * @param e The Error to check
+ * @return true if the error is present
+ * @return false if the error has not been reported
+ */
+extern bool controller_check_error(enum SYS_ERROR e);
+
+/**
+ * @brief Check if any faults have been reported to the system state controller
+ * 
+ * @return true if an unresolved error has been reported
+ */
+extern bool controller_check_fault(void);
+
+/* END System Error Handling */
+
+
+
diff --git a/Q24ECU/core/include/sys_tasks.h b/Q24ECU/core/include/sys_tasks.h
new file mode 100644
index 00000000..4f635d8a
--- /dev/null
+++ b/Q24ECU/core/include/sys_tasks.h
@@ -0,0 +1,117 @@
+/**
+ * @file tasks.h
+ * @author Jacob Chisholm (https://jchisholm204.github.io/)
+ * @brief Central collection of all system tasks + initialization functions
+ * @version 1.0
+ * @date 2024-03-10
+ * 
+ * @copyright Copyright (c) 2024
+ * 
+ */
+
+#pragma once
+#include "drivers/uart.h"
+
+enum Tasks{
+    // Software BSPD (Brake System Plausibility Device)
+    eTask_SBSPD,
+    // 7 Segment Error Screen Control Task
+    eTask_ErrorScreen,
+    // Test Task
+    eTask_TEST1,
+    // CAN Bus Reporting Task
+    eTask_CANSend,
+    // Keep this last
+    eTask_TaskCount // Number of tasks running on the system (`taskHandlers.h`)
+};
+
+/* Global define for all system task handles */
+extern TaskHandle_t xTaskHandles[eTask_TaskCount];
+
+/**
+ * @brief Suspend a task if it is currently running
+ * 
+ * @param task The Task to suspend
+ */
+static inline void task_Suspend(enum Tasks task){
+    if(eTaskGetState(xTaskHandles[task]) != eSuspended)
+        vTaskSuspend(xTaskHandles[task]);
+}
+
+/**
+ * @brief Resume a previously suspended task
+ * 
+ * @param task 
+ */
+static inline void task_Resume(enum Tasks task){
+    if(eTaskGetState(xTaskHandles[task]) == eSuspended)
+        vTaskResume(xTaskHandles[task]);
+}
+
+/**
+ * @brief Print out debug information about all tasks on the system
+ * 
+ * @param Serial The Serial port to write the information to
+ */
+static inline void task_printDebug(UART_Handle_t *Serial){
+    char buf[100];
+    snprintf(buf, 100, "Task:\t   Identifier\t    Priority\t\tState\n");
+    uart_write_str(Serial, buf, 10);
+    for (int i = 0; i < eTask_TaskCount; i++)
+    {
+        snprintf(buf, 100, "%d: %16s\t\t%lu\t\t", i, pcTaskGetName(xTaskHandles[i]), uxTaskPriorityGet(xTaskHandles[i]));
+    uart_write_str(Serial, buf, 10);
+        switch(eTaskGetState(xTaskHandles[i])) {
+            case eRunning:
+                uart_write(Serial, "RUNNING\n", 9, 10);
+                break;
+            case eReady:
+                uart_write(Serial, "READY\n", 6, 10);
+                break;
+            case eBlocked:
+                uart_write(Serial, "BLOCKED\n", 8, 10);
+                break;
+            case eSuspended:    
+                uart_write(Serial, "SUSPENDED\n", 10, 10);
+                break;
+            case eDeleted:   
+                uart_write(Serial, "DELETED\n", 8, 10);
+                break;
+            case eInvalid:   
+                uart_write(Serial, "INVALID\n", 8, 10);
+                break;
+            default:
+                uart_write(Serial, "UNKNOWN\n", 8, 10);
+                break;
+        }
+    }
+}
+
+
+/**
+ * @brief Used to initialize all system tasks 
+ * Tasks are initialized to a halted state
+ * 
+ */
+extern void tasker_init(void);
+
+/**
+ * @brief Helper function to print out all data pertaining to the systems tasks
+ * 
+ */
+extern void task_printDebug(UART_Handle_t *Serial);
+
+/*  BEGIN Global Definitions of all System Tasks */
+
+extern void vTask_SBSPD(void *param);
+
+extern void vTask_ErrorScreen(void *param);
+
+extern void vTask_Test1(void *param);
+
+extern void vTask_CAN_send(void *param);
+
+/*  END Global Definitions of all System Tasks */
+
+
+
diff --git a/Q24ECU/core/include/taskHandlers.h b/Q24ECU/core/include/taskHandlers.h
deleted file mode 100644
index dee4fc5b..00000000
--- a/Q24ECU/core/include/taskHandlers.h
+++ /dev/null
@@ -1,58 +0,0 @@
-/**
- * @file taskHandlers.h
- * @author Jacob Chisholm
- * @brief Global definitions of all task handlers defined in main.c
- * @version 0.1
- * @date 2023-11-19
- * 
- * @copyright Copyright (c) 2023
- * 
- */
-
-#pragma once
-
-#include "FreeRTOS.h"
-#include "FreeRTOSConfig.h"
-#include "task.h"
-
-enum Tasks{
-    eTask_Test1,
-    eTask_BlinkLED,
-    eTask_USART2_Handler,
-    eTask_CAN_send,
-    eTask_CAN_receive,
-    eTask_CAN_rxBufferHandler,
-    eTask_SysError,
-    eTask_ADCMonitor,
-    // Keep this last
-    eTask_TaskCount // Number of tasks running on the system (`taskHandlers.h`)
-};
-
-extern TaskHandle_t xTaskHandles[eTask_TaskCount];
-
-extern void os_task_init(void);
-
-// Located in testTasks.c
-extern void vTask_Test1(void *param);
-
-// Located in testTasks.c
-extern void vTask_BlinkLED(void *param);
-
-// Located in interface_uart.c
-extern void vTask_USART2_Handler(void *param);
-
-// Located in interface_can.c
-extern void vTask_CAN_RXBufferHandler(void *param);
-
-// Located in canRunner.c
-extern void vTask_CAN_send(void *param);
-
-// Located in canRunner.c
-extern void vTask_CAN_receive(void *param);
-
-// Located in interface_sysError.c
-extern void vTask_SysError(void *param);
-
-// Located in interface_adc.c
-extern void vTask_ADCMonitor(void *param);
-
diff --git a/Q24ECU/core/src/drivers/README.md b/Q24ECU/core/src/drivers/README.md
new file mode 100644
index 00000000..3a96f457
--- /dev/null
+++ b/Q24ECU/core/src/drivers/README.md
@@ -0,0 +1,21 @@
+# Drivers
+Drivers are hardware software interfaces that act as a middle ground between the HAL (Hardware Abstraction Layer) and system tasks.
+
+Drivers should use FreeRTOS Semaphores for write and read access where needed and should avoid using tasks when possible.
+
+### Structure
+
+All Drivers should contain write and read methods in addition to an init method as described below. UART is used as an example.
+
+Init Method: `void uart_init(void)`
+Write Method: `SYS_ERROR uart_write(pHandle, data, timeout)`
+Read Method:
+    Depending on the interface the read method can be implemented in one of two ways.
+
+1. Buffers
+
+If the interface requires a buffer for receiving data then the read method may be implemented by providing `*_open_buffer` and `*_close_buffer` methods. In this case, the task opening the buffer must be responsible for all data allocation.
+
+2. Return Methods
+
+If the driver has a simple return type it can be implemented with `*_read(pHandle)`
\ No newline at end of file
diff --git a/Q24ECU/core/src/drivers/adc.c b/Q24ECU/core/src/drivers/adc.c
new file mode 100644
index 00000000..1a542ff2
--- /dev/null
+++ b/Q24ECU/core/src/drivers/adc.c
@@ -0,0 +1,62 @@
+/**
+ * @file adc.c
+ * @author Jacob Chisholm (https://Jchisholm204.github.io)
+ * @brief Source File for ADC Driver
+ * @version 0.1
+ * @date 2024-03-10
+ * 
+ * @copyright Copyright (c) 2024
+ * 
+ */
+
+#include "drivers/adc.h"
+#include "hal/hal_adc.h"
+#include "hal/hal_dma.h"
+#include "hal/hal_gpio.h"
+#include "hal/hal_clock.h"
+
+uint16_t ADC_READINGS[ADC_CHANNEL_MAX];
+
+void adc_init(void){
+    
+    // Zero out ADC Readings
+    for(int i = 0; i < ADC_CHANNEL_MAX; i++){
+        ADC_READINGS[i] = 0;
+    }
+
+    // Reduce the ADC Clock to APB2/8
+    SET_BIT(ADC123_COMMON->CCR, 3 << 16);
+
+    // Configure Analog GPIO Pins
+    gpio_set_mode(PIN_A5Vin_1, GPIO_MODE_ANALOG);
+    gpio_set_mode(PIN_A5Vin_2, GPIO_MODE_ANALOG);
+
+    // Setup DMA for ADC1
+    hal_dma_init(DMA2, DMA2_Stream0, DMA_CHANNEL_0, DMA_PRIORITY_LOW, DMA_MEM_SIZE_16, &(ADC1->DR), ADC_READINGS, ADC_CHANNEL_MAX);
+    // Setup ADC1
+    hal_adc_init(ADC1, ADC_RESOLUTION_12_BIT);
+    // Setup Channel Sequence
+    hal_adc_configChannel(ADC1, PIN_A5Vin_1_ADCCH, ADC_CYCLES_480, ADC_SQ1);
+    hal_adc_configChannel(ADC1, PIN_A5Vin_2_ADCCH, ADC_CYCLES_480, ADC_SQ2);
+    // Set Sequence Length
+    hal_adc_set_sequence_len(ADC1, ADC_CHANNEL_MAX);
+
+    spin(9999999UL); // Wait for ADC to stabilize
+
+    // Enable the DMA Stream
+    hal_dma_start(DMA2_Stream0);
+    // Enable ADC and Start Conversions
+    hal_adc_startConversions(ADC1);
+}
+
+double adc_read(enum ADC_CHANNEL channel){
+    // Return obviously bad data if ADC is not enabled
+    if(!READ_BIT(ADC1->CR2, ADC_CR2_ADON)) return 123.456;
+    // Return 0 if ADC has encountered an overrun
+    if(READ_BIT(ADC1->SR, ADC_SR_OVR)) return 0.0;
+    // Return 0 if channel is out of range
+    if(channel > ADC_CHANNEL_MAX) return 0.0;
+    // TODO: add scaling for 12v/5v
+    return ADC_READINGS[channel]*3.3/4096.0;
+}
+
diff --git a/Q24ECU/core/src/interfaces/interface_can.c b/Q24ECU/core/src/drivers/can.c
similarity index 56%
rename from Q24ECU/core/src/interfaces/interface_can.c
rename to Q24ECU/core/src/drivers/can.c
index 5b52b9d0..5fdd1f02 100644
--- a/Q24ECU/core/src/interfaces/interface_can.c
+++ b/Q24ECU/core/src/drivers/can.c
@@ -1,82 +1,89 @@
-#include "interfaces/interface_can.h"
-#include "stdio.h"
-#include "FreeRTOSConfig.h"
+/**
+ * @file can.c
+ * @author Jacob Chisholm (https://Jchisholm204.github.io)
+ * @brief Source File for CAN Driver
+ * @version 1.1
+ * @date 2024-03-10
+ * 
+ * @copyright Copyright (c) 2024
+ * 
+ */
+
+#include "drivers/can.h"
 #include "FreeRTOS.h"
-#include "task.h"
-#include "stream_buffer.h"
 #include "semphr.h"
+#include "stream_buffer.h"
+#include "task.h"
 #include "nvicConfig.h"
-#include "taskHandlers.h"
+#include <stdio.h>
 
+// Static arrays for storing CAN messages
+#define CAN_DATA_SIZE 1024
+can_msg_t CAN1_DATA[CAN_DATA_SIZE];
+// can_msg_t CAN2_DATA[CAN_DATA_SIZE];
 
-
-// Static Arrays for storing CAN messages
-#define CAN_DATA_ARR_SIZE 1024
-can_msg_t CAN1_DATA[CAN_DATA_ARR_SIZE];
-can_msg_t CAN2_DATA[CAN_DATA_ARR_SIZE];
-
-uint32_t can1_hash(uint32_t id){
-    return id % CAN_DATA_ARR_SIZE;
-}
-
-uint32_t can2_hash(uint32_t id){
-    return id % CAN_DATA_ARR_SIZE;
-}
-
+#define CAN2_HASH(id) (id % CAN_DATA_SIZE)
+#define CAN1_HASH(id) (id % CAN_DATA_SIZE)
 
 // Static Buffer struct for CAN Stream buffers
-struct can_streambuffer{
+struct CAN_StreamBuffer{
     can_msg_t buffer[64];
     StaticStreamBuffer_t RXStaticBuffer;
     StreamBufferHandle_t streamHandle;
 };
 
 // Static Buffers for CAN recieve channels
-static struct can_streambuffer CAN1_RX;
-//static struct can_streambuffer CAN2_RX; ///    Transmission
-
+static struct CAN_StreamBuffer CAN1_StreamBuffer;
 
 // Definitions for CAN Transmit Mailbox Numberings
-#define CAN_TX_SEMAPHORE_COUNT 0
-#define CAN_TX_SEMAPHORE_TX0   1
-#define CAN_TX_SEMAPHORE_TX1   2
-#define CAN_TX_SEMAPHORE_TX2   3
+enum CAN_TX_SEMAPHORES{
+    CAN_TX_SEMAPHORE_COUNT = 0,
+    CAN_TX_SEMAPHORE_TX0,
+    CAN_TX_SEMAPHORE_TX1,
+    CAN_TX_SEMAPHORE_TX2
+};
 // Create Semaphores for CAN1 TX mailboxes
 SemaphoreHandle_t        CAN1_TX_Semaphore[4];         // Semaphores for CAN1 Transmit Mailboxes
 static StaticSemaphore_t CAN1_TX_SemaphoreBuffer[4];   // Static Buffer for CAN1 Semaphores
 
-
-// Create Binary semaphores for CAN 2 TX mailboxes
-//SemaphoreHandle_t        CAN1_TX_Semaphore;         // Counting Semaphore for CAN1 Transmit Mailboxes
-//static StaticSemaphore_t CAN1_TX_SemaphoreBuffer;   // Static Buffer for CAN1 Counting Semaphore
-//SemaphoreHandle_t        CAN1_TX0_Semaphore;        // CAN1 TX 0 Mailbox
-//static StaticSemaphore_t CAN1_TX0_SemaphoreBuffer;  // Static Buffer for TX0 Semaphore
-//SemaphoreHandle_t CAN1_TX1_Semaphore;               // CAN1 TX 1 Mailbox
-//static StaticSemaphore_t CAN1_TX1_SemaphoreBuffer;  // Static Buffer for TX1 Semaphore
-//SemaphoreHandle_t CAN1_TX2_Semaphore;               // CAN1 TX 2 Mailbox
-//static SemaphoreHandle_t CAN1_TX2_SemaphoreBuffer;  // Static Buffer for TX2 Semaphore
-
-
-
-
+// Static Task for CAN RX Buffer Handler
+static TaskHandle_t xTaskHandler_CANRX = NULL;
+static StaticTask_t xTaskBuffer_CANRX;
+static StackType_t xTaskStack_CANRX[configMINIMAL_STACK_SIZE];
 /**
- * @brief OS CAN Setup Handler
- * 
- * This function should be called to init the CAN bus and handler interfaces
- * This function should not be called more than once
- * ALL CAN bus initialization should happen within this function
- * 
- * Called in main
+ * @brief CAN RX Buffer Handler Task
  * 
+ * @param param NULL
  */
-void os_can_setup(void){
-    // NOTE: NART (no automatic re-transmission) should be set to false in application
+void vTask_CAN_RXBufferHandler(void *param);
+
+void can_init(void){
+    // Zero out CAN1 Data
+    for(int i = 0; i < CAN_DATA_SIZE; i++){
+        CAN1_DATA[i].id = 0;
+        CAN1_DATA[i].len = 0;
+        for(int j = 0; j < 8; j++){
+            CAN1_DATA[i].data[j] = 0;
+        }
+    }
+
+    // Zero out CAN2 Data
+    // for(int i = 0; i < CAN_DATA_SIZE; i++){
+    //     CAN2_DATA[i].id = 0;
+    //     CAN2_DATA[i].len = 0;
+    //     for(int j = 0; j < 8; j++){
+    //         CAN2_DATA[i].data[j] = 0;
+    //     }
+    // }
+
+    // Specific to VCU V0 => Write CAN pin low before initialization
     gpio_set_mode(PIN('A', 10), GPIO_MODE_OUTPUT);
     gpio_write(PIN('A', 10), false);
+    // NOTE: NART (no automatic re-transmission) should be set to false in application
     uint8_t can1_status = hal_can_init(CAN1, CAN_1000KBPS, true, PIN('A', 11), PIN('A', 12));
     // On event of CAN bus initialization failure
     if(can1_status != SYS_OK){
-        printf("Initialization Failure: CAN Bus Error: %d", can1_status);
+        // printf("Initialization Failure: CAN Bus Error: %d", can1_status); // No printf statements in production code
         for(;;) __asm__("nop");
     }
 
@@ -100,6 +107,20 @@ void os_can_setup(void){
     CAN1_TX_Semaphore[CAN_TX_SEMAPHORE_TX2] =
         xSemaphoreCreateBinaryStatic(&CAN1_TX_SemaphoreBuffer[CAN_TX_SEMAPHORE_TX2]);
     xSemaphoreGive(CAN1_TX_Semaphore[CAN_TX_SEMAPHORE_TX2]);
+
+
+
+
+    // Create the CAN RX Buffer Handler Task
+    xTaskHandler_CANRX = xTaskCreateStatic(
+        vTask_CAN_RXBufferHandler,
+        "CAN IRQ RX",
+        configMINIMAL_STACK_SIZE,
+        NULL,
+        tskIDLE_PRIORITY+2,
+        xTaskStack_CANRX,
+        &xTaskBuffer_CANRX
+    );
 }
 
 // CAN1 RX IRQ Handler (loads rx stream buffer)
@@ -115,7 +136,7 @@ void CAN1_RX0_IRQHandler(void){
 
     // Send the recieved data into the stream buffer
     xStreamBufferSendFromISR(
-            CAN1_RX.streamHandle,     // Streambuffer to send to
+            CAN1_StreamBuffer.streamHandle,     // Streambuffer to send to
             &rx_msg,                  // Data to copy into the buffer
             sizeof(can_msg_t),        // Size of data
             &xHigherPriorityTaskWoken // Checks if any tasks are waiting on buffer
@@ -133,60 +154,45 @@ void vTask_CAN_RXBufferHandler(void *param){
     
 
     // Create the Recieve Stream buffers
-    CAN1_RX.streamHandle = xStreamBufferCreateStatic(
+    CAN1_StreamBuffer.streamHandle =  xStreamBufferCreateStatic(
             (64*sizeof(can_msg_t)),     // Size of the buffer (in bytes)
             sizeof(can_msg_t),          // Size of Messages in buffer
-            (uint8_t*)CAN1_RX.buffer,   // Pointer to the static buffer
-            &CAN1_RX.RXStaticBuffer     // Pointer to the streambuffer handle
+            (uint8_t*)CAN1_StreamBuffer.buffer,   // Pointer to the static buffer
+            &CAN1_StreamBuffer.RXStaticBuffer     // Pointer to the streambuffer handle
             );
 
-    // CAN2_RX.streamHandle = xStreamBufferCreateStatic((64*sizeof(can_msg_t)), sizeof(can_msg_t), (uint8_t*)CAN2_RX.buffer, &CAN2_RX.RXStaticBuffer);
-
 
-    // Enable the CAN1 RX interrupt on message recieved
+    // Enable the CAN1 RX interrupt on message received
     SET_BIT(CAN1->IER, CAN_IER_FMPIE0);
     NVIC_SetPriority(CAN1_RX0_IRQn, NVIC_Priority_MIN-10U);
     NVIC_EnableIRQ(CAN1_RX0_IRQn);
 
-    // Enable the CAN2 RX interrupt on message recieved
-    //SET_BIT(CAN2->IER, CAN_IER_FMPI0);
-    //NVIC_SetPriority(CAN2_RX0_IRQn, NVIC_Priority_MIN-10U);
-    //NVIC_EnableIRQ(CAN2_RX0_IRQn); 
-
 
     // LOOP waiting for CAN messages
     for(;;){
-        // LOOP pulling and sorting messages from the streambuffer
-        while(xStreamBufferBytesAvailable(CAN1_RX.streamHandle) > 0){
+        // LOOP pulling and sorting messages from the stream buffer
+        while(xStreamBufferBytesAvailable(CAN1_StreamBuffer.streamHandle) > 0){
             // Temp message to store the data comming out of the buffer
             can_msg_t msg;
             // Recieve a message from the buffer
             xStreamBufferReceive(
-                    CAN1_RX.streamHandle, // Stream Buffer Handle
+                    CAN1_StreamBuffer.streamHandle, // Stream Buffer Handle
                     (void*)&msg,          // Pointer to RX Buffer (void*)
                     sizeof(can_msg_t),    // Size of RX Buffer (Shoudl be size of CAN message)
                     10                    // Ticks to Wait
             );
             // Load the message from the streambuffer into the hash table
-            CAN1_DATA[can1_hash(msg.id)] = msg;
+            CAN1_DATA[CAN1_HASH(msg.id)] = msg;
             // Timestamp the message
-            CAN1_DATA[can1_hash(msg.id)].timestamp = xTaskGetTickCount();
+            CAN1_DATA[CAN1_HASH(msg.id)].timestamp = xTaskGetTickCount();
             // printf("%d\n", CAN1_DATA[can1_hash(msg.id)].id);
         }
-        // while(xStreamBufferBytesAvailable(CAN2_RX.streamHandle) > 0){
-        //     // Temp message to store the data comming out of the buffer
-        //     can_msg_t msg;
-        //     // Recieve a message from the buffer
-        //     xStreamBufferReceive(CAN2_RX.streamHandle, (void*)&msg, sizeof(can_msg_t), 10);
-        //     CAN2_DATA[can2_hash(msg.id)] = msg;
-        //     CAN2_DATA[can2_hash(msg.id)].timestamp = xTaskGetTickCount();
-        // }
         vTaskDelay(100);
     }
 }
 
-can_msg_t can_fetch(CAN_TypeDef *CAN, uint32_t id){
-    if(CAN == CAN1) return CAN1_DATA[can1_hash(id)];
+can_msg_t can_read(CAN_TypeDef *CAN, uint32_t id){
+    if(CAN == CAN1) return CAN1_DATA[CAN1_HASH(id)];
     // Return a zero message on event of misused CAN bus
     return (can_msg_t){
         .timestamp = 0,
@@ -195,14 +201,7 @@ can_msg_t can_fetch(CAN_TypeDef *CAN, uint32_t id){
     };
 }
 
-bool can_check_timestamp(CAN_TypeDef *CAN, uint32_t id, uint32_t maxTicks){
-    if(CAN == CAN1) return CAN1_DATA[can1_hash(id)].timestamp + maxTicks < xTaskGetTickCount();
-    // Return false on misused can bus
-    return false;
-}
-
-
-uint8_t can_send_msg(CAN_TypeDef *CAN, can_msg_t *tx_msg, TickType_t timeout){
+enum SYS_ERROR can_write(CAN_TypeDef *CAN, can_msg_t *tx_msg, TickType_t timeout){
     // Check if a transmit mailbox is available
     if(xSemaphoreTake(CAN1_TX_Semaphore[CAN_TX_SEMAPHORE_COUNT], timeout) != pdTRUE){
         // Return failed to aquire TX mailbox
@@ -212,9 +211,9 @@ uint8_t can_send_msg(CAN_TypeDef *CAN, can_msg_t *tx_msg, TickType_t timeout){
     for(uint8_t i = 1; i < 4; i++){
         if(xSemaphoreTake(CAN1_TX_Semaphore[i], 10) == pdTRUE){
             // Utilize the hal to load the selected mailbox
-            hal_can_send(CAN, tx_msg, (i-1)/*mailbox=0..2*/);
+            hal_can_send(CAN, tx_msg, (uint8_t)(i-1)/*mailbox=0..2*/);
             // Wait for mailbox to be empty
-            while(!hal_can_send_ready(CAN, (i-1)));
+            while(!hal_can_send_ready(CAN, (uint8_t)(i-1)));
             // Give back the semaphores
             xSemaphoreGive(CAN1_TX_Semaphore[i]);
             xSemaphoreGive(CAN1_TX_Semaphore[CAN_TX_SEMAPHORE_COUNT]);
@@ -224,4 +223,3 @@ uint8_t can_send_msg(CAN_TypeDef *CAN, can_msg_t *tx_msg, TickType_t timeout){
     }
     return HAL_CAN_FATAL_ERR;
 }
-
diff --git a/Q24ECU/core/src/drivers/uart.c b/Q24ECU/core/src/drivers/uart.c
new file mode 100644
index 00000000..4bc1b4bb
--- /dev/null
+++ b/Q24ECU/core/src/drivers/uart.c
@@ -0,0 +1,140 @@
+/**
+ * @file uart.c
+ * @author Jacob Chisholm (https://Jchisholm204.github.io)
+ * @brief Source file for UART Driver
+ * @version 0.1
+ * @date 2024-03-10
+ * 
+ * @copyright Copyright (c) 2024
+ * 
+ */
+
+#include "drivers/uart.h"
+#include "FreeRTOS.h"
+#include "task.h"
+#include "nvicConfig.h"
+#include "semphr.h"
+#include <string.h>
+
+
+// UART OS Handlers
+UART_Handle_t Serial2 = {
+    .pUART  = USART2,
+    .writeHandler = NULL,
+    .readHandler  = NULL,
+    .pBuffer = NULL
+};
+
+
+UART_Handle_t Serial4 = {
+    .pUART  = UART4,
+    .writeHandler = NULL,
+    .readHandler  = NULL,
+    .pBuffer = NULL
+};
+
+
+void uart_init(unsigned long baud){
+    /* BEGIN Init USART 2 */
+    hal_uart_init(Serial2.pUART, baud, PIN_USART2_TX, PIN_USART2_RX);
+    Serial2.writeHandler = xSemaphoreCreateBinaryStatic(&Serial2.writeSemaphore);
+    if(Serial2.writeHandler != NULL)
+        xSemaphoreGive(Serial2.writeHandler);
+    Serial2.readHandler = xSemaphoreCreateBinaryStatic(&Serial2.readSemaphore);
+    if(Serial4.readHandler != NULL)
+        xSemaphoreGive(Serial2.readHandler);
+    /* END Init USART 2 */
+    
+    /* BEGIN Init USART 4 */
+    hal_uart_init(Serial4.pUART, baud, PIN_UART4_TX, PIN_UART4_RX);
+    Serial4.writeHandler = xSemaphoreCreateBinaryStatic(&Serial4.writeSemaphore);
+    if(Serial4.writeHandler != NULL)
+        xSemaphoreGive(Serial4.writeHandler);
+    Serial4.readHandler = xSemaphoreCreateBinaryStatic(&Serial4.readSemaphore);
+    if(Serial4.readHandler != NULL)
+        xSemaphoreGive(Serial4.readHandler);
+    /* END Init USART 4 */
+    
+}
+
+
+enum SYS_ERROR uart_open_buffer(UART_Handle_t *pHandle, UART_StreamBuffer_t *stream, TickType_t timeout) {
+    if(pHandle == NULL) return SYS_INVALID_ARG;
+    if(stream == NULL) return SYS_INVALID_ARG;
+    if(stream->handle == NULL) return SYS_INVALID_ARG;
+    if(xSemaphoreTake(pHandle->readHandler, timeout)){
+        hal_uart_enable_rxne(pHandle->pUART, true);
+        pHandle->pBuffer = &stream->handle;
+        return SYS_OK;
+    }
+    return UART_UNINIT_ERR;
+}
+
+enum SYS_ERROR uart_close_buffer(UART_Handle_t *pHandle){
+    if(pHandle == NULL) return SYS_INVALID_ARG;
+    if(xSemaphoreGetMutexHolder(pHandle->readHandler) == xTaskGetCurrentTaskHandle()){
+        hal_uart_enable_rxne(pHandle->pUART, false);
+        pHandle->pBuffer = NULL;
+    }
+    return UART_UNINIT_ERR;
+}
+
+enum SYS_ERROR uart_write_byte(UART_Handle_t *pHandle, char byte, TickType_t timeout){
+    if(pHandle == NULL) return UART_UNINIT_ERR;
+    if(pHandle->writeHandler == NULL) return UART_UNINIT_ERR;
+    if(xSemaphoreTake(pHandle->writeHandler, timeout) == pdTRUE){
+        hal_uart_write_byte(pHandle->pUART, byte);
+        xSemaphoreGive(pHandle->writeHandler);
+        return SYS_OK;
+    }
+    return UART_ACC_ERR;
+}
+
+enum SYS_ERROR uart_write(UART_Handle_t *pHandle, char* buf, size_t len, TickType_t timeout){
+    if(pHandle == NULL) return UART_UNINIT_ERR;
+    if(pHandle->writeHandler == NULL) return UART_UNINIT_ERR;
+    if(xSemaphoreTake(pHandle->writeHandler, timeout) == pdTRUE){
+        hal_uart_write_buf(pHandle->pUART, buf, len);
+        xSemaphoreGive(pHandle->writeHandler);
+        return SYS_OK;
+    }
+    return UART_ACC_ERR;
+}
+
+
+enum SYS_ERROR uart_write_str(UART_Handle_t *pHandle, char* str,  TickType_t timeout){
+    if(pHandle == NULL) return UART_UNINIT_ERR;
+    if(pHandle->writeHandler == NULL) return UART_UNINIT_ERR;
+    if(xSemaphoreTake(pHandle->writeHandler, timeout) == pdTRUE){
+        hal_uart_write_buf(pHandle->pUART, str, strlen(str));
+        xSemaphoreGive(pHandle->writeHandler);
+        return SYS_OK;
+    }
+    return UART_ACC_ERR;
+}
+
+/* BEGIN UART Interrupt Request Handlers */
+
+void USART2_IRQHandler(void){
+    BaseType_t xHigherPriorityTaskWoken = pdFALSE;
+    uint8_t rxData = hal_uart_read_byte(Serial2.pUART);
+    // Null check stream buffer (disable IRQ and return on failure)
+    if(Serial2.pBuffer == NULL){
+        hal_uart_enable_rxne(Serial2.pUART, false);
+        return;
+    }
+    xStreamBufferSendFromISR(*Serial2.pBuffer, &rxData, sizeof(uint8_t), &xHigherPriorityTaskWoken);
+
+}
+
+void UART4_IRQHandler(void){
+    BaseType_t xHigherPriorityTaskWoken = pdFALSE;
+    uint8_t rxData = hal_uart_read_byte(Serial4.pUART);
+    // Null check stream buffer (disable IRQ and return on failure)
+    if(Serial4.pBuffer == NULL){
+        hal_uart_enable_rxne(Serial4.pUART, false);
+        return;
+    }
+    xStreamBufferSendFromISR(*Serial4.pBuffer, &rxData, sizeof(uint8_t), &xHigherPriorityTaskWoken);
+
+}
diff --git a/Q24ECU/core/src/interfaces/README.md b/Q24ECU/core/src/interfaces/README.md
deleted file mode 100644
index 522089a9..00000000
--- a/Q24ECU/core/src/interfaces/README.md
+++ /dev/null
@@ -1,12 +0,0 @@
-# Interfaces
-
-This subdirectory contains the C files for all interfaces. Interfaces can be a union between a task and an interrupt. They can also be thread safe hardware drivers.
-An example of an interface would be: An interrupt that pulls data out of the UART data register and adds it to a queue, combined with a task that blocks on the queue, computing the data when the queue is non-empty.
-
-Interface header files may contain definitions to interact with hardware through thread safe methods. These thread safe methods should always be initialized in their corresponding source files (.c).
-
-## Structure
-All files within the interfaces folder should be prefixed with `interface_`. These files should be paired with a matching `interfaces_{name}.h` placed in the `include/interfaces` directory. For large interfaces, such as CAN, a single `interface_can.h` file should be used, regardless of how many `.c` files are present. For smaller interfaces, such as UART, there should only be a single `interfaces_uart.c` file that contains all of the UART interfaces.
-
-## Initialization
-All tasks, interrupts, and hardware interfaces (ports) should be initialized within a function prefixed with `os_` and postfixed with `_setup`. This function should be called in main.
\ No newline at end of file
diff --git a/Q24ECU/core/src/interfaces/interface_adc.c b/Q24ECU/core/src/interfaces/interface_adc.c
deleted file mode 100644
index 1c03222e..00000000
--- a/Q24ECU/core/src/interfaces/interface_adc.c
+++ /dev/null
@@ -1,115 +0,0 @@
-/**
- * @file interface_adc.c
- * @author Jacob Chisholm (https://Jchisholm204.github.io)
- * @brief FreeRTOS ADC Interface for QFSAE VCU
- * @version 0.1
- * @date 2024-02-24
- * 
- * @copyright Copyright (c) 2024
- * 
- * ADC readings are automatically looped and loaded into RAM via the DMA.
- * The number of readings done by the ADC (setup through the sequence length and config channel)
- * must be equal to the number of transfers done by the DMA. This is done through `ADC_CHANNEL_MAX`
- * in the `ADC_CHANNELS` enum.
- * 
- */
-
-#include "interfaces/interface_adc.h"
-#include "hal/hal_gpio.h"
-#include "interfaces/interface_sysError.h"
-#include "nvicConfig.h"
-#include "interrupts.h"
-#include "hal/hal_dma.h"
-
-uint16_t ADC_READINGS[ADC_CHANNEL_MAX];
-
-/**
- * @brief OS ADC Setup Handler
- * 
- * This function should be called to init the ADC bus and handler interfaces
- * This function should not be called more than once
- * ALL CAN bus initialization should happen within this function
- * 
- * Called in main
- * 
- */
-void os_adc_setup(void) {
-
-    // Zero out ADC Readings
-    for (int i = 0; i < ADC_CHANNEL_MAX; i++)
-    {
-        ADC_READINGS[i] = 0;
-    }
-
-    // Reduce the ADC Clock to APB2/8
-    SET_BIT(ADC123_COMMON->CCR, 3 << 16);
-
-    // Configure Analog GPIO Pins
-    gpio_set_mode(PIN_A5Vin_1, GPIO_MODE_ANALOG);
-    gpio_set_mode(PIN_A5Vin_2, GPIO_MODE_ANALOG);
-
-    // Setup DMA for ADC1
-    hal_dma_init(DMA2, DMA2_Stream0, DMA_CHANNEL_0, DMA_PRIORITY_LOW, DMA_MEM_SIZE_16, &(ADC1->DR), ADC_READINGS, ADC_CHANNEL_MAX);
-    // Setup ADC1
-    hal_adc_init(ADC1, ADC_RESOLUTION_12_BIT);
-    // Setup Channel Sequence
-    hal_adc_configChannel(ADC1, PIN_A5Vin_1_ADCCH, ADC_CYCLES_480, ADC_SQ1);
-    hal_adc_configChannel(ADC1, PIN_A5Vin_2_ADCCH, ADC_CYCLES_480, ADC_SQ2);
-    // Set Sequence Length
-    hal_adc_set_sequence_len(ADC1, ADC_CHANNEL_MAX);
-
-    spin(9999999UL); // Wait for ADC to stabilize
-
-    // Enable the DMA Stream
-    hal_dma_start(DMA2_Stream0);
-    // Enable ADC and Start Conversions
-    hal_adc_startConversions(ADC1);
-}
-
-void vTask_ADCMonitor(void *param) {
-    (void)(param); // Cast unused variable to void
-    vTaskDelay(10);
-    printf("Starting Conversions\n");
-    // Enable the DMA Stream
-    hal_dma_start(DMA2_Stream0);
-    // Enable ADC and Start Conversions
-    hal_adc_startConversions(ADC1);
-
-    for(;;) {
-        /**
-         * ADC Overflow Checking and Recovery
-         * This task may be shutdown or changed to replace ADC data with zeros in the future.
-         * Further Error checking should be done on the values produced by the ADC to determine
-         * possible hardware or software faults.
-         */
-        if(READ_BIT(ADC1->SR, ADC_SR_OVR)) sysError_report(ADC_OVERFLOW);
-        if(sysError_check(ADC_OVERFLOW) == true){
-            // Attempt to clear the overflow (Stop all conversion)
-            hal_adc_stopConversions(ADC1);
-            hal_dma_stop(DMA2_Stream0);
-            // Wait for system to catch up
-            vTaskDelay(5);
-            // Clear the Overflow Bit
-            CLEAR_BIT(ADC1->SR, ADC_SR_OVR);
-            // Wait
-            vTaskDelay(5);
-            // Restart the conversions
-            hal_dma_start(DMA2_Stream0);
-            hal_adc_startConversions(ADC1);
-            // Clear the error if the ADC has started a new conversion
-            if(READ_BIT(ADC1->SR, ADC_SR_STRT)){
-                sysError_clear(ADC_OVERFLOW);
-            }
-        }
-        // This task is not designed for fault detection, only software error recovery.
-        // Use long delay with high task priority to ensure that the task runs consistently
-        vTaskDelay(500);
-    }
-}
-
-
-extern double adc_fetch(enum ADC_CHANNEL channel){
-    //if(channel > ADC_CHANNEL_MAX) return 0.0;
-    // TODO: add scaling for 12v/5v
-    return ADC_READINGS[channel]*3.3/4096.0;
-}
diff --git a/Q24ECU/core/src/interfaces/interface_sysError.c b/Q24ECU/core/src/interfaces/interface_sysError.c
deleted file mode 100644
index c4ca41b1..00000000
--- a/Q24ECU/core/src/interfaces/interface_sysError.c
+++ /dev/null
@@ -1,158 +0,0 @@
-/**
- * @file interface_sysError.c
- * @author Jacob Chisholm (https://Jchisholm204.github.io)
- * @brief 
- * @version 0.1
- * @date 2024-02-17
- * 
- * @copyright Copyright (c) 2024
- * 
- * Interface for system error reporting to 7 segment displays.
- * Console logging can be enable with the `PRINTF_REPORT_ERR` macro.
- * 
- * *Note: Seven Segment Displays are only available on VCU Hardware >= V2*
- * 
- */
-
-#include "interfaces/interface_sysError.h"
-#include "taskhandlers.h"
-#include "inttypes.h"
-#include "main.h"
-
-// Internal
-
-// Macros for bit manipulation
-#define byteIndex(error) (uint8_t)(error / 8)
-#define bitIndex(error)  (uint8_t)(error % 8)
-
-// Currently active system errors
-uint8_t system_errors[byteIndex(SYS_ERROR_MAX) + 1];
-// Active errors as of last check
-uint8_t system_errors_prev[byteIndex(SYS_ERROR_MAX) + 1];
-// Errors reported (Only resets on power cycle)
-uint8_t system_errors_persist[byteIndex(SYS_ERROR_MAX) + 1];
-
-/**
- * @brief Check if an error has already been logged
- * 
- * @param error <enum SYS_ERROR> The error to check
- * @return true if the error exists within the system_errors_prev array
- * @return false if the error is new
- */
-bool sysError_checkExisting(enum SYS_ERROR error){
-    return (system_errors_prev[byteIndex(error)] >> bitIndex(error)) & 0x01U;
-}
-
-/**
- * @brief Display the error on the 7 segment displays
- * 
- * @param error <enum SYS_ERROR> The error to display
- */
-void displayError(enum SYS_ERROR error){
-    (void)error;
-    // TODO: Display the error on the 7 segment displays
-
-    // Used for debugging
-    // printf("Displaying Error: %d\n", error);
-}
-
-// Public
-
-void os_sysError_setup(void){
-    // Clear all errors
-    for(int i = 0; i < byteIndex(SYS_ERROR_MAX); i++){
-        system_errors[i] = 0;
-        system_errors_prev[i] = 0;
-        system_errors_persist[i] = 0;
-    }
-    displayError(SYS_OK);
-}
-
-void sysError_report(enum SYS_ERROR error){
-    // Check if no error is present
-    if(error == SYS_OK) return;
-    // Check if the error is system critical
-    if(error == TASK_SHUTDOWN_CRITICAL) {
-        taskENTER_CRITICAL();
-        displayError(TASK_SHUTDOWN_CRITICAL);
-        // Invoke the system critical shutdown function
-        system_critical_shutdown();
-        // Should never reach here
-    }
-    system_errors[byteIndex(error)] |= (uint8_t)(1U << bitIndex(error));
-    vTaskResume(xTaskHandles[eTask_SysError]);
-}
-
-void sysError_reportTsk(enum SYS_ERROR error, TaskHandle_t *task){
-    sysError_report(error);
-    #ifdef PRINTF_REPORT_ERR
-    printf("Suspending due to error %d\n", error);
-    #endif
-    vTaskResume(xTaskHandles[eTask_SysError]);
-    vTaskSuspend(*task);
-}
-
-void sysError_clear(enum SYS_ERROR error){
-    vTaskResume(xTaskHandles[eTask_SysError]);
-    system_errors[byteIndex(error)] &= (uint8_t)(~(1U << bitIndex(error)));
-    
-}
-
-bool sysError_check(enum SYS_ERROR error){
-    return (system_errors[byteIndex(error)] >> bitIndex(error)) & 0x01U;
-}
-
-bool sysError_checkRuntime(enum SYS_ERROR error){
-    return (system_errors_persist[byteIndex(error)] >> bitIndex(error)) & 0x01U;
-}
-
-// Task for handling system errors
-void vTask_SysError(void *param){
-    (void)param;
-    // Delay for 20 ms to allow the system to start
-    vTaskDelay(20);
-
-    for(;;){
-        // Initialize variable to check if there are any errors present
-        bool error = false;
-        // Check all the errors
-        for (int i = 0; i <= SYS_ERROR_MAX; i++) {
-            // Check if a new error has spawned
-            if(sysError_check((enum SYS_ERROR)i) == true && sysError_checkExisting((enum SYS_ERROR)i) == false){
-                #ifdef PRINTF_REPORT_ERR
-                printf("%d\n", i);
-                #endif
-                system_errors_prev[byteIndex(i)] |= (uint8_t)(1U << bitIndex(i));
-                system_errors_persist[byteIndex(i)] |= (uint8_t)(1U << bitIndex(i));
-                error = true;
-            }
-            // Check if a preexisting error has been cleared
-            else if(sysError_check((enum SYS_ERROR)i) == false && sysError_checkExisting((enum SYS_ERROR)i) == true){
-                #ifdef PRINTF_REPORT_ERR
-                printf("Cleared %d\n", i);
-                #endif
-                system_errors_prev[byteIndex(i)] &= (uint8_t)(~(1U << bitIndex(i)));
-            }
-
-            // If an error is present, set the error flag
-            if(sysError_check((enum SYS_ERROR)i) == true){
-                error = true;
-                // Display the error on the 7 segment displays
-                displayError((enum SYS_ERROR)i);
-                // Persist for 1 second, then loop through the rest of the errors
-                vTaskDelay(1000);
-            }
-        }
-
-        if(error == false){
-            // Clear the 7 segment displays
-            displayError(SYS_OK);
-            #ifdef PRINTF_REPORT_ERR
-            // Report SYS_OK to the terminal
-            printf("SYS_OK\n");
-            #endif
-            // Suspend the task indefinitely (Will resume when an error is reported via `sysError_report` or `sysError_reportTsk`)
-            vTaskSuspend(NULL);
-        }
-    }
-}
\ No newline at end of file
diff --git a/Q24ECU/core/src/interfaces/interface_uart.c b/Q24ECU/core/src/interfaces/interface_uart.c
deleted file mode 100644
index 792bfc70..00000000
--- a/Q24ECU/core/src/interfaces/interface_uart.c
+++ /dev/null
@@ -1,79 +0,0 @@
-/**
- * @file interface_uart.c
- * @author Jacob Chisholm (Jchisholm204.github.io)
- * @brief 
- * @version 0.1
- * @date 2023-11-18
- * 
- * @copyright Copyright (c) 2023
- * 
- */
-
-#include "interfaces/interface_uart.h"
-#include "FreeRTOS.h"
-#include "task.h"
-#include "taskHandlers.h"
-#include "nvicConfig.h"
-
-// UART OS Handlers
-uart_t port_uart2;
-uart_t port_uart4;
-
-// UART Baud Rate (Bits per Second)
-#define UART_BAUD 250000
-
-
-/**
- * @brief OS UART Setup handler
- * 
- * This function should be called to initalized all onboard UART interfaces.
- * This function should not be called more than once.
- * All UART initialization should happen within this function
- * 
- * Called in main
- * 
- */
-void os_uart_setup(void){
-
-    // Enable the UART 2 port and setup its IQR handler
-    uart_send_init(&port_uart2, USART2, UART_BAUD, PIN_USART2_TX, PIN_USART2_RX);
-    xStreamBufferSetTriggerLevel(port_uart2.rxbuffer, 5); // set the trigger level of the stream buffer. Port Specific.
-    hal_uart_enable_rxne(port_uart2.port, true);
-    NVIC_SetPriority(USART2_IRQn, (NVIC_Priority_MIN-10));
-    NVIC_EnableIRQ(USART2_IRQn);
-
-    // Enable the UART 4 port (NO IRQ handler for receiving data)
-    uart_send_init(&port_uart4, UART4, UART_BAUD, PIN_UART4_TX, PIN_UART4_RX);
-}
-
-void USART2_IRQHandler(void){
-    // Initialize variable to trigger context switch to false (no context switch)
-    BaseType_t xHigherPriorityTaskWoken = pdFALSE;
-
-    // Receive the data loaded into the UART DR (data register)
-    uint8_t receivedData = 0;
-    // use the uart 2 CMSIS define (reduce risk of hanging interrupt)
-    receivedData = hal_uart_read_byte(USART2);
-
-    // Add the received data into the rx buffer stream
-    xStreamBufferSendFromISR(port_uart2.rxbuffer, &receivedData, sizeof(receivedData), &xHigherPriorityTaskWoken);
-
-    // Check and trigger a context switch if needed
-    portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
-}
-
-void vTask_USART2_Handler(void *param){
-    (void)param;
-    for(;;){
-        uint8_t buf[64];
-        size_t bytes = xStreamBufferReceive(port_uart2.rxbuffer, (void*) &buf, 64, portMAX_DELAY);
-        printf("%s (%d)\n", buf, bytes);
-        // reset the stream buffer
-        for (int i = 0; i < 64; i++)
-        {
-            buf[i] = 0;
-        }
-        
-    }
-}
-
diff --git a/Q24ECU/core/src/interrupts/README.md b/Q24ECU/core/src/interrupts/README.md
index 138e046c..e01a3566 100644
--- a/Q24ECU/core/src/interrupts/README.md
+++ b/Q24ECU/core/src/interrupts/README.md
@@ -1,9 +1,7 @@
 # Interrupts
 
-This folder is for interrupt only functions. For interrupts that pass data into tasks, see `src/interrupts/README.md`
+Interrupts used on their own or for passing notifiers into tasks.
 
-## Structure
-Interrupts should be separated into their own `.c` files. All interrupt share a single `interrupts.h` file that allows the linker to find them.
+All Interrupts should be initialized similarly to tasks and should be shut down when not in use. Using interrupts on their own should be avoided when possible.
 
-## Initialization
-All interrupts that do not interact with tasks are initialized directly in the main function.
\ No newline at end of file
+Ensure that a global definition of the interrupt is placed in the `main.h` file so that it can be linked with the startup script.
\ No newline at end of file
diff --git a/Q24ECU/core/src/main.c b/Q24ECU/core/src/main.c
index 30c095c4..f83f30e2 100644
--- a/Q24ECU/core/src/main.c
+++ b/Q24ECU/core/src/main.c
@@ -12,77 +12,46 @@
 #include "hal/hal_gpio.h"
 #include "hal/hal_tim_basic.h"
 #include "hal/hal_uart.h"
-#include "interfaces/interface_uart.h"
-#include "interfaces/interface_can.h"
-#include "interfaces/interface_sysError.h"
-#include "interfaces/interface_adc.h"
+#include "drivers/can.h"
+#include "drivers/adc.h"
+#include "drivers/uart.h"
 #include "stm32f446xx.h"
 
-int main(void){
-
-    // Initialize Timer Interrupts
+TaskHandle_t xControllerTaskHandle;
 
-    // Enable Timer 6 (Basic Timer) 1Hz (APB2/45000, count to 2000)
-    // TIM_basic_Init(TIM6, 45000U, 2000U);
-    // NVIC_SetPriority(TIM6_DAC_IRQn, NVIC_Priority_MIN); // Enable Timer IRQ (lowest priority)
-    // NVIC_EnableIRQ(TIM6_DAC_IRQn);
-    
-    // Initialize OS Interfaces
+StaticTask_t xControllerTaskBuffer;
+#define ControllerTaskStackSize 0x200
+StackType_t xControllerTaskStack[ControllerTaskStackSize];
 
-    os_uart_setup();
+int main(void){
+    // Initialize the Serial Interface (Baud = 9600 default)
+    uart_init(250000);
 
     // clear terminal
     printf("\033[2J");
     printf("\n");
-    printf("USART Initialized..\n");
-
-    os_sysError_setup();
-    printf("System Error Handler Initialized..\n");
-
-    os_can_setup();
-    printf("CAN Bus Initialized..\n");
-
-    os_adc_setup();
-    printf("ADC Initialized..\n");
 
-    spin(9999999UL);
-
-    printf("System Starting Tasks...\n\n");
-
-    spin(9999999UL);  
-
-    // Initialize all the tasks
-    os_task_init();
-
-    // Print out Task Information
-    printf("Task:\t   Identifier\t    Priority\t\tState\n");
-    for (int i = 0; i < eTask_TaskCount; i++)
-    {
-        printf("%d: %16s\t\t%lu\t\t", i, pcTaskGetName(xTaskHandles[i]), uxTaskPriorityGet(xTaskHandles[i]));
-        switch(eTaskGetState(xTaskHandles[i])) {
-            case eRunning:
-                printf("RUNNING\n");
-                break;
-            case eReady:
-                printf("READY\n");
-                break;
-            case eBlocked:
-                printf("BLOCKED\n");
-                break;
-            case eSuspended:    
-                printf("SUSPENDED\n");
-                break;
-            case eDeleted:   
-                printf("DELETED\n");
-                break;
-            case eInvalid:   
-                printf("INVALID\n");
-                break;
-            default:
-                printf("UNKNOWN\n");
-                break;
-        }
-    }
+    // Initialize System Interfaces
+    printf("USART Initialized..\n");
+    printf("Initializing ADC..\n");
+    adc_init();
+    printf("ADC Initialized\n");
+    printf("Initializing CAN Bus..\n");
+    can_init();
+    printf("CAN Bus Initialized\n");
+    printf("Starting Core Task..\n");
+
+    xControllerTaskHandle = xTaskCreateStatic(
+        vTask_Controller,
+        "Controller",
+        ControllerTaskStackSize,
+        NULL,
+        tskIDLE_PRIORITY+10,
+        xControllerTaskStack,
+        &xControllerTaskBuffer
+    );
+
+    printf("Controller Task Initialized\n");
 
     printf("\n");
     printf("Total Heap Memory: %d B\n", configTOTAL_HEAP_SIZE);
diff --git a/Q24ECU/core/src/sys_state/controller.c b/Q24ECU/core/src/sys_state/controller.c
new file mode 100644
index 00000000..6e00065d
--- /dev/null
+++ b/Q24ECU/core/src/sys_state/controller.c
@@ -0,0 +1,69 @@
+/**
+ * @file core.c
+ * @author Jacob Chisholm (https://Jchisholm204.github.io)
+ * @brief Main System Runtime Header
+ * @version 0.1
+ * @date 2024-03-04
+ * 
+ * This file serves as the header file for the main system runtime
+ * The Core program is responsible for system startup, excluding the bootloader, and hardware interfaces that could prevent the system from starting up properly.
+ * 
+ * The Core's main function is to initialize and maintain the acceleration throttle control (ATC)
+ * tasks including the ADT task itself and the Software Differential Control (SDC) task.
+ * 
+ * @copyright Copyright (c) 2024
+ * 
+ */
+
+#include <stdio.h>
+#include "sys_state.h"
+#include "sys_tasks.h"
+
+#define taskResume(tsk) vTaskResume(xTaskHandles[tsk])
+#define taskSuspend(tsk) vTaskSuspend(xTaskHandles[tsk])
+
+/* BEGIN System State Control Code */
+
+enum SYS_STATE core_state = SYS_STATE_INIT;
+
+enum SYS_STATE core_get_state(void){
+    return core_state;
+}
+
+/* END System State Control Code */
+
+
+void vTask_Controller(void *param){
+    (void) param;
+    uart_write(&Serial2, "Started Core Task\n", 18, 10);
+    // Initialize all tasks to a disabled state
+    tasker_init();
+    task_printDebug(&Serial2);
+    uart_write_str(&Serial2, "Core Task Started\n", 10);
+    // Initialize the system into the idle state
+    core_state = SYS_STATE_IDLE;
+
+    for(;;){
+        // Loop through system states, checking for errors after each state releases control
+        // More information in header file
+        switch (core_state) {
+        case SYS_STATE_IDLE:
+            vState_Idle(&core_state);
+            break;
+        // Both test cases and active require running the same start sequences
+        case SYS_STATE_TEST:
+        case SYS_STATE_TS_ACTIVE:
+            vState_Start(&core_state);
+            break;
+        case SYS_STATE_RTD:
+            vState_RTD(&core_state);
+            break;
+        default:
+            vState_Error(&core_state);
+            break;
+        }
+        // Check to see if the system is in an error state
+        vState_Error(&core_state);
+    } // END Core Task for loop
+} // END Core Task
+
diff --git a/Q24ECU/core/src/sys_state/state_error.c b/Q24ECU/core/src/sys_state/state_error.c
new file mode 100644
index 00000000..a142e499
--- /dev/null
+++ b/Q24ECU/core/src/sys_state/state_error.c
@@ -0,0 +1,60 @@
+/**
+ * @file state_error.c
+ * @author Jacob Chisholm (https://Jchisholm204.github.io)
+ * @brief System Run State Code
+ * @version 0.1
+ * @date 2024-03-11
+ * 
+ * @copyright Copyright (c) 2024
+ * 
+ */
+
+#include "sys_state.h"
+#include "stdbool.h"
+#include "errors.h"
+#include "drivers/uart.h"
+
+uint32_t errors[3];
+#define get_e(e) (bool)((errors[e/3] >> (e % 32)) & 0x01)
+#define set_e(e) ((errors[e/3] |= (uint32_t)(0x1UL << (e % 32))))
+#define clear_e(e) ((errors[e/3] &= ~(uint32_t)(0x1UL << (e % 32))))
+
+void vState_Error(enum SYS_STATE *state){
+    if(*state == SYS_STATE_INIT){
+        errors[0] = 0UL;
+        errors[1] = 0UL;
+        errors[2] = 0UL;
+    }
+    if(*state != SYS_STATE_ERR) return;
+    uart_write_str(&Serial2, "System has entered into an error state...\n", 10U);
+    for(enum SYS_ERROR i = 0; i < SYS_ERROR_MAX; i++){
+        if(get_e(i) == true){
+            char buf[40];
+            snprintf(buf, 40, "Error: %d\n", i);
+            uart_write_str(&Serial2, buf, 10);
+            clear_e(i);
+        }
+    }
+    uart_write_str(&Serial2, "Returning to idle state\n", 10U);
+    // this state is currently unused.
+    *state = SYS_STATE_IDLE;
+    return;
+}
+
+/* BEGIN Error Handling Code */
+
+
+void controller_throw(enum SYS_ERROR e){
+    if(e == SYS_OK) return;
+    set_e(e);
+}
+
+bool controller_check_error(enum SYS_ERROR e){
+    return get_e(e);
+}
+
+bool controller_check_fault(void){
+    return !(errors[0] == 0 && errors[1] == 0 && errors[2] == 0);
+}
+
+/* END Error Handling Code */
diff --git a/Q24ECU/core/src/sys_state/state_idle.c b/Q24ECU/core/src/sys_state/state_idle.c
new file mode 100644
index 00000000..14f105f0
--- /dev/null
+++ b/Q24ECU/core/src/sys_state/state_idle.c
@@ -0,0 +1,42 @@
+/**
+ * @file state_idle.c
+ * @author Jacob Chisholm (https://Jchisholm204.github.io)
+ * @brief System Idle State Code
+ * @version 0.1
+ * @date 2024-03-11
+ * 
+ * This function runs when the system is idleing.
+ * It must monitor for the TS Enable switch. Once the switch has
+ * been pressed, this task must switch the state to TS_Active
+ *
+ * @copyright Copyright (c) 2024
+ * 
+ */
+
+#include "sys_state.h"
+#include "sys_tasks.h"
+
+void vState_Idle(enum SYS_STATE *state){
+    // Check to see if the system state matches the function state
+    if(*state != SYS_STATE_IDLE) return;
+    // Enable the error task
+    // if(eTaskGetState(xTaskHandles[eTask_ErrorScreen]) == eSuspended)
+    //     vTaskResume(xTaskHandles[eTask_ErrorScreen]);
+    // // Enable the SBSPD task
+    // if(eTaskGetState(xTaskHandles[eTask_SBSPD]) == eSuspended)
+    //     vTaskResume(xTaskHandles[eTask_SBSPD]);
+    task_Resume(eTask_TEST1);
+    task_Resume(eTask_CANSend);
+    printf("System is now in the Idle State\n");
+    task_printDebug(&Serial2);
+    // disable inverters
+    // disable shutdown circuit
+    for(;;){
+        if(controller_check_fault() == true){
+            *state = SYS_STATE_ERR;
+            return;
+        }
+        vTaskDelay(200);
+    }
+}
+
diff --git a/Q24ECU/core/src/sys_state/state_rtd.c b/Q24ECU/core/src/sys_state/state_rtd.c
new file mode 100644
index 00000000..f2e3a325
--- /dev/null
+++ b/Q24ECU/core/src/sys_state/state_rtd.c
@@ -0,0 +1,22 @@
+/**
+ * @file state_rtd.c
+ * @author Jacob Chisholm (https://Jchisholm204.github.io)
+ * @brief Ready to Drive System State
+ * @version 0.1
+ * @date 2024-03-12
+ * 
+ * @copyright Copyright (c) 2024
+ * 
+ */
+
+#include "sys_state.h"
+
+
+void vState_RTD(enum SYS_STATE *state){
+    if(*state != SYS_STATE_RTD) return;
+
+    // this state is currently unused.
+    *state = SYS_STATE_IDLE;
+    return;
+}
+
diff --git a/Q24ECU/core/src/sys_state/state_start.c b/Q24ECU/core/src/sys_state/state_start.c
new file mode 100644
index 00000000..6c2cb5ef
--- /dev/null
+++ b/Q24ECU/core/src/sys_state/state_start.c
@@ -0,0 +1,41 @@
+/**
+ * @file state_start.c
+ * @author Jacob Chisholm (https://Jchisholm204.github.io)
+ * @brief High Voltage Power up State (Rules TS Active)
+ * @version 0.1
+ * @date 2024-03-12
+ * 
+ * @copyright Copyright (c) 2024
+ * 
+ */
+
+#include "sys_state.h"
+
+
+void vState_Start(enum SYS_STATE *state){
+
+    // this state is currently unused.
+    *state = SYS_STATE_IDLE;
+    return;
+
+
+    if(*state == SYS_STATE_TS_ACTIVE){
+        // initialize the inverters
+        // initialize the tractive system
+    }
+    if(*state == SYS_STATE_TEST){
+        // only enable the inverters
+    }
+    else {
+        *state = SYS_STATE_ERR;
+        return;
+    }
+
+    // Wait for precharge
+    
+    for(;;){
+        // check for throttle 0 and brake pressed, start button pressed
+    }
+
+}
+
diff --git a/Q24ECU/core/src/sys_tasks/README.md b/Q24ECU/core/src/sys_tasks/README.md
new file mode 100644
index 00000000..1c17666b
--- /dev/null
+++ b/Q24ECU/core/src/sys_tasks/README.md
@@ -0,0 +1,5 @@
+# SYSTEM TASKS
+
+1. System tasks should be added into the enum located in `sys_tasks.h`. Global definitions of the system tasks should also be placed within this file.
+2. Task creation should happen within `sys_tasks/controller.c`. They are initialized to a suspended state.
+3. Resume the tasks within the correct system state.
\ No newline at end of file
diff --git a/Q24ECU/core/src/sys_tasks/SBSPD.c b/Q24ECU/core/src/sys_tasks/SBSPD.c
new file mode 100644
index 00000000..c602f067
--- /dev/null
+++ b/Q24ECU/core/src/sys_tasks/SBSPD.c
@@ -0,0 +1,20 @@
+/**
+ * @file SBSPD.c
+ * @author your name (you@domain.com)
+ * @brief 
+ * @version 0.1
+ * @date 2024-03-11
+ * 
+ * @copyright Copyright (c) 2024
+ * 
+ */
+
+#include "sys_tasks.h"
+
+
+void vTask_SBSPD(void *param){
+    (void)param;
+    for(;;){
+        vTaskDelay(200);
+    }
+}
\ No newline at end of file
diff --git a/Q24ECU/core/src/sys_tasks/canRunner.c b/Q24ECU/core/src/sys_tasks/canRunner.c
new file mode 100644
index 00000000..a6572ee7
--- /dev/null
+++ b/Q24ECU/core/src/sys_tasks/canRunner.c
@@ -0,0 +1,55 @@
+
+/**
+ * @file canRunner.c
+ * @author Jacob Chisholm (https://jchisholm204.github.io/)
+ * @brief CAN Runner - Sample Interaction with CAN Bus
+ * @version 0.1
+ * @date 2024-1-10
+ * 
+ * @copyright Copyright (c) 2023
+ * 
+ */
+#include "main.h"
+#include "task.h"
+#include "interfaces/cm200dx.h"
+#include "sys_tasks.h"
+#include "drivers/can.h"
+#include <stdio.h>
+#include <string.h>
+
+void vTask_CAN_send(void *param){
+    (void)param;
+    vTaskDelay(1);
+    uart_write_str(&Serial2, "Running CAN TX Task from canRunner.c\n", 1000U);
+    can_msg_t tx_msg;
+    tx_msg.id = 33;
+    tx_msg.len = 1;
+    tx_msg.data[0] = 0;
+
+    for(;;){
+        // char buf[40];
+        // snprintf(buf, 40, "Sending: %d\n", tx_msg.data[0]);
+        // uart_send_buf_blocking(&port_uart4, buf, strlen(buf), 1000);
+        can_write(CAN1, &tx_msg, portMAX_DELAY);
+        cm200dx_setPower(RIGHT, 200);
+        tx_msg.data[0]++;
+        if(tx_msg.data[0] > 200) tx_msg.data[0] = 0;
+        vTaskDelay(200);
+    }
+}
+
+// void vTask_CAN_receive(void *param){
+//     (void)param;
+//     vTaskDelay(1);
+//     printf("Initializing RX\n");
+//     uart_send_buf_blocking(&port_uart4, "Running CAN RX Task from canRunner.c\n", 38U, 1000U);    
+//     can_msg_t rx_msg;
+//     for(;;){
+//         rx_msg = can_fetch(CAN1, 200);
+//         char buf[40];
+//         snprintf(buf, 40, "%ld: %d\n", rx_msg.id, rx_msg.data[0]);
+//         // uart_send_buf_blocking(&port_uart4, buf, strlen(buf), 1000);
+//         vTaskDelay(100);
+//     }
+// }
+
diff --git a/Q24ECU/core/src/sys_tasks/controller.c b/Q24ECU/core/src/sys_tasks/controller.c
new file mode 100644
index 00000000..3edbe296
--- /dev/null
+++ b/Q24ECU/core/src/sys_tasks/controller.c
@@ -0,0 +1,51 @@
+/**
+ * @file controller.c
+ * @author Jacob Chisholm (https://Jchisholm204.github.io)
+ * @brief System Task Controller
+ * @version 0.1
+ * @date 2024-03-14
+ * 
+ * This file is responsible for housing management functions for the tasks
+ * 
+ * @copyright Copyright (c) 2024
+ * 
+ */
+#include "sys_tasks.h"
+
+TaskHandle_t xTaskHandles[eTask_TaskCount];
+
+StaticTask_t xTaskBuffers[eTask_TaskCount];
+
+StackType_t xTaskStacks[eTask_TaskCount][configMINIMAL_STACK_SIZE];
+
+void task_init(enum Tasks eTask, TaskFunction_t pvTask, const char* pcName){
+    // Create the task and assign the taskHandle
+    xTaskHandles[eTask] = xTaskCreateStatic(
+        pvTask,
+        pcName,
+        configMINIMAL_STACK_SIZE,
+        NULL,
+        tskIDLE_PRIORITY,
+        xTaskStacks[eTask],
+        &xTaskBuffers[eTask]
+    );
+    // Suspend the task after creation
+    vTaskSuspend(xTaskHandles[eTask]);
+}
+
+
+void tasker_init(void){
+    // Create Software BSPD Task
+    task_init(eTask_SBSPD, vTask_SBSPD, "SBSPD");
+
+    // Create Software BSPD Task
+    task_init(eTask_ErrorScreen, vTask_ErrorScreen, "ErrDisp");
+
+    // Create Test 1 Task
+    task_init(eTask_TEST1, vTask_Test1, "Test1");
+    
+    // Create Test 1 Task
+    task_init(eTask_CANSend, vTask_CAN_send, "CANTX");
+
+}
+
diff --git a/Q24ECU/core/src/sys_tasks/errorScreen.c b/Q24ECU/core/src/sys_tasks/errorScreen.c
new file mode 100644
index 00000000..ac2e3db9
--- /dev/null
+++ b/Q24ECU/core/src/sys_tasks/errorScreen.c
@@ -0,0 +1,29 @@
+/**
+ * @file errorScreen.c
+ * @author your name (you@domain.com)
+ * @brief 
+ * @version 0.1
+ * @date 2024-03-11
+ * 
+ * @copyright Copyright (c) 2024
+ * 
+ */
+
+#include "sys_tasks.h"
+#include "task.h"
+#include "sys_state.h"
+
+void vTask_ErrorScreen(void *param){
+    (void)param;
+    for(;;){
+        // for(int i = SYS_OK; i <= SYS_ERROR_MAX; i++){
+        //     if(core_check_error(i)){
+        //         // Display Error on 7 segment
+        //         vTaskDelay(1000);
+        //     }
+        // }
+        vTaskDelay(100);
+    }
+}
+
+
diff --git a/Q24ECU/core/src/sys_tasks/testTasks.c b/Q24ECU/core/src/sys_tasks/testTasks.c
new file mode 100644
index 00000000..8851c030
--- /dev/null
+++ b/Q24ECU/core/src/sys_tasks/testTasks.c
@@ -0,0 +1,36 @@
+/**
+ * @file testTasks.c
+ * @author Jacob Chisholm (https://jchisholm204.github.io/)
+ * @brief Test Task file - Collection of test tasks
+ * @version 0.1
+ * @date 2023-11-22
+ * 
+ * @copyright Copyright (c) 2023
+ * 
+ */
+
+#include "main.h"
+#include "task.h"
+#include "drivers/adc.h"
+#include "drivers/uart.h"
+#include <stdio.h>
+#include "string.h"
+
+uint16_t adcread = 99;
+uint16_t adcread2 = 99;
+bool adc1 = false;
+
+long unsigned int counter = 0;
+
+void vTask_Test1(void *param){
+    (void)(param); // Cast unused variable to void
+
+    for (;;)
+    {
+        char buf[40];
+        snprintf(buf, 40, "%0.4f %0.4f\n", adc_read(ADC_CHANNEL_5V1), adc_read(ADC_CHANNEL_5V2));
+        uart_write(&Serial4, buf, strlen(buf), 10U);
+        vTaskDelay(100);
+    }
+}
+
diff --git a/Q24ECU/core/src/system/README.md b/Q24ECU/core/src/system/README.md
new file mode 100644
index 00000000..dd8a0848
--- /dev/null
+++ b/Q24ECU/core/src/system/README.md
@@ -0,0 +1,6 @@
+# SYSTEM
+System tasks that execute outside of the scheduler.
+
+Helper functions for FreeRTOS.
+
+System calls and MCU Startup Code.
\ No newline at end of file
diff --git a/Q24ECU/core/src/freertos.c b/Q24ECU/core/src/system/freertos.c
similarity index 100%
rename from Q24ECU/core/src/freertos.c
rename to Q24ECU/core/src/system/freertos.c
diff --git a/Q24ECU/core/src/shutdown.c b/Q24ECU/core/src/system/shutdown.c
similarity index 85%
rename from Q24ECU/core/src/shutdown.c
rename to Q24ECU/core/src/system/shutdown.c
index 060562c3..09bb2b2d 100644
--- a/Q24ECU/core/src/shutdown.c
+++ b/Q24ECU/core/src/system/shutdown.c
@@ -16,13 +16,6 @@
  */
 #include "main.h"
 #include <stdio.h>
-#include "hal/hal_gpio.h"
-#include "hal/hal_tim_basic.h"
-#include "hal/hal_uart.h"
-#include "hal/hal_can.h"
-#include "interfaces/interface_uart.h"
-#include "interfaces/interface_can.h"
-#include "interfaces/interface_sysError.h"
 #include "stm32f4xx.h"
 
 /**
diff --git a/Q24ECU/core/src/startup.c b/Q24ECU/core/src/system/startup.c
similarity index 100%
rename from Q24ECU/core/src/startup.c
rename to Q24ECU/core/src/system/startup.c
diff --git a/Q24ECU/core/src/syscalls.c b/Q24ECU/core/src/system/syscalls.c
similarity index 87%
rename from Q24ECU/core/src/syscalls.c
rename to Q24ECU/core/src/system/syscalls.c
index b87abd89..8877790d 100644
--- a/Q24ECU/core/src/syscalls.c
+++ b/Q24ECU/core/src/system/syscalls.c
@@ -3,7 +3,7 @@
 
 #include <sys/stat.h>
 #include <stm32f4xx.h>
-#include "interfaces/interface_uart.h"
+#include "drivers/uart.h"
 #include "FreeRTOS.h"
 #include "task.h"
 #include "main.h"
@@ -96,19 +96,19 @@ int _write(int fd, char *ptr, int len) {
     // Get the name of the task calling printf - Only run if scheduler has been started
     if(xTaskGetSchedulerState() != taskSCHEDULER_NOT_STARTED) callerID = pcTaskGetName(NULL);
     #endif
-    if(port_uart2.port == NULL)          return -1;
-    if(port_uart2.semaphore == NULL)     return -1;
+    if(Serial2.pUART == NULL)          return -1;
+    if(Serial2.writeHandler == NULL)     return -1;
     // Take over the debug usart
-    if(xSemaphoreTake(port_uart2.semaphore, (TickType_t) 10) == pdTRUE){
+    if(xSemaphoreTake(Serial2.writeHandler, (TickType_t) 10) == pdTRUE){
       #ifdef DEBUG_PRINTF_TASK_NAME
       // Write caller ID, followed by ": ", then the argument given to printf
       if(callerID != NULL){
-        hal_uart_write_buf(port_uart2.port, callerID, strlen(callerID));
-        hal_uart_write_buf(port_uart2.port, ": ", 3);
+        hal_uart_write_buf(Serial2.pUART, callerID, strlen(callerID));
+        hal_uart_write_buf(Serial2.pUART, ": ", 3);
       }
       #endif
-      hal_uart_write_buf(port_uart2.port, ptr, (size_t) len);
-      xSemaphoreGive(port_uart2.semaphore);
+      hal_uart_write_buf(Serial2.pUART, ptr, (size_t) len);
+      xSemaphoreGive(Serial2.writeHandler);
     }
     // hal_uart_write_buf(UART_DEBUG, ptr, (size_t) len);
   } //hal_uart_write_buf(UART_DEBUG, ptr, (size_t) len);
diff --git a/Q24ECU/core/src/taskHandlers.c b/Q24ECU/core/src/taskHandlers.c
deleted file mode 100644
index a73eb4a1..00000000
--- a/Q24ECU/core/src/taskHandlers.c
+++ /dev/null
@@ -1,119 +0,0 @@
-/**
- * @file taskHandlers.c
- * @author Jacob Chisholm (Jchisholm204.github.io)
- * @brief Initialize All Tasks
- * @version 0.1
- * @date 2023-11-28
- * 
- * @copyright Copyright (c) 2023
- * 
- * vTaskDelete() is used to delete a task. The handle to the task to be deleted is passed in as the parameter.
- * This can be used as a method to prevent a task from running at all.
- * 
- * Task information is displayed out to UART2 on startup (printed from main.c)
- * 
- */
-
-#include "taskHandlers.h"
-#include <stdio.h>
-
-
-// Init global handles for tasks
-
-TaskHandle_t xTaskHandles[eTask_TaskCount];
-
-StaticTask_t xTaskBuffers[eTask_TaskCount];
-
-StackType_t xTaskStacks[eTask_TaskCount][configMINIMAL_STACK_SIZE];
-
-void os_task_init(void){
-
-    // Create Task to empty CAN RX Buffer (Part of interface_can.c)
-    xTaskHandles[eTask_CAN_rxBufferHandler] = xTaskCreateStatic(
-        vTask_CAN_RXBufferHandler,
-        "CAN IRQ RX",
-        configMINIMAL_STACK_SIZE,
-        NULL,
-        tskIDLE_PRIORITY+2,
-        xTaskStacks[eTask_CAN_rxBufferHandler],
-        &xTaskBuffers[eTask_CAN_rxBufferHandler]
-    );
-
-    // Create Sample Blink Task
-    xTaskHandles[eTask_Test1] = xTaskCreateStatic(
-        vTask_Test1,
-        "TEST1",
-        configMINIMAL_STACK_SIZE,
-        NULL,
-        tskIDLE_PRIORITY,
-        xTaskStacks[eTask_Test1],
-        &xTaskBuffers[eTask_Test1]
-    );
-    // vTaskDelete(xTaskHandles[eTask_Test1]);
-
-    // Create Sample Print Task
-    xTaskHandles[eTask_BlinkLED] = xTaskCreateStatic(
-        vTask_BlinkLED,
-        "BLINK",
-        configMINIMAL_STACK_SIZE,
-        NULL,
-        tskIDLE_PRIORITY,
-        xTaskStacks[eTask_BlinkLED],
-        &xTaskBuffers[eTask_BlinkLED]
-    );
-    vTaskDelete(xTaskHandles[eTask_BlinkLED]);
-
-    // Create Sample CAN TX task
-    xTaskHandles[eTask_CAN_send] = xTaskCreateStatic(
-        vTask_CAN_send,
-        "CANtx",
-        configMINIMAL_STACK_SIZE,
-        NULL,
-        tskIDLE_PRIORITY,
-        xTaskStacks[eTask_CAN_send],
-        &xTaskBuffers[eTask_CAN_send]
-    );
-
-    // Create Sample CAN RX Task
-    xTaskHandles[eTask_CAN_receive] = xTaskCreateStatic(
-        vTask_CAN_receive,
-        "CANrx",
-        configMINIMAL_STACK_SIZE,
-        NULL,
-        tskIDLE_PRIORITY,
-        xTaskStacks[eTask_CAN_receive],
-        &xTaskBuffers[eTask_CAN_receive]
-    );
-
-    xTaskHandles[eTask_USART2_Handler] = xTaskCreateStatic(
-        vTask_USART2_Handler,
-        "USART2 IRQ RX",
-        configMINIMAL_STACK_SIZE,
-        NULL,
-        tskIDLE_PRIORITY,
-        xTaskStacks[eTask_USART2_Handler],
-        &xTaskBuffers[eTask_USART2_Handler]
-    );
-
-    xTaskHandles[eTask_SysError] = xTaskCreateStatic(
-        vTask_SysError,
-        "SysError",
-        configMINIMAL_STACK_SIZE,
-        NULL,
-        tskIDLE_PRIORITY,
-        xTaskStacks[eTask_SysError],
-        &xTaskBuffers[eTask_SysError]
-    );
-    // vTaskDelete(xTaskHandles[eTask_SysError]);
-
-    xTaskHandles[eTask_ADCMonitor] = xTaskCreateStatic(
-        vTask_ADCMonitor,
-        "ADCMonitor",
-        configMINIMAL_STACK_SIZE,
-        NULL,
-        tskIDLE_PRIORITY+4,
-        xTaskStacks[eTask_ADCMonitor],
-        &xTaskBuffers[eTask_ADCMonitor]
-    );
-}
-
diff --git a/Q24ECU/core/src/tasks/README.md b/Q24ECU/core/src/tasks/README.md
deleted file mode 100644
index b5157f05..00000000
--- a/Q24ECU/core/src/tasks/README.md
+++ /dev/null
@@ -1,9 +0,0 @@
-# Tasks
-
-Tasks are purely software tasks that may interact with interfaces. An example of this would be a throttle control task that retrieves data from the CAN bus and ADC interfaces, runs a computation, and then adds data to the que of messages waiting to get sent out over CAN bus.
-
-## Structure
-All tasks should be contained within their own files. Collections of tasks may be in the same file when they relate. All tasks should be externally defined within `taskHandlers.h`. All tasks should be prefixed with `tsk_`.
-
-## Initialization
-All tasks should be initialized within the `taskHandlers.c` file.  The task handles should also be initialized within this file. External definitions to the task handlers should be put in the `taskHandlers.h` file.
diff --git a/Q24ECU/core/src/tasks/canRunner.c b/Q24ECU/core/src/tasks/canRunner.c
deleted file mode 100644
index 49db411d..00000000
--- a/Q24ECU/core/src/tasks/canRunner.c
+++ /dev/null
@@ -1,55 +0,0 @@
-
-/**
- * @file canRunner.c
- * @author Jacob Chisholm (https://jchisholm204.github.io/)
- * @brief CAN Runner - Sample Interaction with CAN Bus
- * @version 0.1
- * @date 2024-1-10
- * 
- * @copyright Copyright (c) 2023
- * 
- */
-#include "main.h"
-#include "taskHandlers.h"
-#include "task.h"
-#include "interfaces/interface_can.h"
-#include "interfaces/interface_uart.h"
-#include <stdio.h>
-#include <string.h>
-
-void vTask_CAN_send(void *param){
-    (void)param;
-    vTaskDelay(1);
-    printf("Initializing TX\n");
-    uart_send_buf_blocking(&port_uart4, "Running CAN TX Task from canRunner.c\n", 38U, 1000U);
-    can_msg_t tx_msg;
-    tx_msg.id = 33;
-    tx_msg.len = 1;
-    tx_msg.data[0] = 0;
-
-    for(;;){
-        // char buf[40];
-        // snprintf(buf, 40, "Sending: %d\n", tx_msg.data[0]);
-        // uart_send_buf_blocking(&port_uart4, buf, strlen(buf), 1000);
-        (void)can_send_msg(CAN1, &tx_msg, portMAX_DELAY);
-        tx_msg.data[0]++;
-        if(tx_msg.data[0] > 200) tx_msg.data[0] = 0;
-        vTaskDelay(200);
-    }
-}
-
-void vTask_CAN_receive(void *param){
-    (void)param;
-    vTaskDelay(1);
-    printf("Initializing RX\n");
-    uart_send_buf_blocking(&port_uart4, "Running CAN RX Task from canRunner.c\n", 38U, 1000U);    
-    can_msg_t rx_msg;
-    for(;;){
-        rx_msg = can_fetch(CAN1, 200);
-        char buf[40];
-        snprintf(buf, 40, "%ld: %d\n", rx_msg.id, rx_msg.data[0]);
-        // uart_send_buf_blocking(&port_uart4, buf, strlen(buf), 1000);
-        vTaskDelay(100);
-    }
-}
-
diff --git a/Q24ECU/core/src/tasks/testTasks.c b/Q24ECU/core/src/tasks/testTasks.c
deleted file mode 100644
index 3c527aa8..00000000
--- a/Q24ECU/core/src/tasks/testTasks.c
+++ /dev/null
@@ -1,108 +0,0 @@
-/**
- * @file testTasks.c
- * @author Jacob Chisholm (https://jchisholm204.github.io/)
- * @brief Test Task file - Collection of test tasks
- * @version 0.1
- * @date 2023-11-22
- * 
- * @copyright Copyright (c) 2023
- * 
- */
-
-#include "taskHandlers.h"
-#include "main.h"
-#include "task.h"
-#include "interfaces/interface_uart.h"
-#include "interfaces/interface_can.h"
-#include "interfaces/interface_sysError.h"
-#include "hal/hal_adc.h"
-#include "interrupts.h"
-#include "hal/legacy_hal_adc.h"
-#include <stdio.h>
-#include "string.h"
-#include "interfaces/interface_adc.h"
-
-uint16_t adcread = 99;
-uint16_t adcread2 = 99;
-bool adc1 = false;
-
-long unsigned int counter = 0;
-
-void vTask_Test1(void *param){
-    (void)(param); // Cast unused variable to void
-    // printf("Running Task 1\n");
-    // can_msg_t incoming;
-    // printf("setup");
-    // for(;;){
-    //     incoming = can_fetch(CAN1, 200);
-    //     printf(">RPM:%d\n", incoming.data[0]);
-    //     incoming.len = 1;
-    //     incoming.id = 99;
-    //     can_send_msg(CAN1, &incoming, portMAX_DELAY);
-    //     vTaskDelay(200);
-    // }
-    // hal_adc_init(ADC1, ADC_RESOLUTION_12_BIT);
-
-    // SET_BIT(ADC123_COMMON->CCR, 3 << 16);
-    // gpio_set_mode(PIN('A', 4), GPIO_MODE_ANALOG);
-    // gpio_set_mode(PIN('A', 5), GPIO_MODE_ANALOG);
-
-    // hal_adc_configChannel(ADC1, 4, ADC_CYCLES_3, ADC_SQ1);
-    // hal_adc_set_sequence_len(ADC1, 1);
-    // hal_adc_enable(ADC1);
-    // hal_adc_startConversions(ADC1);
-    // hal_adc_init(ADC1, ADC_RESOLUTION_12_BIT);
-    // SET_BIT(ADC1->CR1, ADC_CR1_EOCIE);
-    // NVIC_EnableIRQ(ADC_IRQn);
-    // NVIC_SetPriority(ADC_IRQn, NVIC_Priority_MIN);
-    // hal_adc_configChannel(ADC1, 4, ADC_CYCLES_112, ADC_SQ1);
-    // hal_adc_configChannel(ADC1, 5, ADC_CYCLES_112, ADC_SQ2);
-    // // legacy_hal_adc_configChannel(ADC1, 4, 1);
-    // hal_adc_set_sequence_len(ADC1, 2);
-    // hal_adc_startConversions(ADC1);
-    // CLEAR_BIT(ADC1->SQR1, ADC_SQR1_L);
-    // SET_BIT(ADC1->SQR1, (uint32_t)(1UL << ADC_SQR1_L_Pos));:w
-    
-
-
-    // legacy_hal_adc_init(ADC2);
-    
-    for(;;) {
-        // if(READ_BIT(ADC1->CR2, ADC_CR2_SWSTART) == 0){
-            
-        // }
-        // if(READ_BIT(ADC1->SR, ADC_SR_EOC)){
-            char buf[40];
-            // if(READ_BIT(ADC1->SR, ADC_SR_OVR)){
-            //     printf("Overflow\n");
-            // }
-            // hal_adc_startConversions(ADC1);
-            // SET_BIT(ADC1->CR2, ADC_CR2_SWSTART);
-            // while(!(ADC1->SR & ADC_SR_EOC));
-            snprintf(buf, 40, "%0.4f %0.4f\n", adc_fetch(ADC_CHANNEL_5V1), adc_fetch(ADC_CHANNEL_5V2));
-            uart_send_buf_blocking(&port_uart4, buf, strlen(buf), 10U);
-        //     CLEAR_BIT(ADC1->SR, ADC_SR_EOC);
-            // SET_BIT(ADC1->CR2, ADC_CR2_SWSTART);
-        // }
-        vTaskDelay(100);
-    }
-}
-
-
-void vTask_BlinkLED(void *param){
-    (void)param;
-    // TickType_t lastWakeTime = xTaskGetTickCount();
-    // for(;;){
-    //     // This task is currently not used so perma suspend it
-    //     lastWakeTime = xTaskGetTickCount();
-    //     counter++;
-    //     can_msg_t msg;
-    //     msg.id = 98;
-    //     msg.len = 1;
-    //     msg.data[0] = 22;
-    //     can_send_msg(CAN1, &msg, portMAX_DELAY);
-    //     vTaskDelayUntil(&lastWakeTime, 100);
-    // }
-    vTaskSuspend(NULL);
-}
-
diff --git a/Q24ECU/docs/structure.md b/Q24ECU/docs/structure.md
new file mode 100644
index 00000000..59dcd076
--- /dev/null
+++ b/Q24ECU/docs/structure.md
@@ -0,0 +1,56 @@
+.
+└── VCU_CORE/
+    ├── include/
+    │   ├── drivers/
+    │   │   ├── adc.h
+    │   │   ├── can.h
+    │   │   ├── spi.h
+    │   │   └── uart.h
+    │   ├── hal/
+    │   │   ├── hal_adc.h
+    │   │   ├── hal_can.h
+    │   │   ├── hal_clock.h
+    │   │   ├── hal_dma.h
+    │   │   ├── hal_flash.h
+    │   │   └── hal_spi.h
+    │   ├── interfaces/
+    │   │   ├── cm200dx.h
+    │   │   └── orionbms2.h
+    │   ├── errors.h
+    │   ├── main.h
+    │   ├── sys_state.h
+    │   └── sys_tasks.h
+    ├── src/
+    │   ├── drivers/
+    │   │   ├── adc.c
+    │   │   ├── can.c
+    │   │   ├── uart.c
+    │   │   └── spi.c
+    │   ├── sys_state/
+    │   │   ├── controller.c
+    │   │   ├── state_error.c
+    │   │   ├── state_idle.c
+    │   │   ├── state_rtd.c
+    │   │   └── state_start.c
+    │   ├── sys_tasks/
+    │   │   ├── controller.c
+    │   │   ├── SBSPD.c
+    │   │   ├── logging.c
+    │   │   ├── cooling_ctrl.c
+    │   │   ├── can1_echo.c
+    │   │   ├── can2_echo.c
+    │   │   ├── HVD_watchdog.c
+    │   │   └── GLV_watchdog.c
+    │   ├── system/
+    │   │   ├── freertos.c
+    │   │   ├── shutdown.c
+    │   │   ├── startup.c
+    │   │   └── syscalls.c
+    │   └── main.c
+    └── docs/
+        ├── FreeRTOS.md
+        ├── interfaces.md
+        ├── tasks.md
+        ├── canbus.md
+        ├── README.md
+        └── compiling.md
\ No newline at end of file