diff --git a/Core/Inc/cerberus_conf.h b/Core/Inc/cerberus_conf.h
index 76a9dcf8..48ffef62 100644
--- a/Core/Inc/cerberus_conf.h
+++ b/Core/Inc/cerberus_conf.h
@@ -33,10 +33,6 @@
 #define GPIO_4_GPIO_Port                    GPIOB
 #define WATCHDOG_Pin                        GPIO_PIN_15
 #define WATCHDOG_GPIO_Port                  GPIOB
-#define LED_1_Pin                           GPIO_PIN_8
-#define LED_1_GPIO_Port                     GPIOC
-#define LED_2_Pin                           GPIO_PIN_9
-#define LED_2_GPIO_Port                     GPIOC
 
 #define CANID_TEMP_SENSOR 0xBEEF
 #define CANID_PEDAL_SENSOR 0xDEAD
diff --git a/Core/Inc/monitor.h b/Core/Inc/monitor.h
index 406dd9fe..4e5f97f1 100644
--- a/Core/Inc/monitor.h
+++ b/Core/Inc/monitor.h
@@ -14,13 +14,6 @@ void vWatchdogMonitor(void *pv_params);
 extern osThreadId_t watchdog_monitor_handle;
 extern const osThreadAttr_t watchdog_monitor_attributes;
 
-typedef struct
-{
-    ADC_HandleTypeDef *accel_adc1;
-    ADC_HandleTypeDef *accel_adc2;
-    ADC_HandleTypeDef *brake_adc;
-} pedal_params_t;
-
 /* Parameters for the pedal monitoring task */
 #define MAX_ADC_VAL_12B 4096
 #define PEDAL_DIFF_THRESH   10
diff --git a/Core/Inc/mpu.h b/Core/Inc/mpu.h
new file mode 100644
index 00000000..b6582406
--- /dev/null
+++ b/Core/Inc/mpu.h
@@ -0,0 +1,48 @@
+#ifndef MPU_H
+#define MPU_H
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "stm32f405xx.h"
+#include "sht30.h"
+#include "lsm6dso.h"
+#include "cmsis_os.h"
+
+typedef struct {
+    I2C_HandleTypeDef *hi2c;
+    ADC_HandleTypeDef *accel_adc1;
+    ADC_HandleTypeDef *accel_adc2;
+    ADC_HandleTypeDef *brake_adc;
+    GPIO_TypeDef* led_gpio;
+    GPIO_TypeDef* watchdog_gpio;
+    sht30_t *temp_sensor;
+    lsm6dso_t *imu;
+    osMutexId_t *adc_mutex;
+    osMutexId_t *i2c_mutex;
+    /* Not including LED Mutexes because not necessary */
+} mpu_t;
+
+mpu_t *init_mpu(I2C_HandleTypeDef *hi2c, ADC_HandleTypeDef *accel_adc1, 
+                ADC_HandleTypeDef *accel_adc2, ADC_HandleTypeDef *brake_adc,
+                GPIO_TypeDef* led_gpio, GPIO_TypeDef* watchdog_gpio);
+
+int8_t write_rled(mpu_t *mpu, bool status);
+
+int8_t toggle_rled(mpu_t *mpu);
+
+int8_t write_yled(mpu_t *mpu, bool status);
+
+int8_t toggle_yled(mpu_t *mpu);
+
+int8_t pet_watchdog(mpu_t *mpu);
+
+/* Note: this should be called from within a thread since it yields to scheduler */
+int8_t read_adc(mpu_t *mpu, uint16_t raw[3]);
+
+int8_t read_temp_sensor(mpu_t *mpu, uint16_t *temp, uint16_t *humidity);
+
+int8_t read_accel(mpu_t *mpu, uint16_t accel[3]);
+
+int8_t read_gyro(mpu_t *mpu, uint16_t gyro[3]);
+
+#endif /* MPU */
\ No newline at end of file
diff --git a/Core/Src/can_handler.c b/Core/Src/can_handler.c
index 4ffe6d95..c7fc6875 100644
--- a/Core/Src/can_handler.c
+++ b/Core/Src/can_handler.c
@@ -25,9 +25,9 @@ static osMessageQueueId_t can_inbound_queue;
 /* Relevant Info for Initializing CAN 1 */
 static can_t can1;
 
-static const uint16_t id_list[NUM_INBOUND_CAN_IDS] = {
+static uint16_t id_list[NUM_INBOUND_CAN_IDS] = {
 	//CANID_X,
-	NULL
+	0
 };
 
 /* Relevant Info for Cerberus CAN LUT */
@@ -137,7 +137,6 @@ const osThreadAttr_t can_dispatch_attributes = {
 
 void vCanDispatch(void* pv_params)
 {
-	const uint16_t can_dispatch_delay = 1; //ms
 	fault_data_t fault_data = {
 		.id = CAN_DISPATCH_FAULT,
 		.severity = DEFCON1
@@ -174,4 +173,5 @@ int8_t queue_can_msg(can_msg_t msg)
 		return -1;
 
 	osMessageQueuePut(can_outbound_queue, &msg, 0U, 0U);
+	return 0;
 }
diff --git a/Core/Src/fault.c b/Core/Src/fault.c
index 8b3fa017..5da30f0a 100644
--- a/Core/Src/fault.c
+++ b/Core/Src/fault.c
@@ -19,6 +19,7 @@ int queue_fault(fault_data_t *fault_data)
 		return -1;
 
     osMessageQueuePut(fault_handle_queue, fault_data, 0U, 0U);
+    return 0;
 }
 
 void vFaultHandler(void* pv_params) 
diff --git a/Core/Src/main.c b/Core/Src/main.c
index 74a14ac7..fd626db1 100644
--- a/Core/Src/main.c
+++ b/Core/Src/main.c
@@ -34,6 +34,7 @@
 #include "state_machine.h"
 #include "torque.h"
 #include "pdu.h"
+#include "mpu.h"
 /* USER CODE END Includes */
 
 /* Private typedef -----------------------------------------------------------*/
@@ -161,13 +162,17 @@ int main(void)
   MX_ADC3_Init();
   /* USER CODE BEGIN 2 */
 
-  HAL_GPIO_TogglePin(GPIOC, LED_2_Pin); // Toggle on LED2
+  /* Create Interfaces to Represent Relevant Hardware */
+  mpu_t *mpu = init_mpu(&hi2c1, &hadc1, &hadc2, &hadc3, GPIOC, GPIOB);
+  pdu_t *pdu = init_pdu(&hi2c2);
+
+  toggle_yled(mpu); // Toggle on LED2
   HAL_Delay(500);
-  HAL_GPIO_TogglePin(GPIOC, LED_2_Pin); // Toggle on LED2
+  toggle_yled(mpu); // Toggle on LED2
   HAL_Delay(500);
-  HAL_GPIO_TogglePin(GPIOC, LED_2_Pin); // Toggle on LED2
+  toggle_yled(mpu); // Toggle on LED2
   HAL_Delay(500);
-  HAL_GPIO_TogglePin(GPIOC, LED_2_Pin); // Toggle on LED2
+  toggle_yled(mpu); // Toggle on LED2
 
   /* USER CODE END 2 */
 
@@ -189,15 +194,6 @@ int main(void)
   /* USER CODE BEGIN RTOS_QUEUES */
   onboard_temp_queue = osMessageQueueNew(ONBOARD_TEMP_QUEUE_SIZE, sizeof(onboard_temp_t), NULL);
   imu_queue = osMessageQueueNew(IMU_QUEUE_SIZE, sizeof(imu_data_t), NULL);
-
-  // A bit sloppy since I don't free this, but it will be allocated for lifetime
-  pedal_params_t *pedal_params = malloc(sizeof(pedal_params_t));
-  pedal_params->accel_adc1 = &hadc1;
-  pedal_params->accel_adc2 = &hadc2;
-  pedal_params->brake_adc = &hadc3;
-
-  pdu_t *pdu = init_pdu(&hi2c1);
-
   pedal_data_queue = osMessageQueueNew(PEDAL_DATA_QUEUE_SIZE, sizeof(pedals_t), NULL);
   /* USER CODE END RTOS_QUEUES */
 
@@ -207,10 +203,10 @@ int main(void)
 
   /* USER CODE BEGIN RTOS_THREADS */
   /* Monitors */
-  temp_monitor_handle = osThreadNew(vTempMonitor, &hi2c1, &temp_monitor_attributes);
+  temp_monitor_handle = osThreadNew(vTempMonitor, mpu, &temp_monitor_attributes);
   watchdog_monitor_handle = osThreadNew(vWatchdogMonitor, GPIOB, &watchdog_monitor_attributes);
-  imu_monitor_handle = osThreadNew(vIMUMonitor, &hi2c1, &imu_monitor_attributes);
-  pedals_monitor_handle = osThreadNew(vPedalsMonitor, pedal_params, &pedals_monitor_attributes);
+  imu_monitor_handle = osThreadNew(vIMUMonitor, mpu, &imu_monitor_attributes);
+  pedals_monitor_handle = osThreadNew(vPedalsMonitor, mpu, &pedals_monitor_attributes);
   fusing_monitor_handle = osThreadNew(vFusingMonitor, pdu, &fusing_monitor_attributes);
   shutdown_monitor_handle = osThreadNew(vShutdownMonitor, pdu, &shutdown_monitor_attributes);
 
@@ -745,8 +741,8 @@ void StartDefaultTask(void *argument)
 {
   /* USER CODE BEGIN 5 */
   int i = 0;
-  uint8_t data;
-  HAL_StatusTypeDef err;
+  //uint8_t data;
+  //HAL_StatusTypeDef err;
   /* Infinite loop */
   for(;;) {
     /* Testing getting data from I2C devices */
@@ -766,9 +762,9 @@ void StartDefaultTask(void *argument)
     //HAL_StatusTypeDef err = HAL_ADC_PollForConversion(&hadc1, HAL_MAX_DELAY);
     //if (!err)
     //  serial_print("%d\r\n", HAL_ADC_GetValue(&hadc1));
-      
+
     /* Toggle LED at certain frequency */
-    HAL_GPIO_TogglePin(GPIOC, LED_1_Pin); // Toggle on LED2
+    HAL_GPIO_TogglePin(GPIOC, GPIO_PIN_8); // I am not using MPU interface because I'm lazy 
     i++;
 
     if (i % 2 == 1)
diff --git a/Core/Src/monitor.c b/Core/Src/monitor.c
index 75a9bb9a..665eb2b8 100644
--- a/Core/Src/monitor.c
+++ b/Core/Src/monitor.c
@@ -1,7 +1,7 @@
 #include <string.h>
 #include <stdbool.h>
 #include "monitor.h"
-#include "can.h"
+#include "can_handler.h"
 #include "cerberus_conf.h"
 #include "fault.h"
 #include "queues.h"
@@ -12,6 +12,7 @@
 #include "serial_monitor.h"
 #include "timer.h"
 #include "pdu.h"
+#include "mpu.h"
 
 osThreadId_t temp_monitor_handle;
 const osThreadAttr_t temp_monitor_attributes = {
@@ -23,47 +24,38 @@ const osThreadAttr_t temp_monitor_attributes = {
 void vTempMonitor(void* pv_params)
 {
 	const uint8_t num_samples				= 10;
-	const uint16_t temp_sensor_sample_delay = TEMP_SENS_SAMPLE_DELAY;
-	const uint8_t can_msg_len				= 4;   /* bytes */
 	static onboard_temp_t sensor_data;
 	fault_data_t fault_data = { .id = ONBOARD_TEMP_FAULT, .severity = DEFCON4 };
-	sht30_t temp_sensor;
-	I2C_HandleTypeDef* hi2c1;
 	can_msg_t temp_msg
-		= { .id = CANID_TEMP_SENSOR, .len = can_msg_len, .data = { 0 } };
+		= { .id = CANID_TEMP_SENSOR, .len = 4, .data = { 0 } };
 
-	hi2c1				   = (I2C_HandleTypeDef*)pv_params;
-	temp_sensor.i2c_handle = hi2c1;
-
-	//if (sht30_init(&temp_sensor)) {
-	//	fault_data.diag = "Temp Monitor Init Failed";
-	//	queue_fault(&fault_data);
-	//}
+	mpu_t *mpu = (mpu_t *)pv_params;
 
 	for (;;) {
 		/* Take measurement */
 		//serial_print("Temp Sensor Task\r\n");
-		//if (sht30_get_temp_humid(&temp_sensor)) {
-		//	fault_data.diag = "Failed to get temp";
-		//	queue_fault(&fault_data);
-		//}
+		uint16_t temp, humidity;
+		if (read_temp_sensor(mpu, &temp, &humidity)) {
+			fault_data.diag = "Failed to get temp";
+			queue_fault(&fault_data);
+		}
 
 		/* Run values through LPF of sample size  */
-		//sensor_data.temperature = (sensor_data.temperature + temp_sensor.temp) / num_samples;
-		//sensor_data.humidity	= (sensor_data.humidity + temp_sensor.humidity) / num_samples;
+		sensor_data.temperature = (sensor_data.temperature + temp) / num_samples;
+		sensor_data.humidity	= (sensor_data.humidity + humidity) / num_samples;
 
 		/* Publish to Onboard Temp Queue */
-		//osMessageQueuePut(onboard_temp_queue, &sensor_data, 0U, 0U);
+		osMessageQueuePut(onboard_temp_queue, &sensor_data, 0U, 0U);
 
 		/* Send CAN message */
-		//memcpy(temp_msg.data, &sensor_data, can_msg_len);
-		//if (queue_can_msg(temp_msg)) {
-		//	fault_data.diag = "Failed to send CAN message";
-		//	queue_fault(&fault_data);
-		//}
+		memcpy(temp_msg.data, &sensor_data, temp_msg.len);
+		if (queue_can_msg(temp_msg)) {
+			fault_data.diag = "Failed to send CAN message";
+			queue_fault(&fault_data);
+		}
 
 		/* Yield to other tasks */
-		osDelay(temp_sensor_sample_delay);
+		osDelay(TEMP_SENS_SAMPLE_DELAY);
 	}
 }
 
@@ -76,13 +68,11 @@ const osThreadAttr_t watchdog_monitor_attributes = {
 
 void vWatchdogMonitor(void* pv_params)
 {
-	GPIO_TypeDef* gpio;
-	gpio = (GPIO_TypeDef*)pv_params;
+	mpu_t *mpu = (mpu_t*)pv_params;
 
 	for (;;) {
 		/* Pets Watchdog */
-		HAL_GPIO_WritePin(gpio, GPIO_PIN_15, GPIO_PIN_SET);
-		HAL_GPIO_WritePin(gpio, GPIO_PIN_15, GPIO_PIN_RESET);
+		pet_watchdog(mpu);
 
 		/* Yield to other RTOS tasks */
 		osThreadYield();
@@ -98,53 +88,30 @@ const osThreadAttr_t pedals_monitor_attributes = {
 
 void vPedalsMonitor(void* pv_params)
 {
-	const uint8_t num_samples = 10;
+	//const uint8_t num_samples = 10;
 	enum { ACCELPIN_1, ACCELPIN_2, BRAKEPIN_1, BRAKEPIN_2 };
-	const uint16_t delay_time  = PEDALS_SAMPLE_DELAY;
-	const uint16_t adc_sample_time = 2; /* ms */
-	const uint8_t can_msg_len = 4;	/* bytes */
 
-	nertimer_t diff_timer;
-	nertimer_t sc_timer;
-	nertimer_t oc_timer;
+	//nertimer_t diff_timer;
+	//nertimer_t sc_timer;
+	//nertimer_t oc_timer;
 
 	static pedals_t sensor_data;
 	fault_data_t fault_data = { .id = ONBOARD_PEDAL_FAULT, .severity = DEFCON1 };
 
-	can_msg_t pedal_msg
-		= { .id = CANID_PEDAL_SENSOR, .len = can_msg_len, .data = { 0 } };
+	//can_msg_t pedal_msg
+	//	= { .id = CANID_PEDAL_SENSOR, .len = 4, .data = { 0 } };
 
 	/* Handle ADC Data for two input accelerator value and two input brake value*/
-	pedal_params_t* params = (pedal_params_t*)pv_params;
+	mpu_t *mpu = (mpu_t *)pv_params;
 
-	uint16_t adc_data[4];
-
-	uint32_t curr_tick = HAL_GetTick();
+	uint16_t adc_data[3];
 
 	for (;;) {
 		//serial_print("Pedals Task: %d\r\n", HAL_GetTick() - curr_tick);
-		/*
-		 * Get the value from the adc at the brake and accelerator
-		 * pin addresses and average them to the sensor data value
-		 */
-		HAL_ADC_Start(params->accel_adc1);
-		HAL_ADC_Start(params->accel_adc2);
-		HAL_ADC_Start(params->brake_adc);
-
-		/* Yield to other tasks */
-		osDelay(delay_time);
-
-		HAL_StatusTypeDef err = HAL_ADC_PollForConversion(params->accel_adc1, HAL_MAX_DELAY);
-		if (!err)
-			HAL_ADC_GetValue(params->accel_adc1);
-
-		err = HAL_ADC_PollForConversion(params->accel_adc2, HAL_MAX_DELAY);
-		if (!err)
-			HAL_ADC_GetValue(params->accel_adc2);
-		
-		err = HAL_ADC_PollForConversion(params->brake_adc, HAL_MAX_DELAY);
-		if (!err)
-			HAL_ADC_GetValue(params->brake_adc);
+		if (read_adc(mpu, adc_data)) {
+			fault_data.diag = "Failed to collect ADC Data!";
+			queue_fault(&fault_data);
+		}
 
 		//err = HAL_ADC_PollForConversion(params->brake_adc, HAL_MAX_DELAY);
 		//if (!err)
@@ -194,7 +161,8 @@ void vPedalsMonitor(void* pv_params)
 		//	fault_data.diag = "Failed to send CAN message";
 		//	queue_fault(&fault_data);
 		//}
-  }
+		osDelay(PEDALS_SAMPLE_DELAY);
+	}
 }
 
 osThreadId_t imu_monitor_handle;
@@ -207,80 +175,71 @@ const osThreadAttr_t imu_monitor_attributes = {
 void vIMUMonitor(void *pv_params)
 {
 	const uint8_t num_samples = 10;
-	const uint16_t imu_sample_delay = IMU_SAMPLE_DELAY;
-	const uint8_t accel_msg_len = 6; /* bytes */
-	const uint8_t gyro_msg_len = 6; /* bytes */
 	static imu_data_t sensor_data;
 	fault_data_t fault_data = {
 		.id = IMU_FAULT,
 		.severity = DEFCON3
 	};
-	lsm6dso_t imu;
 	can_msg_t imu_accel_msg = {
 		.id = CANID_IMU,
-		.len = accel_msg_len,
+		.len = 6,
 		.data = {0}
 	};
 	can_msg_t imu_gyro_msg = {
 		.id = CANID_IMU,
-		.len = gyro_msg_len,
+		.len = 6,
 		.data = {0}
 	};
 
-	I2C_HandleTypeDef *hi2c1 = (I2C_HandleTypeDef *)pv_params;
-	imu.i2c_handle = hi2c1;
-
-	/* Initialize IMU */
-	//if (lsm6dso_init(&imu, hi2c1)) {
-	//	fault_data.diag = "IMU Monitor Init Failed";
-	//	queue_fault(&fault_data);
-	//}
+	mpu_t *mpu = (mpu_t *)pv_params;
 
 	for(;;) {
 		//serial_print("IMU Task\r\n");
 		/* Take measurement */
-		//if (lsm6dso_read_accel(&imu)) {
-		//	fault_data.diag = "Failed to get IMU acceleration";
-		//	queue_fault(&fault_data);
-		//}
+		uint16_t accel_data[3];
+		uint16_t gyro_data[3];
+		if (read_accel(mpu,accel_data)) {
+			fault_data.diag = "Failed to get IMU acceleration";
+			queue_fault(&fault_data);
+		}
 
-		//if (lsm6dso_read_gyro(&imu)) {
-		//	fault_data.diag = "Failed to get IMU gyroscope";
-		//	queue_fault(&fault_data);
-		//}
+		if (read_gyro(mpu, gyro_data)) {
+			fault_data.diag = "Failed to get IMU gyroscope";
+			queue_fault(&fault_data);
+		}
 
 		/* Run values through LPF of sample size  */
-		//sensor_data.accel_x = (sensor_data.accel_x + imu.accel_data[0])
-		//					  / num_samples;
-		//sensor_data.accel_y = (sensor_data.accel_y + imu.accel_data[1])
-		//					  / num_samples;
-		//sensor_data.accel_z = (sensor_data.accel_z + imu.accel_data[2])
-		//					  / num_samples;
-		//sensor_data.gyro_x = (sensor_data.gyro_x + imu.gyro_data[0])
-		//					 / num_samples;
-		//sensor_data.gyro_y = (sensor_data.gyro_y + imu.gyro_data[1])
-		//					 / num_samples;
-		//sensor_data.gyro_z = (sensor_data.gyro_z + imu.gyro_data[2])
-		//					 / num_samples;
+		sensor_data.accel_x = (sensor_data.accel_x + accel_data[0])
+							   / num_samples;
+		sensor_data.accel_y = (sensor_data.accel_y + accel_data[1])
+							   / num_samples;
+		sensor_data.accel_z = (sensor_data.accel_z + accel_data[2])
+							   / num_samples;
+		sensor_data.gyro_x = (sensor_data.gyro_x + gyro_data[0])
+							  / num_samples;
+		sensor_data.gyro_y = (sensor_data.gyro_y + gyro_data[1])
+							  / num_samples;
+		sensor_data.gyro_z = (sensor_data.gyro_z + gyro_data[2])
+							  / num_samples;
 
 		/* Publish to IMU Queue */
-		//osMessageQueuePut(imu_queue, &sensor_data, 0U, 0U);
+		osMessageQueuePut(imu_queue, &sensor_data, 0U, 0U);
 
 		/* Send CAN message */
-		//memcpy(imu_accel_msg.data, &sensor_data, accel_msg_len);
-		//if (queue_can_msg(imu_accel_msg)) {
-		//	fault_data.diag = "Failed to send CAN message";
-		//	queue_fault(&fault_data);
-		//}
+		memcpy(imu_accel_msg.data, &sensor_data, imu_accel_msg.len);
+		if (queue_can_msg(imu_accel_msg)) {
+			fault_data.diag = "Failed to send CAN message";
+			queue_fault(&fault_data);
+		}
 		
-		//memcpy(imu_gyro_msg.data, &sensor_data, gyro_msg_len);
-		//if (queue_can_msg(imu_gyro_msg)) {
-		//	fault_data.diag = "Failed to send CAN message";
-		//	queue_fault(&fault_data);
-		//}
+		memcpy(imu_gyro_msg.data, &sensor_data, imu_gyro_msg.len);
+		if (queue_can_msg(imu_gyro_msg)) {
+			fault_data.diag = "Failed to send CAN message";
+			queue_fault(&fault_data);
+		}
 
 		/* Yield to other tasks */
-		osDelay(imu_sample_delay);
+		osDelay(IMU_SAMPLE_DELAY);
 	}
 }
 
@@ -332,16 +291,16 @@ const osThreadAttr_t shutdown_monitor_attributes = {
 
 void vShutdownMonitor(void *pv_params)
 {
-	fault_data_t fault_data = {
-		.id = SHUTDOWN_MONITOR_FAULT,
-		.severity = DEFCON2
-	};
-
-	can_msg_t shutdown_msg = {
-		.id = CANID_SHUTDOWN_LOOP,
-		.len = 8,
-		.data = { 0 }
-	};
+	//fault_data_t fault_data = {
+	//	.id = SHUTDOWN_MONITOR_FAULT,
+	//	.severity = DEFCON2
+	//};
+
+	//can_msg_t shutdown_msg = {
+	//	.id = CANID_SHUTDOWN_LOOP,
+	//	.len = 8,
+	//	.data = { 0 }
+	//};
 
 	pdu_t *pdu = (pdu_t *)pv_params;
 
diff --git a/Core/Src/mpu.c b/Core/Src/mpu.c
new file mode 100644
index 00000000..88ed3000
--- /dev/null
+++ b/Core/Src/mpu.c
@@ -0,0 +1,226 @@
+#include "mpu.h"
+#include <stdlib.h>
+#include <assert.h>
+#include <string.h>
+#include "stm32f405xx.h"
+
+#define YLED_PIN        GPIO_PIN_8
+#define RLED_PIN        GPIO_PIN_9
+#define WATCHDOG_PIN    GPIO_PIN_15
+#define ADC_TIMEOUT     2   /* ms */
+
+static osMutexAttr_t mpu_i2c_mutex_attr;
+static osMutexAttr_t mpu_adc_mutex_attr;
+
+mpu_t *init_mpu(I2C_HandleTypeDef *hi2c, ADC_HandleTypeDef *accel_adc1, 
+                ADC_HandleTypeDef *accel_adc2, ADC_HandleTypeDef *brake_adc,
+                GPIO_TypeDef* led_gpio, GPIO_TypeDef* watchdog_gpio)
+{
+    assert(hi2c);
+    assert(accel_adc1);
+    assert(accel_adc2);
+    assert(brake_adc);
+    assert(led_gpio);
+    assert(watchdog_gpio);
+
+    /* Create MPU struct */
+    mpu_t *mpu = malloc(sizeof(mpu_t));
+    assert(mpu);
+
+    mpu->hi2c = hi2c;
+    mpu->accel_adc1 = accel_adc1;
+    mpu->accel_adc2 = accel_adc2;
+    mpu->brake_adc = brake_adc;
+    mpu->led_gpio = led_gpio;
+    mpu->watchdog_gpio = watchdog_gpio;
+
+    /* Initialize the Onboard Temperature Sensor */
+    mpu->temp_sensor = malloc(sizeof(sht30_t));
+    assert(mpu->temp_sensor);
+    mpu->temp_sensor->i2c_handle = hi2c;
+    assert(!sht30_init(mpu->temp_sensor)); /* This is always connected */
+
+    /* Initialize the IMU */
+    mpu->imu = malloc(sizeof(lsm6dso_t));
+    assert(mpu->imu);
+    assert(!lsm6dso_init(mpu->imu, mpu->hi2c)); /* This is always connected */
+
+    /* Create Mutexes */
+    mpu->i2c_mutex = osMutexNew(&mpu_i2c_mutex_attr);
+    assert(mpu->i2c_mutex);
+
+    mpu->adc_mutex = osMutexNew(&mpu_adc_mutex_attr);
+    assert(mpu->adc_mutex);
+
+    return 0;
+}
+
+int8_t write_rled(mpu_t *mpu, bool status)
+{
+    if (!mpu)
+        return -1;
+
+    HAL_GPIO_WritePin(mpu->led_gpio, RLED_PIN, status);
+    return 0;
+}
+
+int8_t toggle_rled(mpu_t *mpu)
+{
+    if (!mpu)
+        return -1;
+
+    HAL_GPIO_TogglePin(mpu->led_gpio, RLED_PIN);
+    return 0;
+}
+
+int8_t write_yled(mpu_t *mpu, bool status)
+{
+    if (!mpu)
+        return -1;
+
+    HAL_GPIO_WritePin(mpu->led_gpio, YLED_PIN, status);
+    return 0;
+}
+
+int8_t toggle_yled(mpu_t *mpu)
+{
+    if (!mpu)
+        return -1;
+
+    HAL_GPIO_TogglePin(mpu->led_gpio, YLED_PIN);
+    return 0;
+}
+
+int8_t pet_watchdog(mpu_t *mpu)
+{
+    if (!mpu)
+        return -1;
+
+    HAL_GPIO_WritePin(mpu->watchdog_gpio, WATCHDOG_PIN, GPIO_PIN_SET);
+	HAL_GPIO_WritePin(mpu->watchdog_gpio, WATCHDOG_PIN, GPIO_PIN_RESET);
+    return 0;
+}
+
+static int8_t start_adcs(mpu_t *mpu)
+{
+    HAL_StatusTypeDef hal_stat;
+
+    hal_stat = HAL_ADC_Start(mpu->accel_adc1);
+    if (hal_stat)
+        return hal_stat;
+
+    hal_stat = HAL_ADC_Start(mpu->accel_adc2);
+    if (hal_stat)
+        return hal_stat;
+
+    hal_stat = HAL_ADC_Start(mpu->brake_adc);
+    if (hal_stat)
+        return hal_stat;
+
+    return 0;
+}
+
+static int8_t poll_adc_threaded(ADC_HandleTypeDef *adc)
+{
+    HAL_StatusTypeDef hal_stat = HAL_TIMEOUT;
+    while (hal_stat == HAL_TIMEOUT) {
+        hal_stat =  HAL_ADC_PollForConversion(adc, ADC_TIMEOUT);
+        
+        if (hal_stat == HAL_TIMEOUT)
+            osThreadYield();
+    }
+    return hal_stat;
+}
+
+/* Note: this should be called from within a thread since it yields to scheduler */
+int8_t read_adc(mpu_t *mpu, uint16_t raw[3])
+{
+    if (!mpu)
+        return -1;
+
+    osStatus_t mut_stat = osMutexAcquire(mpu->adc_mutex, osWaitForever);
+    if (mut_stat)
+        return mut_stat;
+
+    HAL_StatusTypeDef hal_stat = start_adcs(mpu);
+    if (hal_stat)
+        return hal_stat;
+
+    hal_stat = poll_adc_threaded(mpu->accel_adc1);
+    if (hal_stat)
+        return hal_stat;
+
+    hal_stat = poll_adc_threaded(mpu->accel_adc2);
+    if (hal_stat)
+        return hal_stat;
+
+    hal_stat = poll_adc_threaded(mpu->brake_adc);
+    if (hal_stat)
+        return hal_stat;
+
+    raw[0] = HAL_ADC_GetValue(mpu->accel_adc1);
+    raw[1] = HAL_ADC_GetValue(mpu->accel_adc2);
+    raw[2] = HAL_ADC_GetValue(mpu->brake_adc);
+
+    osMutexRelease(mpu->adc_mutex);
+
+    return 0;
+}
+
+int8_t read_temp_sensor(mpu_t *mpu, uint16_t *temp, uint16_t *humidity)
+{
+    if (!mpu)
+        return -1;
+
+    osStatus_t mut_stat = osMutexAcquire(mpu->i2c_mutex, osWaitForever);
+    if (mut_stat)
+        return mut_stat;
+    
+    HAL_StatusTypeDef hal_stat = sht30_get_temp_humid(mpu->temp_sensor);
+    if (hal_stat)
+        return hal_stat;
+
+    *temp = mpu->temp_sensor->temp;
+    *humidity = mpu->temp_sensor->humidity;
+
+    osMutexRelease(mpu->i2c_mutex);
+    return 0;
+}
+
+int8_t read_accel(mpu_t *mpu, uint16_t accel[3])
+{
+    if (!mpu)
+        return -1;
+
+    osStatus_t mut_stat = osMutexAcquire(mpu->i2c_mutex, osWaitForever);
+    if (mut_stat)
+        return mut_stat;
+    
+    HAL_StatusTypeDef hal_stat = lsm6dso_read_accel(mpu->imu);
+    if (hal_stat)
+        return hal_stat;
+
+    memcpy(accel, mpu->imu->accel_data, 3);
+
+    osMutexRelease(mpu->i2c_mutex);
+    return 0;
+}
+
+int8_t read_gyro(mpu_t *mpu, uint16_t gyro[3])
+{
+    if (!mpu)
+        return -1;
+
+    osStatus_t mut_stat = osMutexAcquire(mpu->i2c_mutex, osWaitForever);
+    if (mut_stat)
+        return mut_stat;
+    
+    HAL_StatusTypeDef hal_stat = lsm6dso_read_gyro(mpu->imu);
+    if (hal_stat)
+        return hal_stat;
+
+    memcpy(gyro, mpu->imu->gyro_data, 3);
+
+    osMutexRelease(mpu->i2c_mutex);
+    return 0;
+}
diff --git a/Core/Src/serial_monitor.c b/Core/Src/serial_monitor.c
index 68a590c0..0caeb59e 100644
--- a/Core/Src/serial_monitor.c
+++ b/Core/Src/serial_monitor.c
@@ -2,6 +2,7 @@
 #include <stdarg.h>
 #include <stdio.h>
 #include <string.h>
+#include <stdlib.h>
 
 #define PRINTF_QUEUE_SIZE 16  /* Strings */
 #define PRINTF_BUFFER_LEN 128 /* Characters */
diff --git a/Core/Src/torque.c b/Core/Src/torque.c
index 29d9aa3c..808a134e 100644
--- a/Core/Src/torque.c
+++ b/Core/Src/torque.c
@@ -40,7 +40,7 @@ void vCalcTorque(void* pv_params) {
 	assert(delay_time < MAX_COMMAND_DELAY);
 
 	pedals_t pedal_data;
-	uint16_t torque;
+	uint16_t torque = 0;
 	osStatus_t stat;
 	dti_t mc;
 
diff --git a/Drivers/Embedded-Base b/Drivers/Embedded-Base
index 8a5c28a9..d102586d 160000
--- a/Drivers/Embedded-Base
+++ b/Drivers/Embedded-Base
@@ -1 +1 @@
-Subproject commit 8a5c28a97067ddf995ce5fcff649398681c80fbd
+Subproject commit d102586d9d42579131fb0fee3c802de0ca5fdeac
diff --git a/Makefile b/Makefile
index 959bb074..7192262f 100644
--- a/Makefile
+++ b/Makefile
@@ -54,6 +54,7 @@ Core/Src/dti.c \
 Core/Src/state_machine.c \
 Core/Src/torque.c \
 Core/Src/pdu.c \
+Core/Src/mpu.c \
 Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_can.c \
 Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rcc.c \
 Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rcc_ex.c \
@@ -165,7 +166,7 @@ C_INCLUDES =  \
 # compile gcc flags
 ASFLAGS = $(MCU) $(AS_DEFS) $(AS_INCLUDES) $(OPT) -Wall -fdata-sections -ffunction-sections
 
-CFLAGS += $(MCU) $(C_DEFS) $(C_INCLUDES) $(OPT) -Wall -fdata-sections -ffunction-sections
+CFLAGS += $(MCU) $(C_DEFS) $(C_INCLUDES) $(OPT) -Wall -fdata-sections -ffunction-sections -Werror
 
 ifeq ($(DEBUG), 1)
 CFLAGS += -g -gdwarf-2