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 e26d82db..fd626db1 100644
--- a/Core/Src/main.c
+++ b/Core/Src/main.c
@@ -741,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 */
diff --git a/Core/Src/monitor.c b/Core/Src/monitor.c
index 4779c500..86627a92 100644
--- a/Core/Src/monitor.c
+++ b/Core/Src/monitor.c
@@ -24,39 +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 };
 	can_msg_t temp_msg
-		= { .id = CANID_TEMP_SENSOR, .len = can_msg_len, .data = { 0 } };
+		= { .id = CANID_TEMP_SENSOR, .len = 4, .data = { 0 } };
 
 	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);
 	}
 }
 
@@ -91,21 +90,18 @@ 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*/
 	mpu_t *mpu = (mpu_t *)pv_params;
@@ -167,7 +163,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;
@@ -180,80 +177,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);
 	}
 }
 
@@ -305,16 +293,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/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/Makefile b/Makefile
index 773ebe53..7192262f 100644
--- a/Makefile
+++ b/Makefile
@@ -166,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