diff --git a/Apps/Inc/CAN_Queue.h b/Apps/Inc/CAN_Queue.h
deleted file mode 100644
index 5425b4a2..00000000
--- a/Apps/Inc/CAN_Queue.h
+++ /dev/null
@@ -1,25 +0,0 @@
-/**
- * @copyright Copyright (c) 2018-2023 UT Longhorn Racing Solar
- * @file CAN_Queue.h
- * @brief Queue that holds all CAN messages that Task_CANBusConsumer needs to send.
- * 
- * @defgroup CAN_Queue
- * @addtogroup CAN_Queue
- * @{
- */
-
-#ifndef CAN_QUEUE_H
-#define CAN_QUEUE_H
-
-#include "CANbus.h"
-
-void CAN_Queue_Init(void);
-
-ErrorStatus CAN_Queue_Post(CANDATA_t message);
-
-ErrorStatus CAN_Queue_Pend(CANDATA_t *message);
-
-#endif
-
-
-/* @} */
diff --git a/Apps/Inc/SendCarCAN.h b/Apps/Inc/SendCarCAN.h
new file mode 100644
index 00000000..99772bfb
--- /dev/null
+++ b/Apps/Inc/SendCarCAN.h
@@ -0,0 +1,23 @@
+#ifndef __SENDCARCAN_H
+#define __SENDCARCAN_H
+
+#include "CANbus.h"
+
+/**
+ * @brief Initialize SendCarCAN
+*/
+void SendCarCAN_Init();
+
+/**
+ * @brief Wrapper to put new message in the CAN queue
+*/
+void SendCarCAN_Put(CANDATA_t message);
+
+/**
+ * @brief return the space left in SendCarCAN_Q for debug purposes
+*/
+#ifdef DEBUG
+uint8_t get_SendCarCAN_Q_Space(void);
+#endif
+
+#endif
\ No newline at end of file
diff --git a/Apps/Inc/SendTritium.h b/Apps/Inc/SendTritium.h
index a132d64a..65ed7be2 100644
--- a/Apps/Inc/SendTritium.h
+++ b/Apps/Inc/SendTritium.h
@@ -12,7 +12,7 @@
 
 #include "common.h"
 
-// #define SENDTRITIUM_PRINT_MES
+#define SENDTRITIUM_PRINT_MES
 
 #define MOTOR_MSG_PERIOD 100
 #define FSM_PERIOD 100
@@ -20,9 +20,9 @@
 #define MOTOR_MSG_COUNTER_THRESHOLD (MOTOR_MSG_PERIOD)/(FSM_PERIOD)
 
 #define FOREACH_Gear(GEAR) \
-        GEAR(FORWARD_GEAR)   \
-        GEAR(NEUTRAL_GEAR)  \
-        GEAR(REVERSE_GEAR)   \
+        GEAR(FORWARD_GEAR),   \
+        GEAR(NEUTRAL_GEAR),  \
+        GEAR(REVERSE_GEAR),   \
 
 typedef enum GEAR_ENUM {
     FOREACH_Gear(GENERATE_ENUM)
@@ -49,6 +49,23 @@ typedef struct TritiumState{
     void (*stateDecider)(void);
 } TritiumState_t;
 
+#ifdef SENDTRITIUM_EXPOSE_VARS
+// Inputs
+extern bool cruiseEnable;
+extern bool cruiseSet;
+extern bool onePedalEnable;
+extern bool regenEnable;
+
+extern uint8_t brakePedalPercent;
+extern uint8_t accelPedalPercent;
+
+extern Gear_t gear;
+
+extern TritiumState_t state;
+extern float velocityObserved;
+extern float cruiseVelSetpoint;
+#endif
+
 // Getter functions for local variables in SendTritium.c
 EXPOSE_GETTER(bool, cruiseEnable)
 EXPOSE_GETTER(bool, cruiseSet)
diff --git a/Apps/Inc/Tasks.h b/Apps/Inc/Tasks.h
index 8d02dd1e..fdd45efb 100644
--- a/Apps/Inc/Tasks.h
+++ b/Apps/Inc/Tasks.h
@@ -32,10 +32,9 @@
 #define TASK_SEND_TRITIUM_PRIO              4
 #define TASK_READ_CAR_CAN_PRIO              5
 #define TASK_UPDATE_DISPLAY_PRIO            6
-#define TASK_SEND_CAR_CAN_PRIO              8
-#define TASK_TELEMETRY_PRIO                 9
-#define TASK_DEBUG_DUMP_PRIO                10
-#define TASK_COMMAND_LINE_PRIO              11
+#define TASK_SEND_CAR_CAN_PRIO              7
+#define TASK_DEBUG_DUMP_PRIO                8
+#define TASK_COMMAND_LINE_PRIO              9
 
 /**
  * Stack Sizes
@@ -49,7 +48,6 @@
 #define TASK_UPDATE_DISPLAY_STACK_SIZE      DEFAULT_STACK_SIZE
 #define TASK_READ_TRITIUM_STACK_SIZE        DEFAULT_STACK_SIZE
 #define TASK_SEND_CAR_CAN_STACK_SIZE        DEFAULT_STACK_SIZE
-#define TASK_TELEMETRY_STACK_SIZE           DEFAULT_STACK_SIZE
 #define TASK_DEBUG_DUMP_STACK_SIZE          DEFAULT_STACK_SIZE
 #define TASK_COMMAND_LINE_STACK_SIZE        DEFAULT_STACK_SIZE
 
@@ -73,8 +71,6 @@ void Task_ReadTritium(void* p_arg);
 
 void Task_SendCarCAN(void* p_arg);
 
-void Task_Telemetry(void* p_arg);
-
 void Task_DebugDump(void *p_arg);
 
 void Task_CommandLine(void* p_arg);
@@ -90,7 +86,6 @@ extern OS_TCB ReadCarCAN_TCB;
 extern OS_TCB UpdateDisplay_TCB;
 extern OS_TCB ReadTritium_TCB;
 extern OS_TCB SendCarCAN_TCB;
-extern OS_TCB Telemetry_TCB;
 extern OS_TCB DebugDump_TCB;
 extern OS_TCB CommandLine_TCB;
 
@@ -104,7 +99,6 @@ extern CPU_STK ReadCarCAN_Stk[TASK_READ_CAR_CAN_STACK_SIZE];
 extern CPU_STK UpdateDisplay_Stk[TASK_UPDATE_DISPLAY_STACK_SIZE];
 extern CPU_STK ReadTritium_Stk[TASK_READ_TRITIUM_STACK_SIZE];
 extern CPU_STK SendCarCAN_Stk[TASK_SEND_CAR_CAN_STACK_SIZE];
-extern CPU_STK Telemetry_Stk[TASK_TELEMETRY_STACK_SIZE];
 extern CPU_STK DebugDump_Stk[TASK_DEBUG_DUMP_STACK_SIZE];
 extern CPU_STK CommandLine_Stk[TASK_COMMAND_LINE_STACK_SIZE];
 
diff --git a/Apps/Inc/common.h b/Apps/Inc/common.h
index a8475f8c..3c5d7722 100644
--- a/Apps/Inc/common.h
+++ b/Apps/Inc/common.h
@@ -21,8 +21,8 @@
 #include "config.h"
 
 /* Used for generating ENUMS */
-#define GENERATE_ENUM(ENUM) ENUM,
-#define GENERATE_STRING(STRING) #STRING,
+#define GENERATE_ENUM(ENUM) ENUM
+#define GENERATE_STRING(STRING) #STRING
 
 /**
  * Used for creating getter functions (returns the value based on given inputs) 
diff --git a/Apps/Src/CAN_Queue.c b/Apps/Src/CAN_Queue.c
deleted file mode 100644
index 98daa1f0..00000000
--- a/Apps/Src/CAN_Queue.c
+++ /dev/null
@@ -1,69 +0,0 @@
-/**
- * @copyright Copyright (c) 2018-2023 UT Longhorn Racing Solar
- * @file CAN_Queue.c
- * @brief 
- * 
- */
-
-#include "CAN_Queue.h"
-#include "os.h"
-#include "CANbus.h"
-#include "Tasks.h"
-
-// fifo
-#define FIFO_TYPE CANDATA_t
-#define FIFO_SIZE 256
-#define FIFO_NAME CAN_fifo
-#include "fifo.h"
-
-static CAN_fifo_t canFifo;
-static OS_SEM canFifo_Sem4;
-static OS_MUTEX canFifo_Mutex;
-
-void CAN_Queue_Init(void) {
-    OS_ERR err;
-    CPU_TS ticks;
-    OSMutexCreate(&canFifo_Mutex, "CAN queue mutex", &err);
-    assertOSError(err);
-    OSSemCreate(&canFifo_Sem4,
-                "CAN queue semaphore",
-                0,
-                &err);
-    assertOSError(err);
-    OSMutexPend(&canFifo_Mutex, 0, OS_OPT_POST_NONE, &ticks, &err);
-    assertOSError(err);
-    CAN_fifo_renew(&canFifo);
-    OSMutexPost(&canFifo_Mutex, OS_OPT_POST_NONE, &err);
-    assertOSError(err);
-}
-
-ErrorStatus CAN_Queue_Post(CANDATA_t message) {
-    OS_ERR err;
-    CPU_TS ticks;
-    OSMutexPend(&canFifo_Mutex, 0, OS_OPT_POST_NONE, &ticks, &err);
-    assertOSError(err);
-    bool success = CAN_fifo_put(&canFifo, message);
-    OSMutexPost(&canFifo_Mutex, OS_OPT_POST_NONE, &err);
-    assertOSError(err);
-
-    if (success) {
-        OSSemPost(&canFifo_Sem4, OS_OPT_POST_1, &err);
-        assertOSError(err);
-    }
-
-    return success ? SUCCESS : ERROR;
-}
-
-ErrorStatus CAN_Queue_Pend(CANDATA_t *message) {
-    OS_ERR err;
-	CPU_TS ticks;
-    
-    OSSemPend(&canFifo_Sem4, 0, OS_OPT_PEND_BLOCKING, &ticks, &err);
-    assertOSError(err);
-    OSMutexPend(&canFifo_Mutex, 0, OS_OPT_POST_NONE, &ticks, &err);
-    assertOSError(err);
-    bool result = CAN_fifo_get(&canFifo, message);
-    OSMutexPost(&canFifo_Mutex, OS_OPT_POST_NONE, &err);
-    assertOSError(err);
-    return result ? SUCCESS : ERROR;
-}
diff --git a/Apps/Src/DebugDump.c b/Apps/Src/DebugDump.c
index 672f04ff..15cd48e9 100644
--- a/Apps/Src/DebugDump.c
+++ b/Apps/Src/DebugDump.c
@@ -2,7 +2,6 @@
 #include "Tasks.h"
 #include "bsp.h"
 #include "CANbus.h"
-#include "CAN_Queue.h"
 #include "Pedals.h"
 #include "Minions.h"
 #include "Contactors.h"
diff --git a/Apps/Src/ReadTritium.c b/Apps/Src/ReadTritium.c
index 22f1b4ef..3b08c339 100755
--- a/Apps/Src/ReadTritium.c
+++ b/Apps/Src/ReadTritium.c
@@ -6,9 +6,9 @@
  */
 
 #include "ReadTritium.h"
-#include "CAN_Queue.h"
 #include "CANbus.h"
 #include "UpdateDisplay.h"
+#include "SendCarCAN.h"
 #include "os_cfg_app.h"
 #include <string.h>
 
@@ -43,7 +43,7 @@ Objective 1:
 	- if error
 		- assertOSError
 - determine information important to telementry
-	- SendCarCAN
+	- Telemetry
 - determine information important for storage
 	- acquire mutex on Storage Array
 	- Store information in Storage Array (based on index) 
@@ -104,6 +104,8 @@ void Task_ReadTritium(void *p_arg){
 				}
 
 			}
+            
+            SendCarCAN_Put(dataBuf); // Forward message on CarCAN for telemetry
 		}
 
 		OSTimeDlyHMSM(0, 0, 0, 10, OS_OPT_TIME_HMSM_NON_STRICT, &err);
diff --git a/Apps/Src/SendCarCAN.c b/Apps/Src/SendCarCAN.c
index 8079707c..612d1a35 100644
--- a/Apps/Src/SendCarCAN.c
+++ b/Apps/Src/SendCarCAN.c
@@ -1,36 +1,169 @@
 /**
  * @copyright Copyright (c) 2018-2023 UT Longhorn Racing Solar
  * @file SendCarCAN.c
- * @brief Implements the SendCarCAN Task
+ * @brief Function implementations for the SendCarCAN application.
  * 
- * Resends the feedback from the motor controller
+ * This contains functions relevant to placing CAN messages in a CarCAN queue and periodically sending
+ * those messages in the SendCarCAN task.
  * 
  */
 
-#include "Tasks.h"
-#include "CANbus.h"
-#include "CAN_Queue.h"
 #include "common.h"
+#include "os_cfg_app.h"
+#include "CANbus.h"
+#include "Minions.h"
+#include "Contactors.h"
+#include "Pedals.h"
+#include "Tasks.h"
+#include "SendCarCAN.h"
+#include "SendTritium.h"
+
+#define IO_STATE_DLY_MS 250u 
+
+// Task_PutIOState
+OS_TCB putIOState_TCB;
+CPU_STK putIOState_Stk[TASK_SEND_CAR_CAN_STACK_SIZE];
+
+//fifo
+#define FIFO_TYPE CANDATA_t
+#define FIFO_SIZE 16
+#define FIFO_NAME SendCarCAN_Q
+#include "fifo.h"
+
+static SendCarCAN_Q_t CANFifo; 
+
+static OS_SEM CarCAN_Sem4;
+static OS_MUTEX CarCAN_Mtx;
+
+static void Task_PutIOState(void *p_arg);
 
 /**
- * @brief Sends the feedback from the motor controller to be read by telemetry and 
- * sends pedal, switch, light, and contactor information to be read by telemetry
- * 
- * @param p_arg 
- */
+ * @brief return the space left in SendCarCAN_Q for debug purposes
+*/
+#ifdef DEBUG
+uint8_t get_SendCarCAN_Q_Space(void) {
+    return (CANFifo.get - CANFifo.put - 1) % (sizeof CANFifo.buffer / sizeof CANFifo.buffer[0]);
+}
+#endif
+
+/**
+ * @brief Wrapper to put new message in the CAN queue
+ * @param message the CAN message to put in SendCarCAN Queue
+*/
+void SendCarCAN_Put(CANDATA_t message){
+    OS_ERR err;
+    CPU_TS ticks;
+
+    OSMutexPend(&CarCAN_Mtx, 0, OS_OPT_PEND_BLOCKING, &ticks, &err);
+    assertOSError(err);
+    
+    bool success = SendCarCAN_Q_put(&CANFifo, message);
+
+    OSMutexPost(&CarCAN_Mtx, OS_OPT_POST_NONE, &err);
+    assertOSError(err);
+
+    if(success) OSSemPost(&CarCAN_Sem4, OS_OPT_POST_1, &err);
+    assertOSError(err);
+}
+
+/**
+ * @brief Initialize SendCarCAN
+*/
+void SendCarCAN_Init(void) {
+    OS_ERR err;
+    
+    OSMutexCreate(&CarCAN_Mtx, "CarCAN_Mtx", &err);
+    assertOSError(err);
+    
+    OSSemCreate(&CarCAN_Sem4, "CarCAN_Sem4", 0, &err);
+    assertOSError(err);
+
+    SendCarCAN_Q_renew(&CANFifo);
+}
+
+/**
+ * @brief Grabs the latest messages from the queue and sends over CarCAN
+*/
 void Task_SendCarCAN(void *p_arg){
-    CANDATA_t motorMsg;
     OS_ERR err;
+    CPU_TS ticks;
+
+    CANDATA_t message;
+    memset(&message, 0, sizeof message);
+
+    // PutIOState
+    OSTaskCreate(
+        (OS_TCB*)&putIOState_TCB,
+        (CPU_CHAR*)"PutIOState",
+        (OS_TASK_PTR)Task_PutIOState,
+        (void*)NULL,
+        (OS_PRIO)TASK_SEND_CAR_CAN_PRIO, // Round-robin with SendCarCAN task
+        (CPU_STK*)putIOState_Stk,
+        (CPU_STK_SIZE)WATERMARK_STACK_LIMIT,
+        (CPU_STK_SIZE)TASK_SEND_CAR_CAN_STACK_SIZE,
+        (OS_MSG_QTY)0,
+        (OS_TICK)0,
+        (void*)NULL,
+        (OS_OPT)(OS_OPT_TASK_STK_CLR),
+        (OS_ERR*)&err
+    );
+    assertOSError(err);
+
+    while (1) {
+          
+        // Check if there's something to send in the queue (either IOState or Car state from sendTritium)
+        OSSemPend(&CarCAN_Sem4, 0, OS_OPT_PEND_BLOCKING, &ticks, &err);
+        assertOSError(err);
+
+        OSMutexPend(&CarCAN_Mtx, 0, OS_OPT_PEND_BLOCKING, &ticks, &err);
+        assertOSError(err);
+    
+        bool res = SendCarCAN_Q_get(&CANFifo, &message);
+        assertOSError(err);
+
+        OSMutexPost(&CarCAN_Mtx, OS_OPT_POST_NONE, &err);
+        assertOSError(err);
 
+        if(res) CANbus_Send(message, true, CARCAN);
+        
+    }
+}
+
+static void putIOState(void){
+    CANDATA_t message;
+    memset(&message, 0, sizeof message);
+    message.ID = IO_STATE;
+    
+    // Get pedal information
+    message.data[0] = Pedals_Read(ACCELERATOR);
+    message.data[1] = Pedals_Read(BRAKE);
+
+    // Get minion information
+    for(pin_t pin = 0; pin < NUM_PINS; pin++){
+        bool pinState = Minions_Read(pin);
+        if(pin == IGN_1 || pin == IGN_2) pinState = !pinState;
+        message.data[2] |= pinState << pin;
+    }
+    
+    // Get contactor info
+    for(contactor_t contactor = 0; contactor < NUM_CONTACTORS; contactor++){
+        bool contactorState = (Contactors_Get(contactor) == ON) ? true : false;
+        message.data[3] |= contactorState << contactor;
+    }
+    // Tell BPS if the array contactor should be on
+    message.data[3] |= (!Minions_Read(IGN_1) || !Minions_Read(IGN_2)) << 2;
+
+    CANbus_Send(message, true, CARCAN);
+}
+
+/**
+ * @brief sends IO information over CarCAN every IO_STATE_DLY_MS
+*/
+static void Task_PutIOState(void *p_arg) {
+    OS_ERR err;
     while (1) {
-        // Send motor msg
-        CAN_Queue_Pend(&motorMsg);
-        CANbus_Send(motorMsg, true, CARCAN);
-
-        // Delay of few milliseconds (100)
-        OSTimeDlyHMSM(0, 0, 0, 100, OS_OPT_TIME_HMSM_STRICT, &err);
-        if (err != OS_ERR_NONE){
-            assertOSError(err);
-        }
+        putIOState();
+        OSTimeDlyHMSM(0, 0, 0, IO_STATE_DLY_MS, OS_OPT_TIME_HMSM_STRICT, &err);
     }
+   
 }
diff --git a/Apps/Src/SendTritium.c b/Apps/Src/SendTritium.c
index 3a34e72e..6ae26c2d 100644
--- a/Apps/Src/SendTritium.c
+++ b/Apps/Src/SendTritium.c
@@ -8,9 +8,11 @@
  * controlled mode, a one-pedal driving mode (with regenerative braking), and cruise control.
  * The logic is determined through a finite state machine implementation.
  * 
- * If the macro DEBUG is defined prior to including SendTritium.h, relevant
- * setters will be exposed for unit testing.
- * 
+ * If the macro SENDTRITIUM_EXPOSE_VARS is defined prior to including SendTritium.h, 
+ * relevant setters will be exposed as externs for unit testing
+ * and hardware inputs won't be read and motor commands won't be sent over MotorCAN.
+ * If the macro SENDTRITIUM_PRINT_MES is also defined prior to including SendTritium.h,
+ * debug info will be printed via UART.
  */
 
 #include "Pedals.h"
@@ -18,6 +20,7 @@
 #include "Minions.h"
 #include "SendTritium.h"
 #include "ReadTritium.h"
+#include "SendCarCAN.h"
 #include "CANbus.h"
 #include "UpdateDisplay.h"
 #include "CANConfig.h"
@@ -59,9 +62,10 @@ float velocitySetpoint = 0;
 float cruiseVelSetpoint = 0;
 
 // Current observed velocity
+
+#ifndef SENDTRITIUM_EXPOSE_VARS
 static float velocityObserved = 0;
 
-#ifndef SENDTRITIUM_PRINT_MES
 // Counter for sending setpoints to motor
 static uint8_t motorMsgCounter = 0;
 #endif
@@ -317,6 +321,20 @@ static uint8_t map(uint8_t input, uint8_t in_min, uint8_t in_max, uint8_t out_mi
     }
 }
 
+
+/**
+ * @brief Put the CONTROL_MODE message onto the CarCAN bus, detailing
+ * the current mode of control.
+*/
+static void putControlModeCAN(){
+    CANDATA_t message;
+    memset(&message, 0, sizeof(message));
+    message.ID = CONTROL_MODE;
+    message.data[0] = state.name;
+
+    SendCarCAN_Put(message);
+}
+
 // State Handlers & Deciders
 
 /**
@@ -632,7 +650,7 @@ void Task_SendTritium(void *p_arg){
     state = FSM[NEUTRAL_DRIVE];
     prevState = FSM[NEUTRAL_DRIVE];
 
-    #ifndef SENDTRITIUM_PRINT_MES
+    #ifndef SENDTRITIUM_EXPOSE_VARS
     CANDATA_t driveCmd = {
         .ID=MOTOR_DRIVE, 
         .idx=0,
@@ -653,7 +671,8 @@ void Task_SendTritium(void *p_arg){
         // Drive
         #ifdef SENDTRITIUM_PRINT_MES
         dumpInfo();
-        #else
+        #endif
+        #ifndef SENDTRITIUM_EXPOSE_VARS
         if(MOTOR_MSG_COUNTER_THRESHOLD == motorMsgCounter){
             memcpy(&driveCmd.data[4], &currentSetpoint, sizeof(float));
             memcpy(&driveCmd.data[0], &velocitySetpoint, sizeof(float));
@@ -664,6 +683,8 @@ void Task_SendTritium(void *p_arg){
         }
         #endif
 
+        putControlModeCAN();        
+
         // Delay of MOTOR_MSG_PERIOD ms
         OSTimeDlyHMSM(0, 0, 0, MOTOR_MSG_PERIOD, OS_OPT_TIME_HMSM_STRICT, &err);
         if (err != OS_ERR_NONE){
diff --git a/Apps/Src/Tasks.c b/Apps/Src/Tasks.c
index 8ec996ba..02b71c6e 100644
--- a/Apps/Src/Tasks.c
+++ b/Apps/Src/Tasks.c
@@ -26,7 +26,6 @@ OS_TCB ReadCarCAN_TCB;
 OS_TCB UpdateDisplay_TCB;
 OS_TCB ReadTritium_TCB;
 OS_TCB SendCarCAN_TCB;
-OS_TCB Telemetry_TCB;
 OS_TCB DebugDump_TCB;
 OS_TCB CommandLine_TCB;
 
@@ -41,7 +40,6 @@ CPU_STK ReadCarCAN_Stk[TASK_READ_CAR_CAN_STACK_SIZE];
 CPU_STK UpdateDisplay_Stk[TASK_UPDATE_DISPLAY_STACK_SIZE];
 CPU_STK ReadTritium_Stk[TASK_READ_TRITIUM_STACK_SIZE];
 CPU_STK SendCarCAN_Stk[TASK_SEND_CAR_CAN_STACK_SIZE];
-CPU_STK Telemetry_Stk[TASK_TELEMETRY_STACK_SIZE];
 CPU_STK DebugDump_Stk[TASK_DEBUG_DUMP_STACK_SIZE];
 CPU_STK CommandLine_Stk[TASK_COMMAND_LINE_STACK_SIZE];
 
diff --git a/Apps/Src/Telemetry.c b/Apps/Src/Telemetry.c
deleted file mode 100644
index dba4dc73..00000000
--- a/Apps/Src/Telemetry.c
+++ /dev/null
@@ -1,65 +0,0 @@
-/**
- * @copyright Copyright (c) 2018-2023 UT Longhorn Racing Solar
- * @file Telemetry.c
- * @brief Implements the SendCarCAN Task
- * 
- * Creates a datatype to house the data to be read by telemetry
- * 
- * Gathers the information of the pedals, lights, switches, and contactors 
- * to be read by telemetry
- * 
- */
-
-#include "Tasks.h"
-#include "CANbus.h"
-#include "CAN_Queue.h"
-#include "Pedals.h"
-#include "Minions.h"
-#include "Contactors.h"
-#include "common.h"
-
-// Make sure updated to the CarData_t and carMSGID are reflected in the CAN Bus IDs excel sheet
-
-/**
- * @brief Sends pedal, switch, light, and contactor information to be read by telemetry
- * 
- * @param p_arg 
- */
-void Task_Telemetry(void *p_arg){
-    CANDATA_t carMsg;
-    carMsg.ID = CARDATA;  // ID is wrong
-    for(int i = 4; i < 8; i++){
-        carMsg.data[i] = 0;
-    }
-    OS_ERR err;
-
-
-    while (1) {
-        // Get pedal information
-        carMsg.data[0] = Pedals_Read(ACCELERATOR);
-        carMsg.data[1] = Pedals_Read(BRAKE);
-
-        // Get minion information
-        carMsg.data[2] = 0;
-        for(pin_t pin = 0; pin < NUM_PINS; pin++){
-            bool pinState = Minions_Read(pin);
-            carMsg.data[2] |= pinState << pin;
-        }
-
-        // Get contactor info
-        carMsg.data[3] = 0;
-        for(contactor_t contactor = 0; contactor < NUM_CONTACTORS; contactor++){
-            bool contactorState = Contactors_Get(contactor) == ON ? true : false;
-            carMsg.data[3] |= contactorState << contactor;
-        }
-
-        // Send car msg
-        CANbus_Send(carMsg, true, CARCAN);
-
-        // Delay of few milliseconds (500)
-        OSTimeDlyHMSM(0, 0, 0, 500, OS_OPT_TIME_HMSM_STRICT, &err);
-        if (err != OS_ERR_NONE){
-            assertOSError(err);
-        }
-    }
-}
diff --git a/Apps/Src/common.c b/Apps/Src/common.c
index 02a76931..0ad5e1d0 100644
--- a/Apps/Src/common.c
+++ b/Apps/Src/common.c
@@ -1,7 +1,7 @@
 #include "common.h"
 
 void print_float(char * str, float f) {
-    if(str) printf(str);
+    if(str) printf("%s", str);
 
     int32_t n = (int32_t)f;
     f -= n;
@@ -9,3 +9,11 @@ void print_float(char * str, float f) {
     int32_t d = (f<0)?-f:f;
     printf("%d.%02d\n\r", (int)n, (int)d);
 }
+
+void print_bin(char * str, uint32_t i){
+    if(str) printf("%s", str);
+
+    for(uint32_t mask=0x80000000L; mask > 0L; mask >>= 1){
+        printf("%d\n\r",(mask & i)?1:0);
+    }
+}
\ No newline at end of file
diff --git a/Apps/Src/main.c b/Apps/Src/main.c
index 7fe1ed2c..aeb19ff1 100644
--- a/Apps/Src/main.c
+++ b/Apps/Src/main.c
@@ -15,8 +15,8 @@
 #include "Display.h"
 #include "Minions.h"
 #include "Pedals.h"
-#include "CAN_Queue.h"
 #include "UpdateDisplay.h"
+#include "SendCarCAN.h"
 
 #define IGN_CONT_PERIOD 100
 
@@ -80,10 +80,10 @@ void Task_Init(void *p_arg){
     Contactors_Init();
     Display_Init();
     Minions_Init();
-    CAN_Queue_Init();
 
     // Initialize applications
     UpdateDisplay_Init();
+    SendCarCAN_Init();
 
     // Initialize SendTritium
     OSTaskCreate(
@@ -175,7 +175,7 @@ void Task_Init(void *p_arg){
     );
     assertOSError(err);
 
-
+    
     while(1){
         CPU_WaitForInt();
     }
diff --git a/BSP/STM32F413/Src/BSP_CAN.c b/BSP/STM32F413/Src/BSP_CAN.c
index a45ef70c..60dbbc65 100644
--- a/BSP/STM32F413/Src/BSP_CAN.c
+++ b/BSP/STM32F413/Src/BSP_CAN.c
@@ -332,6 +332,7 @@ void BSP_CAN3_Init(uint16_t* idWhitelist, uint8_t idWhitelistSize)
 ErrorStatus BSP_CAN_Write(CAN_t bus, uint32_t id, uint8_t data[8], uint8_t length)
 {
 
+    memset(gTxMessage[bus].Data, 0, sizeof gTxMessage[bus].Data);
     gTxMessage[bus].StdId = id;
     gTxMessage[bus].DLC = length;
     for (int i = 0; i < length; i++)
diff --git a/Docs/source/Apps/ExtraFiles.rst b/Docs/source/Apps/ExtraFiles.rst
index b8a99f62..77148c91 100644
--- a/Docs/source/Apps/ExtraFiles.rst
+++ b/Docs/source/Apps/ExtraFiles.rst
@@ -4,26 +4,6 @@ Extra Files
 
 Some files in the Apps folder don't contain any task code, but instead contain higher level interfaces, calibration data, and supporting structures. Some of these files will likely be relocated in the future.
 
-.. _can-queue:
-
-=========
-CAN Queue
-=========
-
-Multiple tasks need to send messages over the car CAN bus. Sending is rather slow, so a queue is employed to absorbed the bursty nature of sending CAN messages. This way, multiple tasks can just post can messages to the queue, and another task (SendCarCAN) takes care to consume and send them to the CAN bus.
-
-The public interface of the file includes:
-
-``void CAN_Queue_Init(void)`` — Initialize the queue and its associated mutex.
-
-``ErrorStatus CAN_Queue_Post(CANDATA_t message)`` — Used by tasks that want to post CAN messages. The messages are held in a FIFO queue.
-
-``ErrorStatus CAN_Queue_Pend(CANDATA_t *message)`` — Used by SendCarCAN to consume the queue. This function blocks the caller until the queue contains at least one message, at which point it will return message and remove it from the queue.
-
-.. doxygengroup:: CAN_Queue
-   :project: doxygen
-   :path: "/doxygen/xml/group__CAN_Queue.xml"
-
 ================
 Pedal to Percent
 ================
diff --git a/Drivers/Inc/CANConfig.h b/Drivers/Inc/CANConfig.h
index 3f683213..9364a4bf 100644
--- a/Drivers/Inc/CANConfig.h
+++ b/Drivers/Inc/CANConfig.h
@@ -14,7 +14,7 @@
 /**
  * Filter Lists for CarCAN and MotorCAN
 */
-#define NUM_CARCAN_FILTERS 4
+#define NUM_CARCAN_FILTERS 6
 #define NUM_MOTORCAN_FILTERS 0
 extern  CANId_t carCANFilterList[NUM_CARCAN_FILTERS];
 extern  CANId_t motorCANFilterList[NUM_MOTORCAN_FILTERS];
diff --git a/Drivers/Inc/CANbus.h b/Drivers/Inc/CANbus.h
index 08779614..46e8b844 100644
--- a/Drivers/Inc/CANbus.h
+++ b/Drivers/Inc/CANbus.h
@@ -45,7 +45,8 @@ typedef enum {
 	TEMPERATURE 					= 0x24B,
 	ODOMETER_AMPHOURS 				= 0x24E,
 	ARRAY_CONTACTOR_STATE_CHANGE 	= 0x24F,
-	CARDATA 						= 0x581,
+	CONTROL_MODE                    = 0x580,
+    IO_STATE 						= 0x581,
 	MAX_CAN_ID
 } CANId_t;
 
diff --git a/Drivers/Inc/Contactors.h b/Drivers/Inc/Contactors.h
index d9cb83ef..77cf7ec1 100644
--- a/Drivers/Inc/Contactors.h
+++ b/Drivers/Inc/Contactors.h
@@ -22,8 +22,8 @@
 #define MOTOR_CONTROLLER_PRECHARGE_BYPASS_PIN           GPIO_Pin_12     
 
 #define FOREACH_contactor(contactor) \
-        contactor(ARRAY_PRECHARGE_BYPASS_CONTACTOR)  \
-        contactor(MOTOR_CONTROLLER_PRECHARGE_BYPASS_CONTACTOR)   \
+        contactor(ARRAY_PRECHARGE_BYPASS_CONTACTOR),  \
+        contactor(MOTOR_CONTROLLER_PRECHARGE_BYPASS_CONTACTOR),   \
 
 typedef enum contactor_ENUM {
     FOREACH_contactor(GENERATE_ENUM)
diff --git a/Drivers/Inc/Minions.h b/Drivers/Inc/Minions.h
index 52d628f0..4ef240c2 100644
--- a/Drivers/Inc/Minions.h
+++ b/Drivers/Inc/Minions.h
@@ -17,16 +17,17 @@
 #include <stdbool.h>
 #include "BSP_GPIO.h"
 
-/* Should keep in line with the LUT in Minions.c */
+// used to index into lookup table
+// if changed, PINS_LOOKARR should be changed in Minions.c
 #define FOREACH_PIN(PIN) \
-        PIN(IGN_1)   \
-        PIN(IGN_2)  \
-        PIN(REGEN_SW)   \
-        PIN(FOR_SW)  \
-        PIN(REV_SW)  \
-        PIN(CRUZ_EN)  \
-        PIN(CRUZ_ST)  \
-        PIN(BRAKELIGHT)  \
+        PIN(IGN_1),   \
+        PIN(IGN_2),  \
+        PIN(REGEN_SW),   \
+        PIN(FOR_SW),  \
+        PIN(REV_SW),  \
+        PIN(CRUZ_EN),  \
+        PIN(CRUZ_ST),  \
+        PIN(BRAKELIGHT),  \
 
 typedef enum MINIONPIN_ENUM {
     FOREACH_PIN(GENERATE_ENUM)
diff --git a/Drivers/Src/CANConfig.c b/Drivers/Src/CANConfig.c
index 6afe3c07..ce8548d1 100644
--- a/Drivers/Src/CANConfig.c
+++ b/Drivers/Src/CANConfig.c
@@ -35,7 +35,12 @@ const CANLUT_T CANLUT[MAX_CAN_ID] = {
 	[TEMPERATURE] 					= {NOIDX, DOUBLE}, /**     TEMPERATURE                     **/
 	[ODOMETER_AMPHOURS] 			= {NOIDX, DOUBLE}, /**     ODOMETER_AMPHOURS               **/
 	[ARRAY_CONTACTOR_STATE_CHANGE] 	= {NOIDX, BYTE  }, /**     ARRAY_CONTACTOR_STATE_CHANGE    **/
-	[CARDATA] 						= {NOIDX, DOUBLE}, /**     CARDATA			               **/
+	[MOTOR_DRIVE] 					= {NOIDX, DOUBLE}, /**     MOTOR_DRIVE                     **/
+	[MOTOR_POWER]					= {NOIDX, DOUBLE}, /**     MOTOR_POWER                     **/
+	[MOTOR_RESET] 					= {NOIDX, DOUBLE}, /**     MOTOR_RESET                     **/
+	[MOTOR_STATUS] 					= {NOIDX, DOUBLE}, /**     MOTOR_STATUS                    **/
+	[IO_STATE] 				        = {NOIDX, DOUBLE}, /**     IO_STATE			               **/
+    [CONTROL_MODE]                  = {NOIDX, BYTE  }, /**     CONTROL_MODE			           **/
 };
 
 /**
diff --git a/Tests/Test_App_ReadCarCAN.c b/Tests/Test_App_ReadCarCAN.c
index d9805cdc..93db729d 100644
--- a/Tests/Test_App_ReadCarCAN.c
+++ b/Tests/Test_App_ReadCarCAN.c
@@ -1,6 +1,5 @@
 #include "Tasks.h"
 #include "CANbus.h"
-#include "CAN_Queue.h"
 #include "ReadCarCAN.h"
 #include "Contactors.h"
 #include "Display.h"
diff --git a/Tests/Test_App_SendCarCAN.c b/Tests/Test_App_SendCarCAN.c
index 78ef52c9..406b53a6 100644
--- a/Tests/Test_App_SendCarCAN.c
+++ b/Tests/Test_App_SendCarCAN.c
@@ -1,89 +1,339 @@
+/**
+ * This test file for SendCarCAN checks if messages added to the queue are sent on CarCAN.
+ * It does this by starting the SendTritium, SendCarCAN, and ReadTritium tasks,
+ * which place messages into the SendCarCAN_Q, and then counting the CarCAN messages it receives in msgReadCount.
+ * It also prints the IO State and, if the SendTritium macro SENDTRITIUM_PRINT_MES is defined, the control mode
+ * for comparison to see if the data received on CarCAN is accurate.
+ * 
+ * This test is intended to be run on hardware with CarCAN in LoopBack mode and the motor controller connected.
+ * The idea is to display the struct msgReadCount using a debugger view what messages we've received.
+ * Alternatively, you can just use it to spin up tasks and then check output using a logic analyzer.
+ * 
+ * If TEST_SOFTWARE is defined prior to compilation, then a fake "motor controller" task
+ * will also be created to send us motor messages.
+ * In this case, MotorCAN should also be in LoopBack mode.
+*/
+
 #include "Tasks.h"
+#include "BSP_UART.h"
 #include "CANbus.h"
-#include "stm32f4xx.h"
-#include "CAN_Queue.h"
+#include "os_cfg_app.h"
+#include "SendCarCAN.h"
+#include "Minions.h"
+#include "CANConfig.h"
+#include "SendTritium.h"
+#include "Contactors.h"
+#include "Pedals.h"
+#include "Display.h"
+#include "UpdateDisplay.h"
+
+#define TEST_SOFTWARE 
+
+#define NUM_MOTOR_MSGS 15 // Messages we forward from the motor controller
+#define NUM_NONMOTOR_MSGS 2 // We also send IO_STATE and CONTROL_MODE
+
+// indexes in msgArray for tracking info 
+#define IO_STATE_IDX 0 
+#define CONTROL_MODE_IDX 1
+
+#define MOTOR_MSG_BASE_ADDRESS 0x240 // Offset to index into msgCount array
+// Test data stored for an individual message type
+typedef struct {
+    CANDATA_t lastMsg;
+    uint16_t numReceived;
+} msgInfo;
+
+// Struct containing all test file message data
+typedef struct {
+    uint32_t lastReceiveTime_ms;
+    uint16_t lastMsgReceived;
+    msgInfo msgArray[NUM_MOTOR_MSGS + NUM_NONMOTOR_MSGS + 1]; // Message types we send plus an extra space 
+    uint8_t SpaceLeftInQ;
+} CANInfo;
+
 
 static OS_TCB Task1_TCB;
-static CPU_STK Task1_Stk[128];
+static CPU_STK Task1Stk[DEFAULT_STACK_SIZE];
+#ifdef TEST_SOFTWARE
+static OS_TCB TaskMC_TCB;
+static CPU_STK TaskMC_Stk[DEFAULT_STACK_SIZE];
+#endif
+static OS_TCB TaskReadCAN_TCB;
+static CPU_STK TaskReadCAN_Stk[DEFAULT_STACK_SIZE];
 
-#define STACK_SIZE 128
+static CANInfo msgReadCount = {0};
 
-void Task1(void *p_arg){
-    CPU_Init();
-    // OS_CPU_SysTickInit();
-    OS_CPU_SysTickInit(SystemCoreClock / (CPU_INT32U) OSCfg_TickRate_Hz);
+// Reads CarCAN and saves message statistics in the msgReadCount struct
+void Task_ReadCAN(void *arg)
+{
+    OS_ERR err;
+	CANDATA_t dataBuf = {0};
+
+
+    while (1) {
+        ErrorStatus status = CANbus_Read(&dataBuf, true, CARCAN);
+
+		if (status == SUCCESS){
+            switch(dataBuf.ID) {
+                case IO_STATE: {
+                    msgReadCount.msgArray[IO_STATE_IDX].numReceived++;
+                    msgReadCount.msgArray[IO_STATE_IDX].lastMsg = dataBuf;
+                    break;
+                }
+
+                case CONTROL_MODE: {
+                    msgReadCount.msgArray[CONTROL_MODE_IDX].numReceived++;
+                    memcpy((void *)&msgReadCount.msgArray[CONTROL_MODE_IDX].lastMsg, &dataBuf, sizeof dataBuf);
+                    break;
+                }
+
+                default: {
+                    uint8_t msgIdx = dataBuf.ID - MOTOR_MSG_BASE_ADDRESS;
+
+                    if (msgIdx >= 0 && msgIdx < NUM_MOTOR_MSGS) {  // Check if the message is from the motor controller
+                        msgReadCount.msgArray[msgIdx + NUM_NONMOTOR_MSGS].numReceived++; // If so, increment based on the ID
+                        memcpy((void *)&msgReadCount.msgArray[msgIdx + NUM_NONMOTOR_MSGS].lastMsg, &dataBuf, sizeof dataBuf);
+
+                    } else { // Place messages not otherwise accounted for into the last index of the array
+                        msgReadCount.msgArray[NUM_MOTOR_MSGS + NUM_NONMOTOR_MSGS].numReceived++;
+                        memcpy((void *)(&msgReadCount.msgArray[NUM_MOTOR_MSGS + NUM_NONMOTOR_MSGS].lastMsg), &dataBuf, sizeof dataBuf);
+                        
+                    }
+                     
+                }
+
+            }
+            
+            // Other CAN stats
+            msgReadCount.lastReceiveTime_ms = (OSTimeGet(&err) / OS_CFG_TICK_RATE_HZ);
+            msgReadCount.lastMsgReceived = dataBuf.ID;
+            msgReadCount.SpaceLeftInQ = get_SendCarCAN_Q_Space();
+
+        } else {
+            // CANbus read is unsuccessful
+            printf("\n\rCANbus_Read error of %x", status);
+        }  
+    }
+
+
+}
+
+#ifdef TEST_SOFTWARE
+// Create a fake motor controller to send messages to the controls system via MotorCAN
+void Task_MC(void *arg) {
     OS_ERR err;
-    // CPU_TS ts;
-    CAN_Queue_Init();
-    CANbus_Init(CARCAN);
+    uint8_t loopCount = 0;
+    CANDATA_t motorMsg = {
+        .ID=VELOCITY,
+        .idx=0,
+        .data={0xD, 0xE, 0xA, 0xD, 0x0, 0x0, 0xE, 0xF}, // Bytes 4-5 store error flags and must be empty
+    };
+    
+    while(1) {
+        if (++loopCount == 5) { // Only send these messages every 1000ms
+            loopCount = 0;
+            motorMsg.ID=TEMPERATURE;
+            CANbus_Send(motorMsg, CAN_NON_BLOCKING, MOTORCAN);
+            motorMsg.ID=ODOMETER_AMPHOURS;
+            CANbus_Send(motorMsg, CAN_NON_BLOCKING, MOTORCAN);
+        }
+        // Send these messages every 200ms
+        motorMsg.ID=MOTOR_STATUS;
+        CANbus_Send(motorMsg, CAN_NON_BLOCKING, MOTORCAN);
+        motorMsg.ID=MC_BUS;
+        CANbus_Send(motorMsg, CAN_NON_BLOCKING, MOTORCAN);
+        motorMsg.ID=MC_PHASE_CURRENT;
+        CANbus_Send(motorMsg, CAN_NON_BLOCKING, MOTORCAN);
+        motorMsg.ID=VOLTAGE_VEC;
+        CANbus_Send(motorMsg, CAN_NON_BLOCKING, MOTORCAN);
+        motorMsg.ID=CURRENT_VEC;
+        CANbus_Send(motorMsg, CAN_NON_BLOCKING, MOTORCAN);
+        motorMsg.ID=BACKEMF;
+        CANbus_Send(motorMsg, CAN_NON_BLOCKING, MOTORCAN);
+        OSTimeDlyHMSM(0, 0, 0, 200, OS_OPT_TIME_HMSM_STRICT, &err);
+    }
     
-    //spawn can send task
+}
+#endif
+
+
+// Initialize other tasks and print IO state
+void Task1(void *arg)
+{
+    OS_ERR err;
+
+    UpdateDisplay_Init();
+
+  // Initialize SendCarCAN
     OSTaskCreate(
         (OS_TCB*)&SendCarCAN_TCB,
-        (CPU_CHAR*)"SendCarCan",
+        (CPU_CHAR*)"SendCarCAN",
         (OS_TASK_PTR)Task_SendCarCAN,
         (void*)NULL,
-        (OS_PRIO)3,
+        (OS_PRIO)TASK_SEND_CAR_CAN_PRIO,
         (CPU_STK*)SendCarCAN_Stk,
-        (CPU_STK_SIZE)STACK_SIZE/10,
-        (CPU_STK_SIZE)STACK_SIZE,
+        (CPU_STK_SIZE)WATERMARK_STACK_LIMIT,
+        (CPU_STK_SIZE)TASK_SEND_CAR_CAN_STACK_SIZE,
         (OS_MSG_QTY)0,
-        (OS_TICK)NULL,
+        (OS_TICK)0,
+        (void*)NULL,
+        (OS_OPT)(OS_OPT_TASK_STK_CLR),
+        (OS_ERR*)&err
+    );
+    assertOSError(err);
+
+    // Initialize SendTritium
+    OSTaskCreate(
+        (OS_TCB*)&SendTritium_TCB,
+        (CPU_CHAR*)"SendTritium",
+        (OS_TASK_PTR)Task_SendTritium,
+        (void*)NULL,
+        (OS_PRIO)TASK_SEND_TRITIUM_PRIO,
+        (CPU_STK*)SendTritium_Stk,
+        (CPU_STK_SIZE)WATERMARK_STACK_LIMIT,
+        (CPU_STK_SIZE)TASK_SEND_TRITIUM_STACK_SIZE,
+        (OS_MSG_QTY)0,
+        (OS_TICK)0,
+        (void*)NULL,
+        (OS_OPT)(OS_OPT_TASK_STK_CLR),
+        (OS_ERR*)&err
+    );
+    assertOSError(err);
+
+    // Initialize ReadTritium
+    OSTaskCreate(
+        (OS_TCB*)&ReadTritium_TCB,
+        (CPU_CHAR*)"ReadTritium",
+        (OS_TASK_PTR)Task_ReadTritium,
+        (void*)NULL,
+        (OS_PRIO)TASK_READ_TRITIUM_PRIO,
+        (CPU_STK*)ReadTritium_Stk,
+        (CPU_STK_SIZE)WATERMARK_STACK_LIMIT,
+        (CPU_STK_SIZE)TASK_READ_TRITIUM_STACK_SIZE,
+        (OS_MSG_QTY)0,
+        (OS_TICK)0,
+        (void*)NULL,
+        (OS_OPT)(OS_OPT_TASK_STK_CLR),
+        (OS_ERR*)&err
+    );
+    assertOSError(err);
+
+    // Create task to read CANbus
+      OSTaskCreate(
+        (OS_TCB*)&TaskReadCAN_TCB,
+        (CPU_CHAR*)"Task_ReadCAN",
+        (OS_TASK_PTR)Task_ReadCAN,
+        (void*)NULL,
+        (OS_PRIO)13, // Lower prio than other running tasks
+        (CPU_STK*)TaskReadCAN_Stk,
+        (CPU_STK_SIZE)WATERMARK_STACK_LIMIT,
+        (CPU_STK_SIZE)DEFAULT_STACK_SIZE,
+        (OS_MSG_QTY)0,
+        (OS_TICK)0,
+        (void*)NULL,
+        (OS_OPT)(OS_OPT_TASK_STK_CLR),
+        (OS_ERR*)&err
+    );
+    assertOSError(err);
+
+    // Initialize UpdateDisplay
+    OSTaskCreate(
+        (OS_TCB*)&UpdateDisplay_TCB,
+        (CPU_CHAR*)"UpdateDisplay",
+        (OS_TASK_PTR)Task_UpdateDisplay,
+        (void*)NULL,
+        (OS_PRIO)TASK_UPDATE_DISPLAY_PRIO,
+        (CPU_STK*)UpdateDisplay_Stk,
+        (CPU_STK_SIZE)WATERMARK_STACK_LIMIT,
+        (CPU_STK_SIZE)TASK_UPDATE_DISPLAY_STACK_SIZE,
+        (OS_MSG_QTY)0,
+        (OS_TICK)0,
+        (void*)NULL,
+        (OS_OPT)(OS_OPT_TASK_STK_CLR),
+        (OS_ERR*)&err
+    );
+    assertOSError(err);
+
+    #ifdef TEST_SOFTWARE
+    // Create a task to simulate the motor controller
+    OSTaskCreate(
+        (OS_TCB*)&TaskMC_TCB,
+        (CPU_CHAR*)"Task_MC",
+        (OS_TASK_PTR)&Task_MC,
+        (void*)NULL,
+        (OS_PRIO)2, // The motor controller is "external" and shouldn't be delayed by other tasks
+        (CPU_STK*)TaskMC_Stk,
+        (CPU_STK_SIZE)WATERMARK_STACK_LIMIT,
+        (CPU_STK_SIZE)DEFAULT_STACK_SIZE,
+        (OS_MSG_QTY)0,
+        (OS_TICK)0,
         (void*)NULL,
-        (OS_OPT)(OS_OPT_TASK_STK_CLR|OS_OPT_TASK_STK_CHK),
+        (OS_OPT)(OS_OPT_TASK_STK_CLR),
         (OS_ERR*)&err
     );
+    assertOSError(err);
+    #endif
+
+   
+    while (1) {
+        // (SendTritium task will print Control Mode for us)
 
-    CANDATA_t msg;
-    msg.ID = MOTOR_STATUS;
-    msg.idx = 0;
-    msg.data[0] = 0x12;
-    msg.data[1] = 0x34;
-    msg.data[2] = 0x56;
-    msg.data[3] = 0x78;
-    msg.data[4] = 0x9A;
-    msg.data[5] = 0xBC;
-    msg.data[6] = 0xDE;
-    msg.data[7] = 0xF0;
-    while(1){
-        (msg.ID)++;
-        if(msg.ID > 0x24F){
-            msg.ID = MOTOR_STATUS;
+        // Print IO State
+        printf("\n\r---- IO State ----");
+        printf("\n\rAccelerator: %d", Pedals_Read(ACCELERATOR));
+        printf("\n\rBrake: %d", Pedals_Read(BRAKE));
+        uint8_t pins = 0;
+        for(pin_t pin = 0; pin < NUM_PINS; pin++){
+            bool pinState = Minions_Read(pin);
+            pins |= pinState << pin;
         }
-        CAN_Queue_Post(msg);
-        OSTimeDlyHMSM(0, 0, 0, 100, OS_OPT_TIME_HMSM_STRICT, &err);
-    }
+        printf("\n\rMinions: %x", pins);
+        uint8_t contactors = 0;
+        for(contactor_t contactor = 0; contactor < NUM_CONTACTORS; contactor++){
+            bool contactorState = (Contactors_Get(contactor) == ON) ? true : false;
+            contactors |= contactorState << contactor;
+        }
+        printf("\n\rContactors: %x", contactors);
+        OSTimeDlyHMSM(0, 0, 0, 10 * FSM_PERIOD, OS_OPT_TIME_HMSM_STRICT, &err);
+    }   
 
 }
-int main(void){ //startup OS stuff, spawn test task
-    
+
+
+int main(void) 
+{
     OS_ERR err;
     OSInit(&err);
-    if(err != OS_ERR_NONE){
-        printf("OS error code %d\n",err);
-    }
+
+    OS_CPU_SysTickInit(SystemCoreClock / (CPU_INT32U)OSCfg_TickRate_Hz);
+
+    CPU_Init();
+    BSP_UART_Init(UART_2);
+    Pedals_Init();
+    CANbus_Init(CARCAN, NULL, NUM_CARCAN_FILTERS);      // CarCAN filter list is normally (CANId_t*)carCANFilterList
+    CANbus_Init(MOTORCAN, NULL, NUM_MOTORCAN_FILTERS);  // but for testing, we'd like to receive all messages
+    SendCarCAN_Init();
+    Minions_Init();
+    Display_Init();
+    TaskSwHook_Init();
+
     OSTaskCreate(
-        (OS_TCB*)&Task1_TCB,
-        (CPU_CHAR*)"Task1",
-        (OS_TASK_PTR)Task1,
-        (void*)NULL,
-        (OS_PRIO)4,
-        (CPU_STK*)Task1_Stk,
-        (CPU_STK_SIZE)STACK_SIZE/10,
-        (CPU_STK_SIZE)STACK_SIZE,
-        (OS_MSG_QTY)0,
+        (OS_TCB *)&Task1_TCB,
+        (CPU_CHAR *)"Task 1",
+        (OS_TASK_PTR)Task1, 
+        (void *)NULL,
+        (OS_PRIO)13, 
+        (CPU_STK *)Task1Stk,
+        (CPU_STK_SIZE)DEFAULT_STACK_SIZE / 10,
+        (CPU_STK_SIZE)DEFAULT_STACK_SIZE,
+        (OS_MSG_QTY)0, 
         (OS_TICK)NULL,
-        (void*)NULL,
-        (OS_OPT)(OS_OPT_TASK_STK_CLR|OS_OPT_TASK_STK_CHK),
-        (OS_ERR*)&err
+        (void *)NULL, 
+        (OS_OPT)(OS_OPT_TASK_STK_CLR), 
+        (OS_ERR *)&err
     );
+    assertOSError(err);
 
-    if (err != OS_ERR_NONE) {
-        printf("Task1 error code %d\n", err);
-    }
     OSStart(&err);
-    if (err != OS_ERR_NONE) {
-        printf("OS error code %d\n", err);
-    }
-    return 0;
+    assertOSError(err);
 }
\ No newline at end of file
diff --git a/Tests/Test_App_Telemetry.c b/Tests/Test_App_Telemetry.c
deleted file mode 100644
index ee265923..00000000
--- a/Tests/Test_App_Telemetry.c
+++ /dev/null
@@ -1,95 +0,0 @@
-#include "os.h"
-#include "Tasks.h"
-#include <bsp.h>
-#include "config.h"
-#include "common.h"
-#include "Pedals.h"
-#include "Minions.h"
-#include "Contactors.h"
-#include "CANbus.h"
-#include "CAN_Queue.h"
-
-static OS_TCB Task1TCB;
-static CPU_STK Task1Stk[DEFAULT_STACK_SIZE];
-
-void Task1(void *arg)
-{   
-    CPU_Init();
-    
-    CANbus_Init(CARCAN);
-    CAN_Queue_Init();
-    // BSP_UART_Init(UART_2);
-    Pedals_Init();
-    Minions_Init();
-    Contactors_Init();
-
-    OS_CPU_SysTickInit(SystemCoreClock / (CPU_INT32U)OSCfg_TickRate_Hz);
-
-    OS_ERR err;
-
-    // Initialize Telemetry
-    OSTaskCreate(
-        (OS_TCB*)&Telemetry_TCB,
-        (CPU_CHAR*)"Telemetry",
-        (OS_TASK_PTR)Task_Telemetry,
-        (void*)NULL,
-        (OS_PRIO)TASK_TELEMETRY_PRIO,
-        (CPU_STK*)Telemetry_Stk,
-        (CPU_STK_SIZE)WATERMARK_STACK_LIMIT,
-        (CPU_STK_SIZE)TASK_TELEMETRY_STACK_SIZE,
-        (OS_MSG_QTY)0,
-        (OS_TICK)0,
-        (void*)NULL,
-        (OS_OPT)(OS_OPT_TASK_STK_CLR),
-        (OS_ERR*)&err
-    );
-    assertOSError(err);
-
-    bool lightState = false;
-    State contactorState = OFF;
-
-    CANDATA_t msg;
-    
-    while (1){
-        // Switches can be modified through hardware
-
-        // Check light
-        lightState = !lightState;
-        Minions_Write(BRAKELIGHT, lightState);
-
-        // Check contactors (HAVE NOTHING HOOKED UP TO CONTACTORS)
-        contactorState = contactorState == OFF ? ON : OFF;
-        Contactors_Set(MOTOR_CONTROLLER_BYPASS_PRECHARGE_CONTACTOR, contactorState, true);
-        Contactors_Set(ARRAY_PRECHARGE_BYPASS_CONTACTOR, contactorState, true);
-
-        CANbus_Read(&msg, CAN_BLOCKING, CARCAN);
-
-        OSTimeDlyHMSM(0, 0, 0, 500, OS_OPT_TIME_HMSM_STRICT, &err);
-        // Use a logic analyzer to read the CAN line and see if the data shows up correctly    
-    }
-}
-
-int main()
-{
-    OS_ERR err;
-    OSInit(&err);
-
-    // create tester thread
-    OSTaskCreate(
-        (OS_TCB *)&Task1TCB,
-        (CPU_CHAR *)"Task 1",
-        (OS_TASK_PTR)Task1,
-        (void *)NULL,
-        (OS_PRIO)5,
-        (CPU_STK *)Task1Stk,
-        (CPU_STK_SIZE)DEFAULT_STACK_SIZE / 10,
-        (CPU_STK_SIZE)DEFAULT_STACK_SIZE,
-        (OS_MSG_QTY)0,
-        (OS_TICK)NULL,
-        (void *)NULL,
-        (OS_OPT)(OS_OPT_TASK_STK_CLR),
-        (OS_ERR *)&err);
-    assertOSError(err);
-
-    OSStart(&err);
-}
\ No newline at end of file
diff --git a/Tests/Test_Renode_CANBPSFault.c b/Tests/Test_Renode_CANBPSFault.c
index 6597ce12..2ae360b1 100644
--- a/Tests/Test_Renode_CANBPSFault.c
+++ b/Tests/Test_Renode_CANBPSFault.c
@@ -8,7 +8,6 @@
 #include "CANbus.h"
 #include "Contactors.h"
 #include "ReadCarCAN.h"
-#include "CAN_Queue.h"
 #include "Minions.h"
 #include "BSP_UART.h"
 #include "Display.h"