diff --git a/.gitignore b/.gitignore
index 7264aa76b..ee3089b2b 100644
--- a/.gitignore
+++ b/.gitignore
@@ -26,3 +26,6 @@ __pycache__
 *can_board_ids.h
 *system_can.dbc
 x86_flash
+
+py/can/can.py
+py/can/messages.py
diff --git a/libraries/codegen/boards/motor_controller.yaml b/libraries/codegen/boards/motor_controller.yaml
index 433b488fd..92d900750 100644
--- a/libraries/codegen/boards/motor_controller.yaml
+++ b/libraries/codegen/boards/motor_controller.yaml
@@ -36,17 +36,6 @@
         velocity_r:
           length: 16
 
-    motor_status:
-      id: 37
-      target:
-        centre_console:
-          watchdog: 0
-      signals:
-        motor_status_l:
-          length: 32
-        motor_status_r:
-          length: 32
-
     motor_sink_temps:
       id: 38
       target:
@@ -69,9 +58,9 @@
           watchdog: 0
       signals:
         dsp_temp_l:
-          length: 32
+          length: 16
         dsp_temp_r:
-          length: 32
+          length: 16
 
     mc_status:
       id: 40
@@ -95,12 +84,3 @@
           length: 8
         precharge_status:
           length: 8
-
-    precharge_completed:
-      id: 16
-      target:
-        centre_console:
-          watchdog: 0
-      signals:
-        notification:
-          length: 8
diff --git a/libraries/codegen/boards/steering.yaml b/libraries/codegen/boards/steering.yaml
index 7b92e9520..495e66ba7 100644
--- a/libraries/codegen/boards/steering.yaml
+++ b/libraries/codegen/boards/steering.yaml
@@ -1,5 +1,5 @@
 # Message Definitions in plaintext are on the wiki at:
-# https://uwmidsun.atlassian.net/l/cp/Pxn8Xhm8
+# https://uwmidsun.atlassian.net/wiki/spaces/ELEC/pages/50003973/CAN+Message+Definitions
 #
 # If you are making changes to this file please update the corresponding entry
 # on the wiki. If you need to add a new message use a reasonable
@@ -24,3 +24,7 @@ Messages:
         length: 8 # (cc_toggle | cc_increade | cc_decrease)
       input_lights:
         length: 8 
+      digital_input:
+        length: 8
+      analog_input:
+        length: 8
diff --git a/libraries/master/src/master_task.c b/libraries/master/src/master_task.c
index 51bf563f9..52b7265e3 100644
--- a/libraries/master/src/master_task.c
+++ b/libraries/master/src/master_task.c
@@ -1,6 +1,6 @@
 #include "master_task.h"
 
-static uint32_t MASTER_MS_CYCLE_TIME = 100;
+static uint32_t MASTER_MS_CYCLE_TIME = 50;
 
 #define MASTER_TASK_PRIORITY (configMAX_PRIORITIES - 2)
 #define MAX_CYCLES_OVER 5
@@ -26,10 +26,12 @@ uint8_t get_cycles_over() {
 }
 #endif
 
-void run_fast_cycle();
-void run_medium_cycle();
-void run_slow_cycle();
-void pre_loop_init();
+void master_no_op() {}
+
+void run_fast_cycle() __attribute__((weak, alias("master_no_op")));
+void run_medium_cycle() __attribute__((weak, alias("master_no_op")));
+void run_slow_cycle() __attribute__((weak, alias("master_no_op")));
+void pre_loop_init() __attribute__((weak, alias("master_no_op")));
 
 void check_late_cycle(BaseType_t delay) {
   if (delay != pdTRUE) {
diff --git a/libraries/ms-common/inc/spi.h b/libraries/ms-common/inc/spi.h
index c412f81b5..82387bef8 100644
--- a/libraries/ms-common/inc/spi.h
+++ b/libraries/ms-common/inc/spi.h
@@ -43,10 +43,10 @@ StatusCode spi_init(SpiPort spi, const SpiSettings *settings);
 StatusCode spi_exchange(SpiPort spi, uint8_t *tx_data, size_t tx_len, uint8_t *rx_data,
                         size_t rx_len);
 
-#ifdef x86
+#ifdef MS_PLATFORM_X86
 // Reads data from the tx buffer into data
-StatusCode spi_get_tx(uint8_t *data, uint8_t len);
+StatusCode spi_get_tx(SpiPort spi, uint8_t *data, uint8_t len);
 
 // Writes from data into the rx buffer
-StatusCode spi_set_rx(uint8_t *data, uint8_t len);
+StatusCode spi_set_rx(SpiPort spi, uint8_t *data, uint8_t len);
 #endif
diff --git a/libraries/ms-common/src/callback_handler.c b/libraries/ms-common/src/callback_handler.c
index 938770ea6..1272bb695 100644
--- a/libraries/ms-common/src/callback_handler.c
+++ b/libraries/ms-common/src/callback_handler.c
@@ -28,7 +28,6 @@ Event prv_find_next_event() {
   }
   // Gets index of least significant 0 bit (first available event)
   Event event = __builtin_ctz(~s_registered_callbacks);
-
   return event;
 }
 
@@ -96,13 +95,13 @@ Event register_callback(CallbackFn cb, void *context) {
 TASK(callback_task, TASK_STACK_512) {
   uint32_t notification = 0;
   Event event = 0;
-  StatusCode notification_status;
   while (true) {
     notify_wait(&notification, BLOCK_INDEFINITELY);
 
-    do {
-      notification_status = event_from_notification(&notification, &event);
+    event_from_notification(&notification, &event);
+    while (event != INVALID_EVENT) {
       prv_trigger_callback(event);
-    } while (notification_status != STATUS_CODE_OK);
+      event_from_notification(&notification, &event);
+    }
   }
 }
diff --git a/libraries/ms-common/src/x86/gpio.c b/libraries/ms-common/src/x86/gpio.c
index 36ec7814e..d4e46abe4 100644
--- a/libraries/ms-common/src/x86/gpio.c
+++ b/libraries/ms-common/src/x86/gpio.c
@@ -50,7 +50,7 @@ StatusCode gpio_set_state(const GpioAddress *address, GpioState state) {
   }
   GpioMode mode = s_gpio_pin_modes[prv_get_index(address)];
   if (mode != GPIO_OUTPUT_OPEN_DRAIN && mode != GPIO_OUTPUT_PUSH_PULL) {
-    LOG_WARN("Attempting to set an output pin, check your configuration");
+    LOG_WARN("Attempting to set an input pin, check your configuration");
     return status_code(STATUS_CODE_INVALID_ARGS);
   }
 
diff --git a/libraries/ms-common/src/x86/spi.c b/libraries/ms-common/src/x86/spi.c
index 3ecb19e09..f7e6bc923 100644
--- a/libraries/ms-common/src/x86/spi.c
+++ b/libraries/ms-common/src/x86/spi.c
@@ -6,7 +6,6 @@
 #include "spi_mcu.h"
 
 #define SPI_BUF_SIZE 32
-#define SPI_QUEUE_DELAY_MS 0
 
 typedef struct {
   uint8_t tx_buf[SPI_BUF_SIZE];
@@ -89,31 +88,27 @@ StatusCode spi_exchange(SpiPort spi, uint8_t *tx_data, size_t tx_len, uint8_t *r
 }
 
 StatusCode spi_get_tx(SpiPort spi, uint8_t *data, uint8_t len) {
-  status_ok_or_return(mutex_lock(&s_port[spi].spi_buf.mutex, SPI_TIMEOUT_MS));
-
+  uint8_t dummy = 0;
   for (size_t tx = 0; tx < len; tx++) {
-    if (queue_receive(&s_port[spi].spi_buf.tx_queue, &data[tx], SPI_QUEUE_DELAY_MS)) {
+    if (queue_receive(&s_port[spi].spi_buf.tx_queue, &data[tx], SPI_TIMEOUT_MS)) {
       queue_reset(&s_port[spi].spi_buf.tx_queue);
-      mutex_unlock(&s_port[spi].spi_buf.mutex);
       return STATUS_CODE_EMPTY;
     }
+    queue_send(&s_port[spi].spi_buf.rx_queue, &dummy, SPI_TIMEOUT_MS);
   }
 
-  mutex_unlock(&s_port[spi].spi_buf.mutex);
   return STATUS_CODE_OK;
 }
 
 StatusCode spi_set_rx(SpiPort spi, uint8_t *data, uint8_t len) {
-  status_ok_or_return(mutex_lock(&s_port[spi].spi_buf.mutex, SPI_TIMEOUT_MS));
-
+  uint8_t dummy = 0;
   for (size_t rx = 0; rx < len; rx++) {
-    if (queue_send(&s_port[spi].spi_buf.rx_queue, &data[rx], SPI_QUEUE_DELAY_MS)) {
+    if (queue_receive(&s_port[spi].spi_buf.tx_queue, &dummy, SPI_TIMEOUT_MS)) {
       queue_reset(&s_port[spi].spi_buf.rx_queue);
-      mutex_unlock(&s_port[spi].spi_buf.mutex);
       return STATUS_CODE_RESOURCE_EXHAUSTED;
     }
+    queue_send(&s_port[spi].spi_buf.rx_queue, &data[rx], SPI_TIMEOUT_MS);
   }
 
-  mutex_unlock(&s_port[spi].spi_buf.mutex);
   return STATUS_CODE_OK;
 }
diff --git a/projects/motor_controller/config.json b/projects/motor_controller/config.json
index efab08f10..975be2f31 100644
--- a/projects/motor_controller/config.json
+++ b/projects/motor_controller/config.json
@@ -4,5 +4,9 @@
         "ms-common",
         "master"
     ],
-    "can": true
-}
\ No newline at end of file
+    "can": true,
+    "mocks": {
+        "motor_can_tx": ["mcp2515_hw_transmit", "mcp2515_hw_init"],
+        "motor_can_rx": ["mcp2515_hw_transmit", "mcp2515_hw_init"]
+    }
+}
diff --git a/projects/motor_controller/inc/mcp2515_defs.h b/projects/motor_controller/inc/mcp2515_defs.h
index 4655853e2..5ff8b84ab 100644
--- a/projects/motor_controller/inc/mcp2515_defs.h
+++ b/projects/motor_controller/inc/mcp2515_defs.h
@@ -130,8 +130,7 @@
 #define MCP2515_TXBNDLC_DLC_MASK 0x0F
 
 // RXB0CTRL: Register 4-1
-#define MCP2515_RXB0CTRL_BUKT_MASK 0x4
-#define MCP2515_RXB0CTRL_BUKT_ROLLOVER 0x4
+#define MCP2515_RXB0CTRL_BUKT 0x4
 
 #define MCP2515_MAX_WRITE_BUFFER_LEN 10
 #define MCP2515_STANDARD_ID_LEN 11
@@ -143,3 +142,11 @@
 
 #define MCP2515_NUM_MASK_REGISTERS_STANDARD 2
 #define MCP2515_NUM_MASK_REGISTERS_EXTENDED 4
+
+// BFPCTRL: Register 4-3
+#define MCP2515_BFPCTRL_B1BFS 0x20
+#define MCP2515_BFPCTRL_B2BFS 0x10
+#define MCP2515_BFPCTRL_B1BFE 0x08
+#define MCP2515_BFPCTRL_B2BFE 0x04
+#define MCP2515_BFPCTRL_B1BFM 0x02
+#define MCP2515_BFPCTRL_B2BFM 0x01
diff --git a/projects/motor_controller/inc/mcp2515_hw.h b/projects/motor_controller/inc/mcp2515_hw.h
index 5a1fea62e..7659d6e6f 100644
--- a/projects/motor_controller/inc/mcp2515_hw.h
+++ b/projects/motor_controller/inc/mcp2515_hw.h
@@ -15,6 +15,8 @@ typedef struct Mcp2515Settings {
   SpiSettings spi_settings;
 
   GpioAddress interrupt_pin;
+  GpioAddress RX0BF;
+  GpioAddress RX1BF;
 
   // same can settings except MCP2515 does not support 1000kbps bitrate
   CanSettings can_settings;
@@ -28,7 +30,7 @@ typedef struct Mcp2515Storage {
 } Mcp2515Storage;
 
 // Initializes CAN using the specified settings.
-StatusCode mcp2515_hw_init(const CanQueue *rx_queue, const Mcp2515Settings *settings);
+StatusCode mcp2515_hw_init(Mcp2515Storage *rx_queue, const Mcp2515Settings *settings);
 
 // StatusCode mcp2515_hw_add_filter_in(uint32_t mask, uint32_t filter, bool extended);
 
@@ -36,4 +38,4 @@ StatusCode mcp2515_hw_set_filter(CanMessageId *filters, bool loopback);
 
 CanHwBusStatus mcp2515_hw_bus_status(void);
 
-StatusCode mcp2515_hw_transmit(uint32_t id, bool extended, const uint64_t data, size_t len);
+StatusCode mcp2515_hw_transmit(uint32_t id, bool extended, uint8_t *data, size_t len);
diff --git a/projects/motor_controller/inc/motor_can.h b/projects/motor_controller/inc/motor_can.h
index 4f3a20030..d7e6f1293 100644
--- a/projects/motor_controller/inc/motor_can.h
+++ b/projects/motor_controller/inc/motor_can.h
@@ -7,13 +7,16 @@
 #define VELOCITY_SCALE 100
 #define TEMP_SCALE 100
 
+// also used as the current for regen braking, might need to be seperated
 #define ACCERLATION_FORCE 1
 #define CRUISE_THROTTLE_THRESHOLD 0
 #define TORQUE_CONTROL_VEL 20000  // unobtainable rpm for current control
-#define VEL_TO_RPM_RATIO 1.0      // TODO: set actual ratio, m/s to motor rpm
+#define VEL_TO_RPM_RATIO 0.57147  // TODO: set actual ratio, m/s to motor (rpm for m/s)
+// wheel diameter 557mm
+// 1000 / (557 * pi) = 0.57147
 
-#define DRIVER_CONTROL_BASE 0x1
-#define MOTOR_CONTROLLER_BASE_L 0x40
-#define MOTOR_CONTROLLER_BASE_R 0x80
+#define DRIVER_CONTROL_BASE 0x500
+#define MOTOR_CONTROLLER_BASE_L 0x400
+#define MOTOR_CONTROLLER_BASE_R 0x80  // TODO: set to actual values
 
 void init_motor_controller_can();
diff --git a/projects/motor_controller/inc/precharge.h b/projects/motor_controller/inc/precharge.h
new file mode 100644
index 000000000..01fd5f1c5
--- /dev/null
+++ b/projects/motor_controller/inc/precharge.h
@@ -0,0 +1,17 @@
+#pragma once
+
+#include <stdbool.h>
+
+#include "gpio.h"
+#include "notify.h"
+#include "status.h"
+
+// Requires GPIO to be initialized
+// Requires GPIO interrupts to be initialized
+
+typedef struct PrechargeSettings {
+  GpioAddress precharge_control;
+  GpioAddress precharge_monitor;
+} PrechargeSettings;
+
+StatusCode precharge_init(const PrechargeSettings *settings, Event event, const Task *task);
diff --git a/projects/motor_controller/inc/precharge_control.h b/projects/motor_controller/inc/precharge_control.h
deleted file mode 100644
index b7d788276..000000000
--- a/projects/motor_controller/inc/precharge_control.h
+++ /dev/null
@@ -1,27 +0,0 @@
-#pragma once
-
-#include <stdbool.h>
-
-#include "gpio.h"
-#include "status.h"
-
-// Requires GPIO to be initialized
-// Requires GPIO interrupts to be initialized
-
-typedef enum {
-  MCI_PRECHARGE_DISCHARGED = 0,
-  MCI_PRECHARGE_INCONSISTENT,
-  MCI_PRECHARGE_CHARGED
-} PrechargeState;
-
-typedef struct PrechargeControlSettings {
-  GpioAddress precharge_control;
-  GpioAddress precharge_monitor;
-  GpioAddress precharge_monitor2;
-} PrechargeControlSettings;
-
-StatusCode prv_set_precharge_control(GpioAddress *address, const GpioState state);
-
-StatusCode run_precharge_rx_cycle();
-
-StatusCode precharge_control_init(const PrechargeControlSettings *settings);
diff --git a/projects/motor_controller/src/main.c b/projects/motor_controller/src/main.c
index 7586dcc7d..f466f8385 100644
--- a/projects/motor_controller/src/main.c
+++ b/projects/motor_controller/src/main.c
@@ -2,53 +2,71 @@
 
 #include "can.h"
 #include "can_board_ids.h"
+#include "can_codegen.h"
 #include "delay.h"
 #include "fsm.h"
+#include "gpio_it.h"
+#include "interrupt.h"
 #include "log.h"
 #include "master_task.h"
 #include "mcp2515.h"
 #include "misc.h"
 #include "motor_can.h"
-#include "precharge_control.h"
+#include "motor_controller_setters.h"
+#include "precharge.h"
 #include "soft_timer.h"
 #include "tasks.h"
 
+#define PRECHARGE_EVENT 0
+
 static CanStorage s_can_storage = { 0 };
 const CanSettings can_settings = {
   .device_id = SYSTEM_CAN_DEVICE_MOTOR_CONTROLLER,
   .bitrate = CAN_HW_BITRATE_500KBPS,
   .tx = { GPIO_PORT_A, 12 },
   .rx = { GPIO_PORT_A, 11 },
-  .loopback = true,
+  .loopback = false,
 };
 static Mcp2515Storage s_mcp2515_storage = { 0 };
-Mcp2515Settings mcp2515_settings = {  // place holder values
-  .spi_port = SPI_PORT_1,
+static Mcp2515Settings s_mcp2515_settings = {
+  .spi_port = SPI_PORT_2,
   .spi_settings = {
-    0
+    .baudrate = 10000000,  // 10Mhz
+    .mode = SPI_MODE_0,
+    .mosi = { GPIO_PORT_B, 15 },
+    .miso = { GPIO_PORT_B, 14 },
+    .sclk = { GPIO_PORT_B, 13 },
+    .cs = { GPIO_PORT_B, 12 },
   },
-  .interrupt_pin = { GPIO_PORT_A, 0 },
+  .interrupt_pin = { GPIO_PORT_A, 8 },
+  .RX0BF = { GPIO_PORT_B, 10 },
+  .RX1BF = { GPIO_PORT_B, 11 },
   .can_settings = {
     .bitrate = CAN_HW_BITRATE_500KBPS,
-    .loopback = true,
+    .loopback = false,
   },
 };
-PrechargeControlSettings precharge_settings = {
-  // place holder values
-  .precharge_control = { GPIO_PORT_A, 10 },
-  .precharge_monitor = { GPIO_PORT_A, 9 },
-  .precharge_monitor2 = { GPIO_PORT_A, 8 },
+static PrechargeSettings s_precharge_settings = {
+  .precharge_control = { GPIO_PORT_A, 9 },
+  .precharge_monitor = { GPIO_PORT_B, 0 },
 };
 
 void pre_loop_init() {}
 
 void run_fast_cycle() {
+  uint32_t notification;
+  notify_get(&notification);
+  if (notification & (1 << PRECHARGE_EVENT)) {
+    LOG_DEBUG("Precharge complete\n");
+    set_mc_status_precharge_status(true);
+  }
+
   run_can_rx_cycle();
   run_mcp2515_rx_cycle();
   wait_tasks(2);
 
-  run_can_tx_cycle();
   run_mcp2515_tx_cycle();
+  run_can_tx_cycle();
   wait_tasks(2);
 }
 
@@ -59,10 +77,15 @@ void run_slow_cycle() {}
 int main() {
   tasks_init();
   log_init();
+  gpio_init();
+  interrupt_init();
+  gpio_it_init();
+
   can_init(&s_can_storage, &can_settings);
-  mcp2515_init(&s_mcp2515_storage, &mcp2515_settings);
-  precharge_control_init(&precharge_settings);
+  mcp2515_init(&s_mcp2515_storage, &s_mcp2515_settings);
   init_motor_controller_can();
+  precharge_init(&s_precharge_settings, PRECHARGE_EVENT, get_master_task());
+
   LOG_DEBUG("Motor Controller Task\n");
 
   init_master_task();
diff --git a/projects/motor_controller/src/mcp2515.c b/projects/motor_controller/src/mcp2515.c
index 3454282ea..921407ccb 100644
--- a/projects/motor_controller/src/mcp2515.c
+++ b/projects/motor_controller/src/mcp2515.c
@@ -10,11 +10,11 @@
 // Storage
 static Mcp2515Storage *s_storage;
 
-TASK(MCP2515_RX, TASK_MIN_STACK_SIZE) {
+TASK(MCP2515_RX, TASK_STACK_256) {
   int counter = 0;
   while (true) {
     notify_wait(NULL, BLOCK_INDEFINITELY);
-    LOG_DEBUG("mcp2515_rx called: %d!\n", counter);
+    // LOG_DEBUG("mcp2515_rx called: %d!\n", counter);
     counter++;
 
     mcp2515_rx_all();
@@ -36,11 +36,11 @@ StatusCode mcp2515_receive(const CanMessage *msg) {
   return ret;
 }
 
-TASK(MCP2515_TX, TASK_MIN_STACK_SIZE) {
+TASK(MCP2515_TX, TASK_STACK_256) {
   int counter = 0;
   while (true) {
     notify_wait(NULL, BLOCK_INDEFINITELY);
-    LOG_DEBUG("mcp2515_tx called: %d!\n", counter);
+    // LOG_DEBUG("mcp2515_tx called: %d!\n", counter);
     counter++;
 
     mcp2515_tx_all();
@@ -74,10 +74,12 @@ StatusCode mcp2515_transmit(const CanMessage *msg) {
     return status_msg(STATUS_CODE_INVALID_ARGS, "CAN: Invalid message ID");
   }
 
-  return mcp2515_hw_transmit(msg->id.raw, msg->extended, msg->data, msg->dlc);
+  return mcp2515_hw_transmit(msg->id.raw, msg->extended, msg->data_u8, msg->dlc);
 }
 
-static void no_op() {}
+static void no_op() {
+  LOG_DEBUG("error - no-op\n");
+}
 
 StatusCode mcp2515_init(Mcp2515Storage *storage, const Mcp2515Settings *settings) {
   memset(storage, 0, sizeof(*storage));
@@ -90,15 +92,15 @@ StatusCode mcp2515_init(Mcp2515Storage *storage, const Mcp2515Settings *settings
     mcp2515_tx_all = no_op;
   }
 
-  mcp2515_hw_init(&storage->rx_queue, settings);
+  mcp2515_hw_init(storage, settings);
 
   status_ok_or_return(can_queue_init(&s_storage->rx_queue));
 
   if (settings->can_settings.mode == CAN_CONTINUOUS) {
     // Create RX and TX Tasks
     // ! Ensure the task priority is lower than the interrupt tasks in mcp2515_hw.c
-    status_ok_or_return(tasks_init_task(MCP2515_RX, TASK_PRIORITY(2), NULL));
     status_ok_or_return(tasks_init_task(MCP2515_TX, TASK_PRIORITY(2), NULL));
+    status_ok_or_return(tasks_init_task(MCP2515_RX, TASK_PRIORITY(2), NULL));
   }
 
   return STATUS_CODE_OK;
diff --git a/projects/motor_controller/src/mcp2515_hw.c b/projects/motor_controller/src/mcp2515_hw.c
index b59f51f22..91bb1c4f8 100644
--- a/projects/motor_controller/src/mcp2515_hw.c
+++ b/projects/motor_controller/src/mcp2515_hw.c
@@ -6,48 +6,17 @@
 #include "log.h"
 #include "mcp2515_defs.h"
 
-typedef struct Mcp2515LoadTxPayload {
-  uint8_t cmd;
-  uint64_t data;
-} __attribute__((packed)) Mcp2515LoadTxPayload;
-
 // TX/RX buffer ID registers - See Registers 3-3 to 3-7, 4-4 to 4-8
-typedef struct Mcp2515IdRegs {
-  uint8_t sidh;
-  union {
-    struct {
-      uint8_t eid_16_17 : 2;
-      uint8_t unimplemented : 1;
-      uint8_t ide : 1;
-      uint8_t srr : 1;
-      uint8_t sid_0_2 : 3;
-    };
-    uint8_t raw;
-  } sidl;
-  uint8_t eid8;
-  uint8_t eid0;
-  union {
-    struct {
-      uint8_t dlc : 4;
-      uint8_t reserved : 2;
-      uint8_t rtr : 1;
-      uint8_t unimplemented : 1;
-    };
-    uint8_t raw;
-  } dlc;
-} Mcp2515IdRegs;
-
-typedef union Mcp2515Id {
+typedef union {
   struct {
-    uint32_t eid0 : 8;
-    uint32_t eid8 : 8;
-    uint32_t eid_16_17 : 2;
-    uint32_t sid_0_2 : 3;
-    uint32_t sidh : 8;
-    uint32_t padding : 3;
+    uint32_t eid : 18;
+    uint8_t _ : 1;
+    uint8_t extended : 1;
+    uint8_t srr : 1;
+    uint32_t sid : 11;
   };
-  uint32_t raw;
-} Mcp2515Id;
+  uint8_t registers[4];
+} Mcp2515IdRegs;
 
 typedef enum {
   MCP2515_FILTER_ID_RXF0 = 0,
@@ -91,7 +60,7 @@ static const uint8_t s_brp_lookup[NUM_CAN_HW_BITRATES] = {
 // SPI commands - See Table 12-1
 static void prv_reset() {
   uint8_t payload[] = { MCP2515_CMD_RESET };
-  spi_exchange(s_storage->spi_port, payload, sizeof(payload), NULL, 0);
+  spi_tx(s_storage->spi_port, payload, sizeof(payload));
 }
 
 static void prv_read(uint8_t addr, uint8_t *read_data, size_t read_len) {
@@ -104,82 +73,74 @@ static void prv_write(uint8_t addr, uint8_t *write_data, size_t write_len) {
   payload[0] = MCP2515_CMD_WRITE;
   payload[1] = addr;
   memcpy(&payload[2], write_data, write_len);
-  spi_exchange(s_storage->spi_port, payload, sizeof(payload), write_data, write_len);
+
+  spi_tx(s_storage->spi_port, payload, write_len + 2);
 }
 
 // See 12.10: *addr = (data & mask) | (*addr & ~mask)
 static void prv_bit_modify(uint8_t addr, uint8_t mask, uint8_t data) {
   uint8_t payload[] = { MCP2515_CMD_BIT_MODIFY, addr, mask, data };
-  spi_exchange(s_storage->spi_port, payload, sizeof(payload), NULL, 0);
+
+  spi_tx(s_storage->spi_port, payload, sizeof(payload));
 }
 
 static uint8_t prv_read_status() {
   uint8_t payload[] = { MCP2515_CMD_READ_STATUS };
-  uint8_t read_data[1] = { 0 };
-  spi_exchange(s_storage->spi_port, payload, sizeof(payload), read_data, sizeof(read_data));
+  uint8_t read_data;
+  spi_exchange(s_storage->spi_port, payload, sizeof(payload), &read_data, sizeof(read_data));
 
-  return read_data[0];
+  return read_data;
 }
 
-static void prv_handle_rx(uint8_t int_flags) {
+static void prv_handle_rx(uint8_t buffer_id) {
   CanMessage rx_msg = { 0 };
-  for (size_t i = 0; i < SIZEOF_ARRAY(s_rx_buffers); i++) {
-    Mcp2515RxBuffer *rx_buf = &s_rx_buffers[i];
-    if (int_flags & rx_buf->int_flag) {
-      // Read ID
-      uint8_t id_payload[] = { MCP2515_CMD_READ_RX | rx_buf->id };
-      Mcp2515IdRegs read_id_regs = { 0 };
-      spi_exchange(s_storage->spi_port, id_payload, sizeof(id_payload), (uint8_t *)&read_id_regs,
-                   sizeof(read_id_regs));
-
-      Mcp2515Id id = {
-        .sid_0_2 = read_id_regs.sidl.sid_0_2,
-        .sidh = read_id_regs.sidh,
-        .eid0 = read_id_regs.eid0,
-        .eid8 = read_id_regs.eid8,
-        .eid_16_17 = read_id_regs.sidl.eid_16_17,
-      };
-
-      rx_msg.extended = read_id_regs.sidl.ide;
-      rx_msg.dlc = read_id_regs.dlc.dlc;
-
-      if (!rx_msg.extended) {
-        // Standard IDs have garbage in the extended fields
-        id.raw >>= MCP2515_EXTENDED_ID_LEN;
-      }
-
-      rx_msg.id.raw = id.raw;
-
-      uint8_t data_payload[] = { MCP2515_CMD_READ_RX | rx_buf->data };
-      uint64_t read_data = 0;
-      spi_exchange(s_storage->spi_port, data_payload, sizeof(data_payload), (uint8_t *)&rx_msg.data,
-                   sizeof(rx_msg.data));
-      // Clear the interrupt flag so a new message can be loaded
-      prv_bit_modify(MCP2515_CTRL_REG_CANINTF, rx_buf->int_flag, 0x0);
-
-      can_queue_push(&s_storage->rx_queue, &rx_msg);
-    }
+  // Read ID and Data
+  uint8_t command = MCP2515_CMD_READ_RX | s_rx_buffers[buffer_id].id;
+  uint8_t data[5 + 8];  // id + data
+  spi_exchange(s_storage->spi_port, &command, 1, data, sizeof(data));
+
+  Mcp2515IdRegs read_id_regs = {
+    .registers = { data[3], data[2], data[1], data[0] },
+  };
+
+  rx_msg.extended = read_id_regs.extended;
+  rx_msg.dlc = data[4] & 0xf;
+
+  if (!rx_msg.extended) {
+    rx_msg.id.raw = read_id_regs.sid;
+  } else {
+    rx_msg.id.raw = (uint32_t)(read_id_regs.sid << MCP2515_EXTENDED_ID_LEN) | read_id_regs.eid;
   }
+  memcpy(rx_msg.data_u8, &data[5], rx_msg.dlc);
+
+  can_queue_push(&s_storage->rx_queue, &rx_msg);
 }
 
-static void prv_handle_error(uint8_t int_flags, uint8_t err_flags) {
+static void prv_handle_error() {
+  struct {
+    uint8_t canintf;
+    uint8_t eflg;
+  } regs;
+
+  prv_read(MCP2515_CTRL_REG_CANINTF, &regs.canintf, sizeof(regs));
+  // LOG_DEBUG("handle error %x %x\n", regs.canintf, regs.eflg);
   // Clear flags
-  if (int_flags & MCP2515_CANINT_EFLAG) {
+  if (regs.canintf & MCP2515_CANINT_EFLAG) {
     // Clear error flag
     prv_bit_modify(MCP2515_CTRL_REG_CANINTF, MCP2515_CANINT_EFLAG, 0);
   }
 
-  if (err_flags & (MCP2515_EFLG_RX0_OVERFLOW | MCP2515_EFLG_RX1_OVERFLOW)) {
+  if (regs.canintf & (MCP2515_EFLG_RX0_OVERFLOW | MCP2515_EFLG_RX1_OVERFLOW)) {
     // RX overflow - clear error flags
-    uint8_t clear = 0;
-    prv_write(MCP2515_CTRL_REG_EFLG, &clear, 1);
+    // maybe trigger notification for rx
+    prv_bit_modify(MCP2515_CTRL_REG_EFLG, MCP2515_EFLG_RX0_OVERFLOW | MCP2515_EFLG_RX1_OVERFLOW, 0);
   }
 
-  s_storage->errors.eflg = err_flags;
+  s_storage->errors.eflg = regs.eflg;
   prv_read(MCP2515_CTRL_REG_TEC, &s_storage->errors.tec, 1);
   prv_read(MCP2515_CTRL_REG_REC, &s_storage->errors.rec, 1);
 
-  if (err_flags) {
+  if (regs.eflg) {
     // TODO: handle errors
     // s_storage->errors
   }
@@ -195,12 +156,18 @@ static StatusCode mcp2515_hw_init_after_schedular_start() {
                  MCP2515_CANCTRL_OPMODE_MASK | MCP2515_CANCTRL_CLKOUT_MASK,
                  MCP2515_CANCTRL_OPMODE_CONFIG | MCP2515_CANCTRL_CLKOUT_CLKPRE_4);
 
-  // set RXB0 ctrl BUKT bit on to enable rollover to rx1
-  prv_bit_modify(MCP2515_CTRL_REG_RXB0CTRL, 1 << 3, 1 << 3);
+  // set RXB0CTRL.BUKT bit on to enable rollover to rx1
+  prv_bit_modify(MCP2515_CTRL_REG_RXB0CTRL, MCP2515_RXB0CTRL_BUKT, MCP2515_RXB0CTRL_BUKT);
+  // set RXnBF to be message buffer full interrupt
+  prv_bit_modify(
+      MCP2515_CTRL_REG_BFPCTRL,
+      MCP2515_BFPCTRL_B1BFE | MCP2515_BFPCTRL_B2BFE | MCP2515_BFPCTRL_B1BFM | MCP2515_BFPCTRL_B2BFM,
+      MCP2515_BFPCTRL_B1BFE | MCP2515_BFPCTRL_B2BFE | MCP2515_BFPCTRL_B1BFM |
+          MCP2515_BFPCTRL_B2BFM);
   // 5.7 Timing configurations:
   // In order:
-  // CNF3: PS2 Length = 6
-  // CNF2: PS1 Length = 8, PRSEG Length = 1
+  // CNF3: PHSEG2 Length = 6
+  // CNF2: PHSEG1 Length = 8, PRSEG Length = 1
   // CNF1: BRP = 0 (500kbps), 1 (250kbps), 3 (125kbps)
   // CANINTE: Enable error and receive interrupts
   // CANINTF: clear all IRQ flags
@@ -209,11 +176,10 @@ static StatusCode mcp2515_hw_init_after_schedular_start() {
     0x05,
     MCP2515_CNF2_BTLMODE_CNF3 | MCP2515_CNF2_SAMPLE_3X | (0x07 << 3),
     s_brp_lookup[mcp2515_bitrate],
-    MCP2515_CANINT_EFLAG | MCP2515_CANINT_RX1IE | MCP2515_CANINT_RX0IE,
+    MCP2515_CANINT_EFLAG,
     0x00,
     0x00,
   };
-
   prv_write(MCP2515_CTRL_REG_CNF3, s_registers, SIZEOF_ARRAY(s_registers));
 
   // Sanity check: read register after first write
@@ -221,8 +187,12 @@ static StatusCode mcp2515_hw_init_after_schedular_start() {
   uint8_t reg_val = 0;
   prv_read(MCP2515_CTRL_REG_CNF3, &reg_val, 1);
 
-  LOG_DEBUG("MCP2515 Init Status: %s\n",
-            reg_val == 0x05 ? "Connection SUCCESSFUL\n" : "Connection UNSUCCESSFUL\n");
+  LOG_DEBUG("MCP2515 Init Status:");
+  if (reg_val == 0x05) {
+    LOG_DEBUG("Connection SUCCESSFUL\n");
+  } else {
+    LOG_DEBUG("Connection UNSUCCESSFUL\n");
+  }
 
   // Leave config mode
   uint8_t opmode =
@@ -232,29 +202,33 @@ static StatusCode mcp2515_hw_init_after_schedular_start() {
   return STATUS_CODE_OK;
 }
 
-TASK(MCP2515_INTERRUPT, TASK_MIN_STACK_SIZE) {
+TASK(MCP2515_INTERRUPT, TASK_STACK_256) {
   mcp2515_hw_init_after_schedular_start();
 
+  LOG_DEBUG("MCP2515_INTERRUPT hw initilaized\n");
+
   uint32_t notification;
   while (true) {
     notify_wait(&notification, BLOCK_INDEFINITELY);
-    // Read CANINTF and EFLG
-    struct {
-      uint8_t canintf;
-      uint8_t eflg;
-    } regs;
-    prv_read(MCP2515_CTRL_REG_CANINTF, (uint8_t *)&regs, sizeof(regs));
-    // Mask out flags we don't care about
-    regs.canintf &= MCP2515_CANINT_EFLAG | MCP2515_CANINT_RX0IE | MCP2515_CANINT_RX1IE;
-
-    // Either RX or error
-    prv_handle_rx(regs.canintf);
-    prv_handle_error(regs.canintf, regs.eflg);
+
+    // LOG_DEBUG("events: %lx\n", notification);
+
+    if (notify_check_event(&notification, 0)) {  // Error interrupt
+      // LOG_DEBUG("handle error\n");
+      prv_handle_error();
+    }
+    if (notify_check_event(&notification, 1)) {  // RX0BF
+      prv_handle_rx(0);
+    }
+    if (notify_check_event(&notification, 2)) {  // RX1BF
+      prv_handle_rx(1);
+    }
   }
 }
 
 // Initializes CAN using the specified settings.
-StatusCode mcp2515_hw_init(const CanQueue *rx_queue, const Mcp2515Settings *settings) {
+StatusCode mcp2515_hw_init(Mcp2515Storage *storage, const Mcp2515Settings *settings) {
+  s_storage = storage;
   // settings->spi_settings.mode = SPI_MODE_0;
   status_ok_or_return(spi_init(settings->spi_port, &settings->spi_settings));
 
@@ -265,16 +239,27 @@ StatusCode mcp2515_hw_init(const CanQueue *rx_queue, const Mcp2515Settings *sett
   mcp2515_bitrate = settings->can_settings.bitrate;
   mcp2515_loopback = settings->can_settings.loopback;
 
+  s_storage->spi_port = settings->spi_port;
+  s_storage->loopback = settings->can_settings.loopback;
+
   // active low
   status_ok_or_return(
       gpio_init_pin(&settings->interrupt_pin, GPIO_INPUT_FLOATING, GPIO_STATE_HIGH));
+  status_ok_or_return(gpio_init_pin(&settings->RX0BF, GPIO_INPUT_FLOATING, GPIO_STATE_HIGH));
+  status_ok_or_return(gpio_init_pin(&settings->RX1BF, GPIO_INPUT_FLOATING, GPIO_STATE_HIGH));
+
   const InterruptSettings it_settings = {
     .type = INTERRUPT_TYPE_INTERRUPT,
     .priority = INTERRUPT_PRIORITY_NORMAL,
     .edge = INTERRUPT_EDGE_FALLING,
   };
+
   status_ok_or_return(
       gpio_it_register_interrupt(&settings->interrupt_pin, &it_settings, 0, MCP2515_INTERRUPT));
+  status_ok_or_return(
+      gpio_it_register_interrupt(&settings->RX0BF, &it_settings, 1, MCP2515_INTERRUPT));
+  status_ok_or_return(
+      gpio_it_register_interrupt(&settings->RX1BF, &it_settings, 2, MCP2515_INTERRUPT));
 
   // ! Ensure the task priority is higher than the rx/tx tasks in mcp2515.c
   status_ok_or_return(tasks_init_task(MCP2515_INTERRUPT, TASK_PRIORITY(3), NULL));
@@ -286,106 +271,104 @@ StatusCode mcp2515_hw_add_filter_in(uint32_t mask, uint32_t filter, bool extende
 
 // CanHwBusStatus mcp2515_hw_bus_status(void);
 
-StatusCode mcp2515_hw_transmit(uint32_t id, bool extended, const uint64_t data, size_t len) {
+StatusCode mcp2515_hw_transmit(uint32_t id, bool extended, uint8_t *data, size_t len) {
   // Ensure the CANCTRL register is set to the correct value
+  //
   prv_bit_modify(MCP2515_CTRL_REG_CANCTRL, 0x1f, 0x0f);
   // Get free transmit buffer
+  uint8_t status = prv_read_status();
+  // LOG_DEBUG("stats: %x\n", status); //
   uint8_t tx_status = __builtin_ffs(
-      ~prv_read_status() & (MCP2515_STATUS_TX0REQ | MCP2515_STATUS_TX1REQ | MCP2515_STATUS_TX2REQ));
+      ~status & (MCP2515_STATUS_TX0REQ | MCP2515_STATUS_TX1REQ | MCP2515_STATUS_TX2REQ));
+
   if (tx_status == 0) {
+    LOG_DEBUG("Failed to tx, buffer full\n");
     return status_code(STATUS_CODE_RESOURCE_EXHAUSTED);
   }
+  // LOG_DEBUG("message on buffer %d\n", (tx_status - 3) / 2);
 
   // Status format: 0b01010100 = all TXxREQ bits set
   // ffs returns 1-indexed: (x-3)/2 -> 0b00000111 = all TXxREQ bits set
   Mcp2515TxBuffer *tx_buf = &s_tx_buffers[(tx_status - 3) / 2];
 
-  Mcp2515Id tx_id = { .raw = id };
-  // If it's a standard id, make sure it lines up in the right bits
-  if (tx_id.raw >> MCP2515_STANDARD_ID_LEN == 0) {
-    tx_id.raw <<= MCP2515_EXTENDED_ID_LEN;
+  Mcp2515IdRegs tx_id_regs = { 0 };
+  if (extended) {
+    // extended
+    tx_id_regs.sid = id >> MCP2515_STANDARD_ID_LEN;
+    tx_id_regs.eid = id;
+    tx_id_regs.extended = true;
+  } else {
+    // standard
+    tx_id_regs.sid = id;
+    tx_id_regs.extended = false;
   }
 
-  Mcp2515IdRegs tx_id_regs = {
-    .sidh = tx_id.sidh,
-    .sidl = { .eid_16_17 = tx_id.eid_16_17,
-              .ide = extended,
-              .srr = false,
-              .sid_0_2 = tx_id.sid_0_2 },
-    .eid0 = tx_id.eid0,
-    .eid8 = tx_id.eid8,
-    .dlc = { .dlc = len, .rtr = false },
-  };
-
+  // This payload buffer can be reused if needed for stack mem.
   // Load ID: SIDH, SIDL, EID8, EID0, RTSnDLC
   uint8_t id_payload[] = {
-    MCP2515_CMD_LOAD_TX | tx_buf->id,
-    tx_id_regs.sidh,
-    tx_id_regs.sidl.raw,
-    tx_id_regs.eid8,
-    tx_id_regs.eid0,
-    tx_id_regs.dlc.raw,
+    MCP2515_CMD_LOAD_TX | tx_buf->id,  // tx_id_regs 3-3 to 3-6
+    tx_id_regs.registers[3],          tx_id_regs.registers[2],
+    tx_id_regs.registers[1],          tx_id_regs.registers[0],
+    (len & 0xf) | (false << 6),  // Reg 3-7 RTR and DLC
   };
-  status_ok_or_return(spi_exchange(s_storage->spi_port, id_payload, sizeof(id_payload), NULL, 0));
+  spi_tx(s_storage->spi_port, id_payload, sizeof(id_payload));
 
   // Load data
-  Mcp2515LoadTxPayload data_payload = {
-    .cmd = MCP2515_CMD_LOAD_TX | tx_buf->data,
-    .data = data,
-  };
-  status_ok_or_return(
-      spi_exchange(s_storage->spi_port, (uint8_t *)&data_payload, sizeof(data_payload), NULL, 0));
+  uint8_t data_payload[9] = { MCP2515_CMD_LOAD_TX | tx_buf->data };
+  memcpy(&data_payload[1], data, len);
+
+  spi_tx(s_storage->spi_port, data_payload, len + 1);
 
   // Send message
   uint8_t send_payload[] = { MCP2515_CMD_RTS | tx_buf->rts };
-  status_ok_or_return(
-      spi_exchange(s_storage->spi_port, send_payload, sizeof(send_payload), NULL, 0));
+  spi_tx(s_storage->spi_port, send_payload, sizeof(send_payload));
 
-  return STATUS_CODE_OK;
-}
+  uint8_t data_test[] = { MCP2515_CMD_READ, 0x30 };
+  uint8_t receive = 0;
+  StatusCode ret = spi_exchange(SPI_PORT_2, data_test, 2, &receive, 1);
+  // LOG_DEBUG("status: %x\n", receive);  // if 8, then its ok
 
-// Must be called within the RX handler, returns whether a message was processed
-// bool mcp2515_hw_receive(uint32_t *id, bool *extended, uint64_t *data, size_t *len);
+  return ret;
+}
 
 // Call with MCP2515 in Config mode to set filters
 static void prv_configure_filters(CanMessageId *filters) {
-  Mcp2515Id default_filter = { .raw = filters[MCP2515_FILTER_ID_RXF0] };
   for (size_t i = 0; i < NUM_MCP2515_FILTER_IDS; i++) {
-    Mcp2515Id filter = { .raw = filters[i] };
-    if (default_filter.raw == 0) {
-      continue;
-    }
-
     // Prevents us from filtering for id 0x0
     if (filters[i] == 0) {
-      filter = default_filter;
+      continue;
     }
-
-    uint8_t maskRegH = MCP2515_REG_RXM0SIDH;
-    if (i == MCP2515_FILTER_ID_RXF1) maskRegH = MCP2515_REG_RXM1SIDH;
-    // If it's a standard id, ensure it's placed in the right bits
-    if (filter.raw >> MCP2515_STANDARD_ID_LEN == 0) {
-      filter.raw <<= MCP2515_EXTENDED_ID_LEN;
+    Mcp2515IdRegs id_regs = { 0 };
+
+    if (filters[i] >> MCP2515_STANDARD_ID_LEN) {
+      // extended
+      id_regs.extended = true;
+      id_regs.sid = filters[i] >> MCP2515_EXTENDED_ID_LEN;
+      id_regs.eid = filters[i];
+    } else {
+      // standard
+      id_regs.extended = false;
+      id_regs.sid = filters[i];
     }
-    bool standard = filter.raw << (32 - MCP2515_EXTENDED_ID_LEN) == 0;
-    size_t numMaskRegisters =
-        standard ? MCP2515_NUM_MASK_REGISTERS_STANDARD : MCP2515_NUM_MASK_REGISTERS_EXTENDED;
+
+    uint8_t maskRegH = (i == MCP2515_FILTER_ID_RXF0) ? MCP2515_REG_RXM0SIDH : MCP2515_REG_RXM1SIDH;
+    size_t numMaskRegisters = id_regs.extended ? MCP2515_NUM_MASK_REGISTERS_EXTENDED
+                                               : MCP2515_NUM_MASK_REGISTERS_STANDARD;
     // Set the filter masks to 0xff so we filter on the whole message
     for (size_t i = 0; i < numMaskRegisters; i++) {
       prv_bit_modify(maskRegH + i, 0xff, 0xff);
     }
-    // If it is just a standard id, then shift it up to match the struct
-    uint8_t filterRegH = MCP2515_REG_RXF0SIDH;
-    if (i == MCP2515_FILTER_ID_RXF1) filterRegH = MCP2515_REG_RXF1SIDH;
-    uint8_t filterRegL = filterRegH + 1;
+
+    uint8_t filterRegH =
+        (i == MCP2515_FILTER_ID_RXF0) ? MCP2515_REG_RXF0SIDH : MCP2515_REG_RXF1SIDH;
     // Set sidh
-    prv_bit_modify(filterRegH, 0xff, filter.sidh);
+    prv_bit_modify(filterRegH, 0xff, id_regs.registers[3]);
     // Set sidl and eid16-17
-    prv_bit_modify(filterRegL, 0xff, (filter.sid_0_2 << 5) | ((!standard) << 3) | filter.eid_16_17);
+    prv_bit_modify(filterRegH + 1, 0xff, id_regs.registers[2]);
     // Set eid8-15
-    prv_bit_modify(filterRegH + 2, 0xff, filter.eid8);
+    prv_bit_modify(filterRegH + 2, 0xff, id_regs.registers[1]);
     // Set eid0-7
-    prv_bit_modify(filterRegH + 3, 0xff, filter.eid0);
+    prv_bit_modify(filterRegH + 3, 0xff, id_regs.registers[0]);
   }
 }
 
diff --git a/projects/motor_controller/src/motor_can.c b/projects/motor_controller/src/motor_can.c
index 14df78430..5a275cbc3 100644
--- a/projects/motor_controller/src/motor_can.c
+++ b/projects/motor_controller/src/motor_can.c
@@ -3,6 +3,7 @@
 
 #include <stdint.h>
 
+#include "log.h"
 #include "mcp2515.h"
 #include "motor_controller_getters.h"
 #include "motor_controller_setters.h"
@@ -57,10 +58,10 @@ static void prv_update_target_current_velocity() {
   bool regen = get_drive_output_regen_braking();
   bool cruise = get_drive_output_cruise_control();
 
-  if (cruise && throttle_percent > CRUISE_THROTTLE_THRESHOLD) {
-    drive_state = DRIVE;
+  if (drive_state == DRIVE && cruise && throttle_percent <= CRUISE_THROTTLE_THRESHOLD) {
+    drive_state = CRUISE;
   }
-  if (brake_percent > 0 || throttle_percent == 0) {
+  if (brake_percent > 0 || (throttle_percent == 0 && drive_state != CRUISE)) {
     drive_state = regen ? BRAKE : NEUTRAL;
   }
 
@@ -93,17 +94,39 @@ static void prv_update_target_current_velocity() {
   }
 }
 
+static inline uint8_t pack_left_shift_u32(uint32_t value, uint8_t shift, uint8_t mask) {
+  return (uint8_t)((uint8_t)(value << shift) & mask);
+}
+
+static inline uint8_t pack_right_shift_u32(uint32_t value, uint8_t shift, uint8_t mask) {
+  return (uint8_t)((uint8_t)(value >> shift) & mask);
+}
+
 static void motor_controller_tx_all() {
-  // TODO: add can watchdog to shut down motor controller if messages are not received from
-  // center console
+  // don't send drive command if didn't get centre console's drive output msg
+  if (!get_received_drive_output()) {
+    LOG_DEBUG("NO drive output\n");
+    return;
+  }
+  // if (!get_pedal_output_brake_output()) return;
+  // don't send drive command if not precharged
+  if (!g_tx_struct.mc_status_precharge_status) {
+    LOG_DEBUG("no precharge\n");
+    return;
+  }
+
   prv_update_target_current_velocity();
 
   CanMessage message = {
     .id.raw = DRIVER_CONTROL_BASE + 0x01,
     .dlc = 8,
   };
-  memcpy(&message.data_u32[0], &s_target_current, sizeof(s_target_current));
-  memcpy(&message.data_u32[1], &s_target_velocity, sizeof(s_target_velocity));
+  // s_target_velocity = 160.0;
+  memcpy(&message.data_u32[0], &s_target_velocity, sizeof(uint32_t));
+  memcpy(&message.data_u32[1], &s_target_current, sizeof(uint32_t));
+
+  LOG_DEBUG("s_target_current: %d\n", (int)(s_target_current * 100));
+  LOG_DEBUG("s_target_velocity: %d\n", (int)(s_target_velocity * 100));
 
   mcp2515_transmit(&message);
 }
@@ -113,35 +136,46 @@ static void motor_controller_rx_all() {
   while (mcp2515_receive(&msg) == STATUS_CODE_OK) {
     switch (msg.id.raw) {
       case MOTOR_CONTROLLER_BASE_L + STATUS:
-        set_motor_status_motor_status_l(msg.data_u32[1]);
+        set_mc_status_error_bitset_l(msg.data_u16[1] >> 1);
+        set_mc_status_limit_bitset_l(msg.data_u16[0]);
         break;
       case MOTOR_CONTROLLER_BASE_R + STATUS:
-        set_motor_status_motor_status_r(msg.data_u32[1]);
+        set_mc_status_error_bitset_r(msg.data_u16[1] >> 1);
+        set_mc_status_limit_bitset_r(msg.data_u16[0]);
         break;
 
       case MOTOR_CONTROLLER_BASE_L + BUS_MEASUREMENT:
-        set_motor_controller_vc_mc_current_l(prv_get_float(msg.data_u32[0]) * CURRENT_SCALE);
-        set_motor_controller_vc_mc_voltage_l(prv_get_float(msg.data_u32[1]) * VOLTAGE_SCALE);
+        set_motor_controller_vc_mc_current_l(prv_get_float(msg.data_u32[1]) * CURRENT_SCALE);
+        set_motor_controller_vc_mc_voltage_l(prv_get_float(msg.data_u32[0]) * VOLTAGE_SCALE);
         break;
       case MOTOR_CONTROLLER_BASE_R + BUS_MEASUREMENT:
-        set_motor_controller_vc_mc_current_r(prv_get_float(msg.data_u32[0]) * CURRENT_SCALE);
-        set_motor_controller_vc_mc_voltage_r(prv_get_float(msg.data_u32[1]) * VOLTAGE_SCALE);
+        set_motor_controller_vc_mc_current_r(prv_get_float(msg.data_u32[1]) * CURRENT_SCALE);
+        set_motor_controller_vc_mc_voltage_r(prv_get_float(msg.data_u32[0]) * VOLTAGE_SCALE);
         break;
 
       case MOTOR_CONTROLLER_BASE_L + VEL_MEASUREMENT:
-        set_motor_velocity_velocity_l(prv_get_float(msg.data_u32[0]) * VELOCITY_SCALE);
+        set_motor_velocity_velocity_l(
+            (uint16_t)(int16_t)(prv_get_float(msg.data_u32[1]) * VELOCITY_SCALE));
         break;
       case MOTOR_CONTROLLER_BASE_R + VEL_MEASUREMENT:
-        set_motor_velocity_velocity_r(prv_get_float(msg.data_u32[0]) * VELOCITY_SCALE);
+        set_motor_velocity_velocity_r(
+            (uint16_t)(int16_t)(prv_get_float(msg.data_u32[1]) * VELOCITY_SCALE));
         break;
 
       case MOTOR_CONTROLLER_BASE_L + HEAT_SINK_MOTOR_TEMP:
-        set_motor_sink_temps_heatsink_temp_l(prv_get_float(msg.data_u32[0]) * TEMP_SCALE);
-        set_motor_sink_temps_motor_temp_l(prv_get_float(msg.data_u32[1]) * TEMP_SCALE);
+        set_motor_sink_temps_heatsink_temp_l(prv_get_float(msg.data_u32[1]) * TEMP_SCALE);
+        set_motor_sink_temps_motor_temp_l(prv_get_float(msg.data_u32[0]) * TEMP_SCALE);
         break;
       case MOTOR_CONTROLLER_BASE_R + HEAT_SINK_MOTOR_TEMP:
-        set_motor_sink_temps_heatsink_temp_r(prv_get_float(msg.data_u32[0]) * TEMP_SCALE);
-        set_motor_sink_temps_motor_temp_r(prv_get_float(msg.data_u32[1]) * TEMP_SCALE);
+        set_motor_sink_temps_heatsink_temp_r(prv_get_float(msg.data_u32[1]) * TEMP_SCALE);
+        set_motor_sink_temps_motor_temp_r(prv_get_float(msg.data_u32[0]) * TEMP_SCALE);
+        break;
+
+      case MOTOR_CONTROLLER_BASE_L + DSP_BOARD_TEMP:
+        set_dsp_board_temps_dsp_temp_l(prv_get_float(msg.data_u32[0]) * TEMP_SCALE);
+        break;
+      case MOTOR_CONTROLLER_BASE_R + DSP_BOARD_TEMP:
+        set_dsp_board_temps_dsp_temp_r(prv_get_float(msg.data_u32[0]) * TEMP_SCALE);
         break;
     }
   }
diff --git a/projects/motor_controller/src/precharge.c b/projects/motor_controller/src/precharge.c
new file mode 100644
index 000000000..c09cef342
--- /dev/null
+++ b/projects/motor_controller/src/precharge.c
@@ -0,0 +1,24 @@
+#include "precharge.h"
+
+#include "can.h"
+#include "gpio.h"
+#include "gpio_it.h"
+#include "interrupt.h"
+#include "log.h"
+#include "master_task.h"
+#include "motor_controller_getters.h"
+#include "motor_controller_setters.h"
+#include "status.h"
+
+StatusCode precharge_init(const PrechargeSettings *settings, Event event, const Task *task) {
+  // gpio_init_pin(&settings->precharge_control, GPIO_OUTPUT_PUSH_PULL, GPIO_STATE_LOW);
+  gpio_init_pin(&settings->precharge_monitor, GPIO_INPUT_FLOATING, GPIO_STATE_LOW);
+
+  InterruptSettings monitor_it_settings = {
+    .type = INTERRUPT_TYPE_INTERRUPT,
+    .priority = INTERRUPT_PRIORITY_NORMAL,
+    .edge = INTERRUPT_EDGE_RISING,
+  };
+  return gpio_it_register_interrupt(&settings->precharge_monitor, &monitor_it_settings, event,
+                                    task);
+}
diff --git a/projects/motor_controller/src/precharge_control.c b/projects/motor_controller/src/precharge_control.c
deleted file mode 100644
index 8d5acd383..000000000
--- a/projects/motor_controller/src/precharge_control.c
+++ /dev/null
@@ -1,67 +0,0 @@
-#include "precharge_control.h"
-
-#include "can.h"
-#include "gpio.h"
-#include "gpio_it.h"
-#include "interrupt.h"
-#include "log.h"
-#include "motor_controller_getters.h"
-#include "motor_controller_setters.h"
-#include "status.h"
-
-static GpioAddress s_precharge_control;
-
-StatusCode prv_set_precharge_control(GpioAddress *address, const GpioState state) {
-  gpio_set_state(&s_precharge_control, state);
-  return STATUS_CODE_OK;
-}
-
-TASK(PRECHARGE_INTERRUPT, TASK_MIN_STACK_SIZE) {
-  uint32_t notification;
-  while (true) {
-    notify_wait(&notification, BLOCK_INDEFINITELY);
-
-    if (notify_check_event(&notification, 0)) {
-      if (get_drive_output_precharge() == MCI_PRECHARGE_DISCHARGED) {
-        // inconsistent until second precharge result
-        set_mc_status_precharge_status(MCI_PRECHARGE_INCONSISTENT);
-      } else {
-        set_mc_status_precharge_status(MCI_PRECHARGE_CHARGED);
-      }
-    }
-    if (notify_check_event(&notification, 1)) {
-      if (get_drive_output_precharge() == MCI_PRECHARGE_DISCHARGED) {
-        // inconsistent until second precharge result
-        set_mc_status_precharge_status(MCI_PRECHARGE_INCONSISTENT);
-      } else {
-        set_mc_status_precharge_status(MCI_PRECHARGE_CHARGED);
-      }
-    }
-  }
-}
-
-StatusCode run_precharge_rx_cycle() {
-  if (get_drive_output_precharge() == MCI_PRECHARGE_CHARGED) {
-    return prv_set_precharge_control(&s_precharge_control, GPIO_STATE_HIGH);
-  } else {
-    return prv_set_precharge_control(&s_precharge_control, GPIO_STATE_LOW);
-  }
-}
-
-StatusCode precharge_control_init(const PrechargeControlSettings *settings) {
-  interrupt_init();
-
-  s_precharge_control = settings->precharge_control;
-
-  InterruptSettings monitor_it_settings = {
-    .type = INTERRUPT_TYPE_INTERRUPT,
-    .priority = INTERRUPT_PRIORITY_NORMAL,
-    .edge = INTERRUPT_EDGE_RISING,
-  };
-  status_ok_or_return(gpio_it_register_interrupt(&settings->precharge_monitor, &monitor_it_settings,
-                                                 0, PRECHARGE_INTERRUPT));
-  status_ok_or_return(gpio_it_register_interrupt(&settings->precharge_monitor2,
-                                                 &monitor_it_settings, 1, PRECHARGE_INTERRUPT));
-  status_ok_or_return(tasks_init_task(PRECHARGE_INTERRUPT, TASK_PRIORITY(2), NULL));
-  return STATUS_CODE_OK;
-}
diff --git a/projects/motor_controller/test/test_mcp2515_hw.c b/projects/motor_controller/test/test_mcp2515_hw.c
new file mode 100644
index 000000000..889f69a4e
--- /dev/null
+++ b/projects/motor_controller/test/test_mcp2515_hw.c
@@ -0,0 +1,189 @@
+#include "callback_handler.h"
+#include "delay.h"
+#include "gpio_it.h"
+#include "interrupt.h"
+#include "log.h"
+#include "mcp2515.h"
+#include "queues.h"
+#include "spi.h"
+#include "status.h"
+#include "stdint.h"
+#include "test_helpers.h"
+#include "unity.h"
+
+typedef union {
+  struct {
+    uint32_t eid : 18;
+    uint8_t _ : 1;
+    uint8_t extended : 1;
+    uint8_t srr : 1;
+    uint32_t sid : 11;
+  };
+  uint8_t registers[4];
+} Mcp2515IdRegs;
+
+Mcp2515Storage s_storage;
+static Mcp2515Settings s_mcp2515_settings = {
+  .spi_port = SPI_PORT_2,
+  .spi_settings = {
+    .baudrate = 10000000,  // 10 Mhz
+    .mode = SPI_MODE_0,
+    .mosi = { GPIO_PORT_B, 15 },
+    .miso = { GPIO_PORT_B, 14 },
+    .sclk = { GPIO_PORT_B, 13 },
+    .cs = { GPIO_PORT_B, 12 },
+  },
+  .interrupt_pin = { GPIO_PORT_A, 8 },
+  .RX0BF = { GPIO_PORT_B, 10 },
+  .RX1BF = { GPIO_PORT_B, 11 },
+  .can_settings = {
+    .bitrate = CAN_HW_BITRATE_500KBPS,
+    .loopback = true,
+  },
+};
+
+bool initialized = false;
+void setup_test() {
+  if (initialized) return;
+  initialized = true;
+
+  gpio_init();
+  interrupt_init();
+  gpio_it_init();
+  callback_init(1);
+
+  can_queue_init(&s_storage.rx_queue);
+
+  StatusCode code = mcp2515_hw_init(&s_storage, &s_mcp2515_settings);
+  TEST_ASSERT_OK(code);
+}
+
+void teardown_test() {}
+
+TEST_IN_TASK
+void test_mcp2515_init_after_schedular_start(void) {
+  uint8_t data[10];
+
+  // reset
+  TEST_ASSERT_OK(spi_get_tx(SPI_PORT_2, data, 1));
+  TEST_ASSERT_EQUAL(MCP2515_CMD_RESET, data[0]);
+
+  // Set to Config mode, CLKOUT /4
+  TEST_ASSERT_OK(spi_get_tx(SPI_PORT_2, data, 4));
+  TEST_ASSERT_EQUAL(MCP2515_CMD_BIT_MODIFY, data[0]);
+  TEST_ASSERT_EQUAL(MCP2515_CTRL_REG_CANCTRL, data[1]);
+
+  // set RXB0CTRL.BUKT bit on to enable rollover to rx1
+  TEST_ASSERT_OK(spi_get_tx(SPI_PORT_2, data, 4));
+  TEST_ASSERT_EQUAL(MCP2515_CMD_BIT_MODIFY, data[0]);
+  TEST_ASSERT_EQUAL(MCP2515_CTRL_REG_RXB0CTRL, data[1]);
+
+  // set RXnBF to be message buffer full interrupt
+  TEST_ASSERT_OK(spi_get_tx(SPI_PORT_2, data, 4));
+  TEST_ASSERT_EQUAL(MCP2515_CMD_BIT_MODIFY, data[0]);
+  TEST_ASSERT_EQUAL(MCP2515_CTRL_REG_BFPCTRL, data[1]);
+
+  // timing config
+  TEST_ASSERT_OK(spi_get_tx(SPI_PORT_2, data, 8));
+  TEST_ASSERT_EQUAL(MCP2515_CMD_WRITE, data[0]);
+  TEST_ASSERT_EQUAL(MCP2515_CTRL_REG_CNF3, data[1]);
+
+  // sanity check
+  TEST_ASSERT_OK(spi_get_tx(SPI_PORT_2, data, 2));
+  TEST_ASSERT_EQUAL(MCP2515_CMD_READ, data[0]);
+  TEST_ASSERT_EQUAL(MCP2515_CTRL_REG_CNF3, data[1]);
+  data[0] = 0x05;
+  spi_set_rx(SPI_PORT_2, data, 1);
+
+  // Leave config mode
+  TEST_ASSERT_OK(spi_get_tx(SPI_PORT_2, data, 4));
+  TEST_ASSERT_EQUAL(MCP2515_CMD_BIT_MODIFY, data[0]);
+  TEST_ASSERT_EQUAL(MCP2515_CTRL_REG_CANCTRL, data[1]);
+}
+
+bool transmit_message(void *data) {
+  CanMessage *message = (CanMessage *)data;
+  status_ok_or_return(
+      mcp2515_hw_transmit(message->id.raw, message->extended, message->data_u8, message->dlc));
+  return true;
+}
+
+TEST_IN_TASK
+void test_tx(void) {
+  CanMessage message = {
+    .extended = false,
+    .id.raw = 641,  // 1241,
+    .dlc = 8,
+    .data_u8 = { 0, 1, 2, 3, 4, 5, 6, 7 },
+  };
+  // run transmit_message(&message); async
+  Event event = register_callback(transmit_message, &message);
+  notify(callback_task, event);
+  delay_ms(1);
+
+  uint8_t data[10];
+  // Ensure the CANCTRL register is set to the correct value
+  TEST_ASSERT_OK(spi_get_tx(SPI_PORT_2, data, 4));
+  TEST_ASSERT_EQUAL(MCP2515_CMD_BIT_MODIFY, data[0]);
+  TEST_ASSERT_EQUAL(MCP2515_CTRL_REG_CANCTRL, data[1]);
+
+  // read status
+  TEST_ASSERT_OK(spi_get_tx(SPI_PORT_2, data, 1));
+  TEST_ASSERT_EQUAL(MCP2515_CMD_READ_STATUS, data[0]);
+  data[0] = 0x00;  // all buffer free
+  spi_set_rx(SPI_PORT_2, data, 1);
+
+  // id
+  TEST_ASSERT_OK(spi_get_tx(SPI_PORT_2, data, 6));
+  TEST_ASSERT_EQUAL(MCP2515_CMD_LOAD_TX | MCP2515_LOAD_TXB0SIDH, data[0]);
+  TEST_ASSERT_EQUAL(message.id.raw >> 3, data[1]);
+  TEST_ASSERT_EQUAL((message.id.raw & 7) << 5, data[2]);
+  TEST_ASSERT_EQUAL(message.dlc, data[5] & 0xf);
+
+  // data
+  TEST_ASSERT_OK(spi_get_tx(SPI_PORT_2, data, 1 + message.dlc));
+  TEST_ASSERT_EQUAL(MCP2515_CMD_LOAD_TX | MCP2515_LOAD_TXB0D0, data[0]);
+  for (size_t i = 0; i < message.dlc; ++i) {
+    TEST_ASSERT_EQUAL(message.data_u8[i], data[i + 1]);
+  }
+
+  // send message (rts)
+  TEST_ASSERT_OK(spi_get_tx(SPI_PORT_2, data, 1));
+  TEST_ASSERT_EQUAL(MCP2515_CMD_RTS | MCP2515_RTS_TXB0, data[0]);
+}
+
+TEST_IN_TASK
+void test_rx(void) {
+  uint8_t data;
+  // trigger RX0BF gpio pin interrupt, expect an spi_exchange
+  TEST_ASSERT_OK(gpio_it_trigger_interrupt(&s_mcp2515_settings.RX0BF));
+
+  // id and data
+  Mcp2515IdRegs id_regs = { .sid = 641 };
+  uint8_t regs_data[] = {
+    id_regs.registers[3],
+    id_regs.registers[2],
+    id_regs.registers[1],
+    id_regs.registers[0],
+    8,
+    0,
+    1,
+    2,
+    3,
+    4,
+    5,
+    6,
+    7,
+  };
+  TEST_ASSERT_OK(spi_get_tx(SPI_PORT_2, &data, 1));
+  spi_set_rx(SPI_PORT_2, regs_data, sizeof(regs_data));
+  TEST_ASSERT_EQUAL(MCP2515_CMD_READ_RX | MCP2515_READ_RXB0SIDH, data);
+
+  // assert message
+  CanMessage message;
+  TEST_ASSERT_OK(queue_receive(&s_storage.rx_queue.queue, &message, 100));
+
+  TEST_ASSERT_EQUAL(641, message.id.raw);
+  TEST_ASSERT_EQUAL(8, message.dlc);
+  TEST_ASSERT_EQUAL_UINT8_ARRAY(&regs_data[5], message.data_u8, 8);
+}
diff --git a/projects/motor_controller/test/test_motor_can_rx.c b/projects/motor_controller/test/test_motor_can_rx.c
new file mode 100644
index 000000000..29aee0eda
--- /dev/null
+++ b/projects/motor_controller/test/test_motor_can_rx.c
@@ -0,0 +1,166 @@
+// Test motor_can rx (status responses)
+
+#include <stdio.h>
+
+#include "can.h"
+#include "can_board_ids.h"
+#include "can_codegen.h"
+#include "can_queue.h"
+#include "delay.h"
+#include "fsm.h"
+#include "gpio_it.h"
+#include "interrupt.h"
+#include "log.h"
+#include "mcp2515.h"
+#include "misc.h"
+#include "motor_can.h"
+#include "precharge.h"
+#include "soft_timer.h"
+#include "task_test_helpers.h"
+#include "tasks.h"
+#include "test_helpers.h"
+#include "unity.h"
+
+static Mcp2515Storage s_mcp2515_storage = { 0 };
+static Mcp2515Settings s_mcp2515_settings = {
+  .can_settings = {
+    .mode = CAN_CONTINUOUS,
+  },
+};
+
+StatusCode TEST_MOCK(mcp2515_hw_transmit)(uint32_t id, bool extended, uint8_t *data, size_t len) {
+  return STATUS_CODE_OK;
+}
+
+StatusCode TEST_MOCK(mcp2515_hw_init)(const CanQueue *rx_queue, const Mcp2515Settings *settings) {
+  return STATUS_CODE_OK;
+}
+
+static uint32_t prv_get_uint32(float f) {
+  union {
+    float f;
+    uint32_t u;
+  } fu = { .f = f };
+  return fu.u;
+}
+
+bool initialized = false;
+void setup_test(void) {
+  if (initialized) return;
+  initialized = true;
+  tasks_init();
+  log_init();
+  mcp2515_init(&s_mcp2515_storage, &s_mcp2515_settings);
+  init_motor_controller_can();
+}
+
+void teardown_test(void) {}
+
+void run_motor_controller_cycle() {
+  run_mcp2515_rx_cycle();
+  wait_tasks(1);
+  run_mcp2515_tx_cycle();
+  wait_tasks(1);
+}
+
+void push_mc_message(uint32_t id, float data_1, float data_2) {
+  CanMessage message = {
+    .id.raw = id,
+    .data_u32[0] = prv_get_uint32(data_1),
+    .data_u32[1] = prv_get_uint32(data_2),
+    .dlc = 8,
+  };
+  TEST_ASSERT_OK(can_queue_push(&s_mcp2515_storage.rx_queue, &message));
+}
+
+TEST_IN_TASK
+void test_status(void) {
+  CanMessage status_l = {
+    .id.raw = MOTOR_CONTROLLER_BASE_L + 0x01,
+    .data_u8[0] = 1,             // Receive error count
+    .data_u8[1] = 2,             // Transmit error count
+    .data_u16[1] = 3,            // Active motor
+    .data_u16[2] = 0b011001101,  // Error Flags
+    .data_u16[3] = 0b110011010,  // Limit Flags
+    .dlc = 8,
+  };
+  can_queue_push(&s_mcp2515_storage.rx_queue, &status_l);
+  CanMessage status_r = {
+    .id.raw = MOTOR_CONTROLLER_BASE_R + 0x01,
+    .data_u8[0] = 1,             // Receive error count
+    .data_u8[1] = 2,             // Transmit error count
+    .data_u16[1] = 3,            // Active motor
+    .data_u16[2] = 0b100110010,  // Error Flags
+    .data_u16[3] = 0b001100101,  // Limit Flags
+    .dlc = 8,
+  };
+  can_queue_push(&s_mcp2515_storage.rx_queue, &status_r);
+
+  run_motor_controller_cycle();
+
+  TEST_ASSERT_EQUAL(0b01100110, g_tx_struct.mc_status_error_bitset_l);
+  TEST_ASSERT_EQUAL(0b10011010, g_tx_struct.mc_status_limit_bitset_l);
+  TEST_ASSERT_EQUAL(0b10011001, g_tx_struct.mc_status_error_bitset_r);
+  TEST_ASSERT_EQUAL(0b01100101, g_tx_struct.mc_status_limit_bitset_r);
+}
+
+// Bus Measurement (Bus Current: A, Bus Voltage: V)
+TEST_IN_TASK
+void test_current_voltage(void) {
+  push_mc_message(MOTOR_CONTROLLER_BASE_L + 0x02, 0.1f, 0.2f);
+  push_mc_message(MOTOR_CONTROLLER_BASE_R + 0x02, 0.3f, 0.4f);
+
+  run_motor_controller_cycle();
+
+  TEST_ASSERT_EQUAL(10, g_tx_struct.motor_controller_vc_mc_current_l);
+  TEST_ASSERT_EQUAL(20, g_tx_struct.motor_controller_vc_mc_voltage_l);
+  TEST_ASSERT_EQUAL(30, g_tx_struct.motor_controller_vc_mc_current_r);
+  TEST_ASSERT_EQUAL(40, g_tx_struct.motor_controller_vc_mc_voltage_r);
+}
+
+// Velocity Measurement (Vehicle Velocity: m/s, Motor Velocity: rpm)
+TEST_IN_TASK
+void test_velocity(void) {
+  push_mc_message(MOTOR_CONTROLLER_BASE_L + 0x03, 0.1f, 0.2f);
+  push_mc_message(MOTOR_CONTROLLER_BASE_R + 0x03, 0.3f, 0.4f);
+
+  run_motor_controller_cycle();
+
+  TEST_ASSERT_EQUAL(10, g_tx_struct.motor_velocity_velocity_l);
+  TEST_ASSERT_EQUAL(30, g_tx_struct.motor_velocity_velocity_r);
+
+  // negative
+  push_mc_message(MOTOR_CONTROLLER_BASE_L + 0x03, -0.1f, 0.2f);
+  push_mc_message(MOTOR_CONTROLLER_BASE_R + 0x03, -0.3f, 0.4f);
+
+  run_motor_controller_cycle();
+
+  TEST_ASSERT_EQUAL((uint16_t)-10, g_tx_struct.motor_velocity_velocity_l);
+  TEST_ASSERT_EQUAL((uint16_t)-30, g_tx_struct.motor_velocity_velocity_r);
+}
+
+// Heat-sink & Motor Temperature Measurement (Heat-sink Temp: C, Motor Temp: C)
+TEST_IN_TASK
+void test_heatsink_motor_temp(void) {
+  push_mc_message(MOTOR_CONTROLLER_BASE_L + 0x0B, 0.1f, 0.2f);
+  push_mc_message(MOTOR_CONTROLLER_BASE_R + 0x0B, 0.3f, 0.4f);
+
+  run_motor_controller_cycle();
+
+  TEST_ASSERT_EQUAL(10, g_tx_struct.motor_sink_temps_heatsink_temp_l);
+  TEST_ASSERT_EQUAL(20, g_tx_struct.motor_sink_temps_motor_temp_l);
+  TEST_ASSERT_EQUAL(30, g_tx_struct.motor_sink_temps_heatsink_temp_r);
+  TEST_ASSERT_EQUAL(40, g_tx_struct.motor_sink_temps_motor_temp_r);
+}
+
+// DSP Board Temperature Measurement (Reserved, DSP Board Temp: C)
+TEST_IN_TASK
+void test_dsp_temp(void) {
+  push_mc_message(MOTOR_CONTROLLER_BASE_L + 0x0C, 0.1f, 0.2f);
+  push_mc_message(MOTOR_CONTROLLER_BASE_R + 0x0C, 0.3f, 0.4f);
+
+  run_motor_controller_cycle();
+
+  TEST_ASSERT_EQUAL(20, g_tx_struct.dsp_board_temps_dsp_temp_l);
+  TEST_ASSERT_EQUAL(40, g_tx_struct.dsp_board_temps_dsp_temp_r);
+}
diff --git a/projects/motor_controller/test/test_motor_can_tx.c b/projects/motor_controller/test/test_motor_can_tx.c
new file mode 100644
index 000000000..4287de6b2
--- /dev/null
+++ b/projects/motor_controller/test/test_motor_can_tx.c
@@ -0,0 +1,260 @@
+// Test motor_can tx (drive commands)
+
+#include <stdio.h>
+
+#include "can.h"
+#include "can_board_ids.h"
+#include "can_codegen.h"
+#include "can_queue.h"
+#include "log.h"
+#include "mcp2515.h"
+#include "misc.h"
+#include "motor_can.h"
+#include "task_test_helpers.h"
+#include "tasks.h"
+#include "test_helpers.h"
+#include "unity.h"
+
+static Mcp2515Storage s_mcp2515_storage = { 0 };
+static Mcp2515Settings s_mcp2515_settings = {
+  .can_settings = {
+    .mode = CAN_CONTINUOUS,
+  },
+};
+
+static CanQueue s_mcp2515_tx_queue;
+
+StatusCode TEST_MOCK(mcp2515_hw_transmit)(uint32_t id, bool extended, uint8_t *data, size_t len) {
+  CanMessage message = {
+    .id.raw = id,
+    .extended = extended,
+    .dlc = len,
+  };
+  memcpy(message.data_u8, data, len);
+  return can_queue_push(&s_mcp2515_tx_queue, &message);
+}
+
+StatusCode TEST_MOCK(mcp2515_hw_init)(const CanQueue *rx_queue, const Mcp2515Settings *settings) {
+  return STATUS_CODE_OK;
+}
+
+static uint32_t prv_get_uint32(float f) {
+  union {
+    float f;
+    uint32_t u;
+  } fu = { .f = f };
+  return fu.u;
+}
+
+// for test
+typedef enum DriveState {
+  DRIVE,
+  NEUTRAL,
+  REVERSE,
+} DriveState;
+
+static void assert_drive_command(float current_percent, float velocity_rpm) {
+  CanMessage can_message;
+  TEST_ASSERT_OK(can_queue_pop(&s_mcp2515_tx_queue, &can_message));
+
+  TEST_ASSERT_EQUAL(DRIVER_CONTROL_BASE + 0x01, can_message.id.raw);
+  TEST_ASSERT_EQUAL(prv_get_uint32(current_percent), can_message.data_u32[0]);
+  TEST_ASSERT_EQUAL(prv_get_uint32(velocity_rpm), can_message.data_u32[1]);
+}
+
+bool initialized = false;
+void setup_test(void) {
+  // set received drive_output and precharge to true, true for most test
+  g_rx_struct.received_drive_output = true;
+  g_tx_struct.mc_status_precharge_status = true;
+
+  if (initialized) {
+    return;
+  }
+  initialized = true;
+
+  tasks_init();
+  log_init();
+  mcp2515_init(&s_mcp2515_storage, &s_mcp2515_settings);
+  init_motor_controller_can();
+  can_queue_init(&s_mcp2515_tx_queue);
+}
+
+void teardown_test(void) {}
+
+void run_motor_controller_cycle() {
+  run_mcp2515_rx_cycle();
+  wait_tasks(1);
+  run_mcp2515_tx_cycle();
+  wait_tasks(1);
+}
+
+TEST_IN_TASK
+void no_drive_command_without_center_console_msg(void) {
+  g_rx_struct.received_drive_output = false;
+  g_tx_struct.mc_status_precharge_status = true;
+
+  run_motor_controller_cycle();
+
+  CanMessage can_message;
+  TEST_ASSERT_EQUAL(STATUS_CODE_EMPTY, can_queue_pop(&s_mcp2515_tx_queue, &can_message));
+}
+
+TEST_IN_TASK
+void no_drive_command_without_precharge(void) {
+  g_rx_struct.received_drive_output = true;
+  g_tx_struct.mc_status_precharge_status = false;
+
+  run_motor_controller_cycle();
+
+  CanMessage can_message;
+  TEST_ASSERT_EQUAL(STATUS_CODE_EMPTY, can_queue_pop(&s_mcp2515_tx_queue, &can_message));
+}
+
+TEST_IN_TASK
+void test_reverse(void) {
+  g_rx_struct.drive_output_drive_state = REVERSE;
+  g_rx_struct.drive_output_cruise_control = false;
+  g_rx_struct.drive_output_regen_braking = false;
+  g_rx_struct.drive_output_target_velocity = prv_get_uint32(10.0f);
+  g_rx_struct.pedal_output_brake_output = prv_get_uint32(0.0f);
+  g_rx_struct.pedal_output_throttle_output = prv_get_uint32(0.3f);
+
+  run_motor_controller_cycle();
+
+  assert_drive_command(0.3f, -TORQUE_CONTROL_VEL);
+}
+
+TEST_IN_TASK
+void test_neutral(void) {
+  g_rx_struct.drive_output_drive_state = NEUTRAL;
+  g_rx_struct.drive_output_cruise_control = false;
+  g_rx_struct.drive_output_regen_braking = false;
+  g_rx_struct.drive_output_target_velocity = prv_get_uint32(10.0f);
+  g_rx_struct.pedal_output_brake_output = prv_get_uint32(0.0f);
+  g_rx_struct.pedal_output_throttle_output = prv_get_uint32(0.5f);
+
+  run_motor_controller_cycle();
+
+  assert_drive_command(0.0f, 0.0f);
+}
+
+TEST_IN_TASK
+void test_neutral_no_pedal(void) {
+  g_rx_struct.drive_output_drive_state = DRIVE;
+  g_rx_struct.drive_output_cruise_control = false;
+  g_rx_struct.drive_output_regen_braking = false;
+  g_rx_struct.drive_output_target_velocity = prv_get_uint32(10.0f);
+  g_rx_struct.pedal_output_brake_output = prv_get_uint32(0.0f);
+  g_rx_struct.pedal_output_throttle_output = prv_get_uint32(0.0f);
+
+  run_motor_controller_cycle();
+
+  assert_drive_command(0.0f, 0.0f);
+}
+
+TEST_IN_TASK
+void test_brake(void) {
+  g_rx_struct.drive_output_drive_state = REVERSE;
+  g_rx_struct.drive_output_cruise_control = false;
+  g_rx_struct.drive_output_regen_braking = false;
+  g_rx_struct.drive_output_target_velocity = prv_get_uint32(0.0f);
+  g_rx_struct.pedal_output_brake_output = prv_get_uint32(0.5f);
+  g_rx_struct.pedal_output_throttle_output = prv_get_uint32(0.2f);
+
+  run_motor_controller_cycle();
+
+  assert_drive_command(0.0f, 0.0f);
+}
+
+TEST_IN_TASK
+void test_regen_brake(void) {
+  g_rx_struct.drive_output_drive_state = DRIVE;
+  g_rx_struct.drive_output_cruise_control = false;
+  g_rx_struct.drive_output_regen_braking = true;
+  g_rx_struct.drive_output_target_velocity = prv_get_uint32(10.0f);
+  g_rx_struct.pedal_output_brake_output = prv_get_uint32(0.5f);
+  g_rx_struct.pedal_output_throttle_output = prv_get_uint32(0.9f);
+
+  run_motor_controller_cycle();
+
+  assert_drive_command(1.0f, 0.0f);
+}
+
+TEST_IN_TASK
+void test_drive(void) {
+  g_rx_struct.drive_output_cruise_control = false;
+  g_rx_struct.drive_output_regen_braking = true;
+  g_rx_struct.drive_output_target_velocity = prv_get_uint32(0.0f);
+  g_rx_struct.pedal_output_brake_output = prv_get_uint32(0.0f);
+  g_rx_struct.pedal_output_throttle_output = prv_get_uint32(0.3f);
+
+  run_motor_controller_cycle();
+
+  assert_drive_command(0.3f, TORQUE_CONTROL_VEL);
+}
+
+TEST_IN_TASK
+void test_cruise_drive(void) {
+  g_rx_struct.drive_output_cruise_control = true;
+  g_rx_struct.drive_output_regen_braking = true;
+  g_rx_struct.drive_output_target_velocity = prv_get_uint32(10.0f);
+  g_rx_struct.pedal_output_brake_output = prv_get_uint32(0.0f);
+  g_rx_struct.pedal_output_throttle_output = prv_get_uint32(0.0f);
+
+  run_motor_controller_cycle();
+
+  assert_drive_command(1.0f, 10.0f * VEL_TO_RPM_RATIO);
+}
+
+TEST_IN_TASK
+void test_cruise_braking(void) {
+  g_rx_struct.drive_output_cruise_control = true;
+  g_rx_struct.drive_output_regen_braking = false;
+  g_rx_struct.drive_output_target_velocity = prv_get_uint32(10.0f);
+  g_rx_struct.pedal_output_brake_output = prv_get_uint32(0.1f);
+  g_rx_struct.pedal_output_throttle_output = prv_get_uint32(0.0f);
+
+  run_motor_controller_cycle();
+
+  assert_drive_command(0.0f, 0.0f);
+}
+
+TEST_IN_TASK
+void test_cruise_regen_braking(void) {
+  g_rx_struct.drive_output_cruise_control = true;
+  g_rx_struct.drive_output_regen_braking = true;
+  g_rx_struct.drive_output_target_velocity = prv_get_uint32(10.0f);
+  g_rx_struct.pedal_output_brake_output = prv_get_uint32(0.1f);
+  g_rx_struct.pedal_output_throttle_output = prv_get_uint32(0.0f);
+
+  run_motor_controller_cycle();
+
+  assert_drive_command(1.0f, 0.0f);
+}
+
+TEST_IN_TASK
+void test_cruise_throttle(void) {
+  g_rx_struct.drive_output_cruise_control = true;
+  g_rx_struct.drive_output_regen_braking = true;
+  g_rx_struct.drive_output_target_velocity = prv_get_uint32(10.0f);
+  g_rx_struct.pedal_output_brake_output = prv_get_uint32(0.0f);
+  g_rx_struct.pedal_output_throttle_output = prv_get_uint32(0.3f);
+
+  run_motor_controller_cycle();
+
+  assert_drive_command(0.3f, TORQUE_CONTROL_VEL);
+}
+
+TEST_IN_TASK
+void test_cruise_throttle_and_brake(void) {
+  g_rx_struct.drive_output_cruise_control = true;
+  g_rx_struct.drive_output_regen_braking = true;
+  g_rx_struct.drive_output_target_velocity = prv_get_uint32(10.0f);
+  g_rx_struct.pedal_output_brake_output = prv_get_uint32(0.3f);
+  g_rx_struct.pedal_output_throttle_output = prv_get_uint32(0.3f);
+
+  run_motor_controller_cycle();
+
+  assert_drive_command(1.0f, 0);
+}
diff --git a/projects/motor_controller/test/test_precharge.c b/projects/motor_controller/test/test_precharge.c
new file mode 100644
index 000000000..64b125b1d
--- /dev/null
+++ b/projects/motor_controller/test/test_precharge.c
@@ -0,0 +1,66 @@
+#include <stdio.h>
+
+#include "can.h"
+#include "can_board_ids.h"
+#include "can_codegen.h"
+#include "delay.h"
+#include "fsm.h"
+#include "gpio_it.h"
+#include "interrupt.h"
+#include "log.h"
+#include "mcp2515.h"
+#include "misc.h"
+#include "motor_can.h"
+#include "motor_controller_setters.h"
+#include "precharge.h"
+#include "soft_timer.h"
+#include "task_test_helpers.h"
+#include "tasks.h"
+#include "test_helpers.h"
+#include "unity.h"
+
+#define PRECHARGE_EVENT 0
+
+static PrechargeSettings precharge_settings = {
+  .precharge_control = { GPIO_PORT_A, 9 },
+  .precharge_monitor = { GPIO_PORT_B, 0 },
+};
+
+bool initialized = false;
+
+void setup_test(void) {
+  if (initialized) return;
+  initialized = true;
+
+  interrupt_init();
+  gpio_it_init();
+  tasks_init();
+  log_init();
+  precharge_init(&precharge_settings, PRECHARGE_EVENT, test_task);
+}
+
+void teardown_test(void) {}
+
+// from main.c
+void run_fast_cycle() {
+  uint32_t notification;
+  notify_get(&notification);
+  if (notification & (1 << PRECHARGE_EVENT)) {
+    LOG_DEBUG("Precharge complete\n");
+    set_mc_status_precharge_status(true);
+  }
+}
+
+TEST_IN_TASK
+void test_precharge(void) {
+  // expect to have tx struct mc_status_precharge_status to be false on startup
+  TEST_ASSERT_FALSE(g_tx_struct.mc_status_precharge_status);
+
+  gpio_it_trigger_interrupt(&precharge_settings.precharge_monitor);
+
+  // run cycle once
+  run_fast_cycle();
+
+  // expect to have tx struct mc_status_precharge_status set to true
+  TEST_ASSERT_TRUE(g_tx_struct.mc_status_precharge_status);
+}
diff --git a/py/can_a/__init__.py b/py/can_a/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/py/can_a/can_a.py b/py/can_a/can_a.py
new file mode 100644
index 000000000..4efd21ff9
--- /dev/null
+++ b/py/can_a/can_a.py
@@ -0,0 +1,519 @@
+import struct
+import subprocess
+
+# Board and Message ids
+SYSTEM_CAN_DEVICE_BABYDRIVER = 0
+SYSTEM_CAN_DEVICE_BMS_CARRIER = 1
+SYSTEM_CAN_DEVICE_CAN_COMMUNICATION = 2
+SYSTEM_CAN_DEVICE_CAN_DEBUG = 3
+SYSTEM_CAN_DEVICE_CENTRE_CONSOLE = 4
+SYSTEM_CAN_DEVICE_CHARGER = 5
+SYSTEM_CAN_DEVICE_MOTOR_CONTROLLER = 6
+SYSTEM_CAN_DEVICE_NEW_CAN = 7
+SYSTEM_CAN_DEVICE_PEDAL = 8
+SYSTEM_CAN_DEVICE_POWER_DISTRIBUTION = 9
+SYSTEM_CAN_DEVICE_POWER_SELECT = 10
+SYSTEM_CAN_DEVICE_SOLAR_SENSE = 11
+SYSTEM_CAN_DEVICE_STEERING = 12
+SYSTEM_CAN_DEVICE_TELEMETRY = 13
+SYSTEM_CAN_DEVICE_UV_CUTOFF = 14
+NUM_SYSTEM_CAN_DEVICES = 15
+
+SYSTEM_CAN_MESSAGE_BABYDRIVER_BABYDRIVER = (63 << 5) + SYSTEM_CAN_DEVICE_BABYDRIVER
+SYSTEM_CAN_MESSAGE_BMS_CARRIER_BATTERY_STATUS = (0 << 5) + SYSTEM_CAN_DEVICE_BMS_CARRIER
+SYSTEM_CAN_MESSAGE_BMS_CARRIER_BATTERY_VT = (30 << 5) + SYSTEM_CAN_DEVICE_BMS_CARRIER
+SYSTEM_CAN_MESSAGE_BMS_CARRIER_BATTERY_AGGREGATE_VC = (33 << 5) + SYSTEM_CAN_DEVICE_BMS_CARRIER
+SYSTEM_CAN_MESSAGE_BMS_CARRIER_BATTERY_FAN_STATE = (57 << 5) + SYSTEM_CAN_DEVICE_BMS_CARRIER
+SYSTEM_CAN_MESSAGE_BMS_CARRIER_BATTERY_RELAY_STATE = (58 << 5) + SYSTEM_CAN_DEVICE_BMS_CARRIER
+SYSTEM_CAN_MESSAGE_CAN_COMMUNICATION_ONE_SHOT_MSG = (0 << 5) + SYSTEM_CAN_DEVICE_CAN_COMMUNICATION
+SYSTEM_CAN_MESSAGE_CAN_DEBUG_TEST_DEBUG = (0 << 5) + SYSTEM_CAN_DEVICE_CAN_DEBUG
+SYSTEM_CAN_MESSAGE_CENTRE_CONSOLE_CC_POWER_CONTROL = (1 << 5) + SYSTEM_CAN_DEVICE_CENTRE_CONSOLE
+SYSTEM_CAN_MESSAGE_CENTRE_CONSOLE_DRIVE_OUTPUT = (9 << 5) + SYSTEM_CAN_DEVICE_CENTRE_CONSOLE
+SYSTEM_CAN_MESSAGE_CHARGER_REQUEST_TO_CHARGE = (48 << 5) + SYSTEM_CAN_DEVICE_CHARGER
+SYSTEM_CAN_MESSAGE_CHARGER_CHARGER_CONNECTED_STATE = (50 << 5) + SYSTEM_CAN_DEVICE_CHARGER
+SYSTEM_CAN_MESSAGE_CHARGER_CHARGER_FAULT = (53 << 5) + SYSTEM_CAN_DEVICE_CHARGER
+SYSTEM_CAN_MESSAGE_MOTOR_CONTROLLER_MOTOR_CONTROLLER_VC = (
+    35 << 5) + SYSTEM_CAN_DEVICE_MOTOR_CONTROLLER
+SYSTEM_CAN_MESSAGE_MOTOR_CONTROLLER_MOTOR_VELOCITY = (36 << 5) + SYSTEM_CAN_DEVICE_MOTOR_CONTROLLER
+SYSTEM_CAN_MESSAGE_MOTOR_CONTROLLER_MOTOR_STATUS = (40 << 5) + SYSTEM_CAN_DEVICE_MOTOR_CONTROLLER
+SYSTEM_CAN_MESSAGE_MOTOR_CONTROLLER_MOTOR_SINK_TEMPS = (
+    38 << 5) + SYSTEM_CAN_DEVICE_MOTOR_CONTROLLER
+SYSTEM_CAN_MESSAGE_MOTOR_CONTROLLER_DSP_BOARD_TEMPS = (39 << 5) + SYSTEM_CAN_DEVICE_MOTOR_CONTROLLER
+SYSTEM_CAN_MESSAGE_MOTOR_CONTROLLER_MC_STATUS = (40 << 5) + SYSTEM_CAN_DEVICE_MOTOR_CONTROLLER
+SYSTEM_CAN_MESSAGE_MOTOR_CONTROLLER_PRECHARGE_COMPLETED = (
+    16 << 5) + SYSTEM_CAN_DEVICE_MOTOR_CONTROLLER
+SYSTEM_CAN_MESSAGE_NEW_CAN_TRANSMIT_MSG1 = (0 << 5) + SYSTEM_CAN_DEVICE_NEW_CAN
+SYSTEM_CAN_MESSAGE_NEW_CAN_TRANSMIT_MSG2 = (1 << 5) + SYSTEM_CAN_DEVICE_NEW_CAN
+SYSTEM_CAN_MESSAGE_NEW_CAN_TRANSMIT_MSG3 = (2 << 5) + SYSTEM_CAN_DEVICE_NEW_CAN
+SYSTEM_CAN_MESSAGE_PEDAL_PEDAL_OUTPUT = (18 << 5) + SYSTEM_CAN_DEVICE_PEDAL
+SYSTEM_CAN_MESSAGE_POWER_DISTRIBUTION_UV_CUTOFF_NOTIFICATION = (
+    45 << 5) + SYSTEM_CAN_DEVICE_POWER_DISTRIBUTION
+SYSTEM_CAN_MESSAGE_POWER_DISTRIBUTION_CURRENT_MEASUREMENT = (
+    54 << 5) + SYSTEM_CAN_DEVICE_POWER_DISTRIBUTION
+SYSTEM_CAN_MESSAGE_POWER_DISTRIBUTION_PD_FAULT = (62 << 5) + SYSTEM_CAN_DEVICE_POWER_DISTRIBUTION
+SYSTEM_CAN_MESSAGE_POWER_DISTRIBUTION_HORN_AND_LIGHTS = (
+    46 << 5) + SYSTEM_CAN_DEVICE_POWER_DISTRIBUTION
+SYSTEM_CAN_MESSAGE_POWER_DISTRIBUTION_LIGHTS_SYNC = (23 << 5) + SYSTEM_CAN_DEVICE_POWER_DISTRIBUTION
+SYSTEM_CAN_MESSAGE_POWER_DISTRIBUTION_POWER_INFO = (3 << 5) + SYSTEM_CAN_DEVICE_POWER_DISTRIBUTION
+SYSTEM_CAN_MESSAGE_POWER_SELECT_POWER_SELECT_STATUS = (1 << 5) + SYSTEM_CAN_DEVICE_POWER_SELECT
+SYSTEM_CAN_MESSAGE_POWER_SELECT_POWER_SELECT_AUX_MEASUREMENTS = (
+    2 << 5) + SYSTEM_CAN_DEVICE_POWER_SELECT
+SYSTEM_CAN_MESSAGE_POWER_SELECT_POWER_SELECT_DCDC_MEASUREMENTS = (
+    3 << 5) + SYSTEM_CAN_DEVICE_POWER_SELECT
+SYSTEM_CAN_MESSAGE_SOLAR_SENSE_MPPT_1 = (21 << 5) + SYSTEM_CAN_DEVICE_SOLAR_SENSE
+SYSTEM_CAN_MESSAGE_SOLAR_SENSE_MPPT_2 = (22 << 5) + SYSTEM_CAN_DEVICE_SOLAR_SENSE
+SYSTEM_CAN_MESSAGE_SOLAR_SENSE_MPPT_3 = (23 << 5) + SYSTEM_CAN_DEVICE_SOLAR_SENSE
+SYSTEM_CAN_MESSAGE_SOLAR_SENSE_MPPT_4 = (24 << 5) + SYSTEM_CAN_DEVICE_SOLAR_SENSE
+SYSTEM_CAN_MESSAGE_SOLAR_SENSE_MPPT_5 = (25 << 5) + SYSTEM_CAN_DEVICE_SOLAR_SENSE
+SYSTEM_CAN_MESSAGE_SOLAR_SENSE_MPPT_6 = (20 << 5) + SYSTEM_CAN_DEVICE_SOLAR_SENSE
+SYSTEM_CAN_MESSAGE_SOLAR_SENSE_CURRENT_SENSE = (26 << 5) + SYSTEM_CAN_DEVICE_SOLAR_SENSE
+SYSTEM_CAN_MESSAGE_SOLAR_SENSE_THERMAL_STATUS = (27 << 5) + SYSTEM_CAN_DEVICE_SOLAR_SENSE
+SYSTEM_CAN_MESSAGE_SOLAR_SENSE_THERMAL_TEMPS = (28 << 5) + SYSTEM_CAN_DEVICE_SOLAR_SENSE
+SYSTEM_CAN_MESSAGE_SOLAR_SENSE_SOLAR_INFO = (29 << 5) + SYSTEM_CAN_DEVICE_SOLAR_SENSE
+SYSTEM_CAN_MESSAGE_STEERING_STEERING_INFO = (21 << 5) + SYSTEM_CAN_DEVICE_STEERING
+SYSTEM_CAN_MESSAGE_TELEMETRY_TEST_MSG = (0 << 5) + SYSTEM_CAN_DEVICE_TELEMETRY
+SYSTEM_CAN_MESSAGE_UV_CUTOFF_UV_CUTOFF_NOTIFICATION1 = (45 << 5) + SYSTEM_CAN_DEVICE_UV_CUTOFF
+# Send single message
+
+import can
+
+bus = can.Bus(interface='socketcan', channel='can0', bitrate=50000)
+
+def send_message(id, data):
+    # id: int, data: str
+    # cmd = f"cansend vcan0 {id:0>3x}#{data}"
+    # print(cmd)
+    # msg = can.Message(arbitration_id=id, data=data)
+    # bus.send(msg)
+    subprocess.run(["cansend", "can0", f"{id:0>3x}#{data}"])
+
+
+def pack(num, size):
+    if isinstance(num, float) and (size == 32):
+        return struct.pack("<f", num).hex()
+    elif (size == 32):
+        return struct.pack("i", num).hex()
+    elif (size == 16):
+        return struct.pack("h", num).hex()
+    elif (size == 8):
+        return struct.pack("b", num).hex()
+
+
+def send_babydriver_babydriver(id, data0, data1, data2, data3, data4, data5, data6):
+    send_message(
+        SYSTEM_CAN_MESSAGE_BABYDRIVER_BABYDRIVER,
+        pack(
+            id,
+            8) +
+        pack(
+            data0,
+            8) +
+        pack(
+            data1,
+            8) +
+        pack(
+            data2,
+            8) +
+        pack(
+            data3,
+            8) +
+        pack(
+            data4,
+            8) +
+        pack(
+            data5,
+            8) +
+        pack(
+            data6,
+            8))
+
+
+def send_bms_carrier_battery_status(batt_perc, status, fault):
+    send_message(SYSTEM_CAN_MESSAGE_BMS_CARRIER_BATTERY_STATUS,
+                 pack(batt_perc, 16) + pack(status, 8) + pack(fault, 8))
+
+
+def send_bms_carrier_battery_vt(module_id, voltage, temperature):
+    send_message(SYSTEM_CAN_MESSAGE_BMS_CARRIER_BATTERY_VT, pack(
+        module_id, 16) + pack(voltage, 16) + pack(temperature, 16))
+
+
+def send_bms_carrier_battery_aggregate_vc(voltage, current):
+    send_message(
+        SYSTEM_CAN_MESSAGE_BMS_CARRIER_BATTERY_AGGREGATE_VC,
+        pack(
+            voltage,
+            32) +
+        pack(
+            current,
+            32))
+
+
+def send_bms_carrier_battery_fan_state(fan_1, fan_2, fan_3, fan_4, fan_5, fan_6, fan_7, fan_8):
+    send_message(
+        SYSTEM_CAN_MESSAGE_BMS_CARRIER_BATTERY_FAN_STATE,
+        pack(
+            fan_1,
+            8) +
+        pack(
+            fan_2,
+            8) +
+        pack(
+            fan_3,
+            8) +
+        pack(
+            fan_4,
+            8) +
+        pack(
+            fan_5,
+            8) +
+        pack(
+            fan_6,
+            8) +
+        pack(
+            fan_7,
+            8) +
+        pack(
+            fan_8,
+            8))
+
+
+def send_bms_carrier_battery_relay_state(hv, gnd):
+    send_message(SYSTEM_CAN_MESSAGE_BMS_CARRIER_BATTERY_RELAY_STATE, pack(hv, 8) + pack(gnd, 8))
+
+
+def send_can_communication_one_shot_msg(sig1, sig2):
+    send_message(SYSTEM_CAN_MESSAGE_CAN_COMMUNICATION_ONE_SHOT_MSG, pack(sig1, 16) + pack(sig2, 16))
+
+
+def send_can_debug_test_debug(operation, operandA, operandB, operandC):
+    send_message(SYSTEM_CAN_MESSAGE_CAN_DEBUG_TEST_DEBUG, pack(operation, 8) +
+                 pack(operandA, 8) + pack(operandB, 8) + pack(operandC, 8))
+
+
+def send_centre_console_cc_power_control(power_event, hazard_enabled):
+    send_message(SYSTEM_CAN_MESSAGE_CENTRE_CONSOLE_CC_POWER_CONTROL,
+                 pack(power_event, 8) + pack(hazard_enabled, 8))
+
+
+def send_centre_console_drive_output(
+        target_velocity, drive_state, cruise_control, regen_braking, precharge):
+    send_message(
+        SYSTEM_CAN_MESSAGE_CENTRE_CONSOLE_DRIVE_OUTPUT,
+        pack(
+            target_velocity,
+            32) +
+        pack(
+            drive_state,
+            8) +
+        pack(
+            cruise_control,
+            8) +
+        pack(
+            regen_braking,
+            8) +
+        pack(
+            precharge,
+            8))
+
+
+def send_charger_request_to_charge(signal1):
+    send_message(SYSTEM_CAN_MESSAGE_CHARGER_REQUEST_TO_CHARGE, pack(signal1, 8))
+
+
+def send_charger_charger_connected_state(is_connected):
+    send_message(SYSTEM_CAN_MESSAGE_CHARGER_CHARGER_CONNECTED_STATE, pack(is_connected, 8))
+
+
+def send_charger_charger_fault(fault):
+    send_message(SYSTEM_CAN_MESSAGE_CHARGER_CHARGER_FAULT, pack(fault, 8))
+
+
+def send_motor_controller_motor_controller_vc(
+        mc_voltage_l, mc_current_l, mc_voltage_r, mc_current_r):
+    send_message(
+        SYSTEM_CAN_MESSAGE_MOTOR_CONTROLLER_MOTOR_CONTROLLER_VC,
+        pack(
+            mc_voltage_l,
+            16) +
+        pack(
+            mc_current_l,
+            16) +
+        pack(
+            mc_voltage_r,
+            16) +
+        pack(
+            mc_current_r,
+            16))
+
+
+def send_motor_controller_motor_velocity(velocity_l, velocity_r):
+    send_message(SYSTEM_CAN_MESSAGE_MOTOR_CONTROLLER_MOTOR_VELOCITY,
+                 pack(velocity_l, 16) + pack(velocity_r, 16))
+
+
+def send_motor_controller_motor_status(motor_status_l, motor_status_r):
+    send_message(SYSTEM_CAN_MESSAGE_MOTOR_CONTROLLER_MOTOR_STATUS,
+                 pack(motor_status_l, 32) + pack(motor_status_r, 32))
+
+
+def send_motor_controller_motor_sink_temps(
+        motor_temp_l, heatsink_temp_l, motor_temp_r, heatsink_temp_r):
+    send_message(
+        SYSTEM_CAN_MESSAGE_MOTOR_CONTROLLER_MOTOR_SINK_TEMPS,
+        pack(
+            motor_temp_l,
+            16) +
+        pack(
+            heatsink_temp_l,
+            16) +
+        pack(
+            motor_temp_r,
+            16) +
+        pack(
+            heatsink_temp_r,
+            16))
+
+
+def send_motor_controller_dsp_board_temps(dsp_temp_l, dsp_temp_r):
+    send_message(SYSTEM_CAN_MESSAGE_MOTOR_CONTROLLER_DSP_BOARD_TEMPS,
+                 pack(dsp_temp_l, 32) + pack(dsp_temp_r, 32))
+
+
+def send_motor_controller_mc_status(limit_bitset_l, error_bitset_l, limit_bitset_r,
+                                    error_bitset_r, board_fault_bitset, overtemp_bitset, precharge_status):
+    send_message(
+        SYSTEM_CAN_MESSAGE_MOTOR_CONTROLLER_MC_STATUS,
+        pack(
+            limit_bitset_l,
+            8) +
+        pack(
+            error_bitset_l,
+            8) +
+        pack(
+            limit_bitset_r,
+            8) +
+        pack(
+            error_bitset_r,
+            8) +
+        pack(
+            board_fault_bitset,
+            8) +
+        pack(
+            overtemp_bitset,
+            8) +
+        pack(
+            precharge_status,
+            8))
+
+
+def send_motor_controller_precharge_completed(notification):
+    send_message(SYSTEM_CAN_MESSAGE_MOTOR_CONTROLLER_PRECHARGE_COMPLETED, pack(notification, 8))
+
+
+def send_new_can_transmit_msg1(status):
+    send_message(SYSTEM_CAN_MESSAGE_NEW_CAN_TRANSMIT_MSG1, pack(status, 8))
+
+
+def send_new_can_transmit_msg2(signal, signal2):
+    send_message(SYSTEM_CAN_MESSAGE_NEW_CAN_TRANSMIT_MSG2, pack(signal, 8) + pack(signal2, 8))
+
+
+def send_new_can_transmit_msg3(help):
+    send_message(SYSTEM_CAN_MESSAGE_NEW_CAN_TRANSMIT_MSG3, pack(help, 8))
+
+
+def send_pedal_pedal_output(throttle_output, brake_output):
+    send_message(
+        SYSTEM_CAN_MESSAGE_PEDAL_PEDAL_OUTPUT,
+        pack(
+            throttle_output,
+            32) +
+        pack(
+            brake_output,
+            32))
+
+
+def send_power_distribution_uv_cutoff_notification(signal1):
+    send_message(SYSTEM_CAN_MESSAGE_POWER_DISTRIBUTION_UV_CUTOFF_NOTIFICATION, pack(signal1, 8))
+
+
+def send_power_distribution_current_measurement(current_id, current):
+    send_message(SYSTEM_CAN_MESSAGE_POWER_DISTRIBUTION_CURRENT_MEASUREMENT,
+                 pack(current_id, 16) + pack(current, 16))
+
+
+def send_power_distribution_pd_fault(fault_data, faulting_output):
+    send_message(SYSTEM_CAN_MESSAGE_POWER_DISTRIBUTION_PD_FAULT,
+                 pack(fault_data, 16) + pack(faulting_output, 16))
+
+
+def send_power_distribution_horn_and_lights(horn_state, lights_state):
+    send_message(SYSTEM_CAN_MESSAGE_POWER_DISTRIBUTION_HORN_AND_LIGHTS,
+                 pack(horn_state, 8) + pack(lights_state, 8))
+
+
+def send_power_distribution_lights_sync(signal1):
+    send_message(SYSTEM_CAN_MESSAGE_POWER_DISTRIBUTION_LIGHTS_SYNC, pack(signal1, 8))
+
+
+def send_power_distribution_power_info(power_state, pd_fault):
+    send_message(SYSTEM_CAN_MESSAGE_POWER_DISTRIBUTION_POWER_INFO,
+                 pack(power_state, 8) + pack(pd_fault, 8))
+
+
+def send_power_select_power_select_status(status, fault):
+    send_message(
+        SYSTEM_CAN_MESSAGE_POWER_SELECT_POWER_SELECT_STATUS,
+        pack(
+            status,
+            8) +
+        pack(
+            fault,
+            8))
+
+
+def send_power_select_power_select_aux_measurements(
+        aux_voltage, aux_current, aux_temp, power_supply_current):
+    send_message(SYSTEM_CAN_MESSAGE_POWER_SELECT_POWER_SELECT_AUX_MEASUREMENTS, pack(
+        aux_voltage, 16) + pack(aux_current, 16) + pack(aux_temp, 16) + pack(power_supply_current, 16))
+
+
+def send_power_select_power_select_dcdc_measurements(
+        dcdc_voltage, dcdc_current, dcdc_temp, power_supply_voltage):
+    send_message(SYSTEM_CAN_MESSAGE_POWER_SELECT_POWER_SELECT_DCDC_MEASUREMENTS, pack(
+        dcdc_voltage, 16) + pack(dcdc_current, 16) + pack(dcdc_temp, 16) + pack(power_supply_voltage, 16))
+
+
+def send_solar_sense_mppt_1(current, voltage, pwm, status):
+    send_message(
+        SYSTEM_CAN_MESSAGE_SOLAR_SENSE_MPPT_1,
+        pack(
+            current,
+            16) +
+        pack(
+            voltage,
+            16) +
+        pack(
+            pwm,
+            16) +
+        pack(
+            status,
+            16))
+
+
+def send_solar_sense_mppt_2(current, voltage, pwm, status):
+    send_message(
+        SYSTEM_CAN_MESSAGE_SOLAR_SENSE_MPPT_2,
+        pack(
+            current,
+            16) +
+        pack(
+            voltage,
+            16) +
+        pack(
+            pwm,
+            16) +
+        pack(
+            status,
+            16))
+
+
+def send_solar_sense_mppt_3(current, voltage, pwm, status):
+    send_message(
+        SYSTEM_CAN_MESSAGE_SOLAR_SENSE_MPPT_3,
+        pack(
+            current,
+            16) +
+        pack(
+            voltage,
+            16) +
+        pack(
+            pwm,
+            16) +
+        pack(
+            status,
+            16))
+
+
+def send_solar_sense_mppt_4(current, voltage, pwm, status):
+    send_message(
+        SYSTEM_CAN_MESSAGE_SOLAR_SENSE_MPPT_4,
+        pack(
+            current,
+            16) +
+        pack(
+            voltage,
+            16) +
+        pack(
+            pwm,
+            16) +
+        pack(
+            status,
+            16))
+
+
+def send_solar_sense_mppt_5(current, voltage, pwm, status):
+    send_message(
+        SYSTEM_CAN_MESSAGE_SOLAR_SENSE_MPPT_5,
+        pack(
+            current,
+            16) +
+        pack(
+            voltage,
+            16) +
+        pack(
+            pwm,
+            16) +
+        pack(
+            status,
+            16))
+
+
+def send_solar_sense_mppt_6(current, voltage, pwm, status):
+    send_message(
+        SYSTEM_CAN_MESSAGE_SOLAR_SENSE_MPPT_6,
+        pack(
+            current,
+            16) +
+        pack(
+            voltage,
+            16) +
+        pack(
+            pwm,
+            16) +
+        pack(
+            status,
+            16))
+
+
+def send_solar_sense_current_sense(current, voltage, relay_status):
+    send_message(SYSTEM_CAN_MESSAGE_SOLAR_SENSE_CURRENT_SENSE, pack(
+        current, 16) + pack(voltage, 16) + pack(relay_status, 8))
+
+
+def send_solar_sense_thermal_status(overtemp, fullspeed, fan_fail, temp_1, temp_2):
+    send_message(SYSTEM_CAN_MESSAGE_SOLAR_SENSE_THERMAL_STATUS, pack(overtemp, 8) +
+                 pack(fullspeed, 8) + pack(fan_fail, 8) + pack(temp_1, 16) + pack(temp_2, 16))
+
+
+def send_solar_sense_thermal_temps(temp_3, temp_4, temp_5, temp_6):
+    send_message(SYSTEM_CAN_MESSAGE_SOLAR_SENSE_THERMAL_TEMPS, pack(
+        temp_3, 16) + pack(temp_4, 16) + pack(temp_5, 16) + pack(temp_6, 16))
+
+
+def send_solar_sense_solar_info(solar_fault):
+    send_message(SYSTEM_CAN_MESSAGE_SOLAR_SENSE_SOLAR_INFO, pack(solar_fault, 8))
+
+
+def send_steering_steering_info(input_cc, input_lights):
+    send_message(SYSTEM_CAN_MESSAGE_STEERING_STEERING_INFO,
+                 pack(input_cc, 8) + pack(input_lights, 8))
+
+
+def send_telemetry_test_msg(test):
+    send_message(SYSTEM_CAN_MESSAGE_TELEMETRY_TEST_MSG, pack(test, 8))
+
+
+def send_uv_cutoff_uv_cutoff_notification1(signal1):
+    send_message(SYSTEM_CAN_MESSAGE_UV_CUTOFF_UV_CUTOFF_NOTIFICATION1, pack(signal1, 8))
diff --git a/py/can_a/config.json b/py/can_a/config.json
new file mode 100644
index 000000000..29c8f9193
--- /dev/null
+++ b/py/can_a/config.json
@@ -0,0 +1,3 @@
+{
+    "no_lint": true
+}
\ No newline at end of file
diff --git a/py/can_a/util.py b/py/can_a/util.py
new file mode 100644
index 000000000..277962e43
--- /dev/null
+++ b/py/can_a/util.py
@@ -0,0 +1,26 @@
+import re
+
+CANDUMP_PATTERN = re.compile(r"(\w*)  (\w*)   \[([0-9])\]  ((?:\w\w ?)+)")
+
+
+def set_output(output):
+    '''set the terminal output to the output string'''
+    # 'static' variable
+    if "output" not in set_output.__dict__:
+        set_output.output = ""
+    # print("OUTPUTS SET")
+    if output != set_output.output:
+        print("\033c", end="")
+        print(output)
+        set_output.output = output
+
+
+def parse_line(line):
+    '''get the can id and data from a candump line'''
+    _, can_id, length, data = CANDUMP_PATTERN.match(line).groups()
+    # if (match == N)
+    can_id = int(can_id, base=16)
+    length = int(length)
+    data = bytes.fromhex(data)
+
+    return can_id, data
diff --git a/py/motor_controller/__init__.py b/py/motor_controller/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/py/motor_controller/main.py b/py/motor_controller/main.py
index 728c54261..61ab82196 100644
--- a/py/motor_controller/main.py
+++ b/py/motor_controller/main.py
@@ -7,18 +7,17 @@
 import struct
 import sys
 from collections import defaultdict
-from can.messages import can
-from can.util import set_output, parse_line
+from can_a import can_a
+from can_a.util import set_output, parse_line
+import threading
 
-DRIVE_STATE_STR = ["NEUTRAL", "DRIVE", "REVERSE"]
+DRIVE_STATE_STR = ["DRIVE", "NEUTRAL", "REVERSE"]
+CAN = "can0"
 
-
-def handle_stdin():
+def handle_stdin(state):
     '''handle stdin input, updates velocity, drive state, etc.'''
-    state = {}
-    line = sys.stdin.readline().strip()
-    if line:
-        # output_str = ""
+    while True:
+        line = sys.stdin.readline().strip()
         try:
             cmd, val = line.split(" ")
             if cmd == "vel":
@@ -29,15 +28,23 @@ def handle_stdin():
                 state["throttle"] = float(val)
             elif cmd == "brake":
                 state["brake"] = float(val)
+            elif cmd == "cruise":
+                state["cruise"] = int(val)
+            elif cmd == "regen":
+                state["regen"] = int(val)
+            elif cmd == "precharge":
+                state["precharge"] = int(val)
+            
         except BaseException:
             pass
-    return state
 
 
 def update_display(state, rx):
     '''update terminal output'''
     set_output(f"""
-state: {DRIVE_STATE_STR[state['state']]}, vel: {state['vel']:>5}, throttle: {state['throttle']}, brake: {state['brake']}
+state:    {DRIVE_STATE_STR[state['state']]}, vel: {state['vel']:>5}, 
+cruise:   {state['cruise']}, regen: {state["regen"]}, precharge: {state["precharge"]}
+throttle: {state['throttle']}, brake: {state['brake']}
             left      right
 velocity:    {rx["vel_l"]:<8d}  {rx["vel_r"]:<8d}
 voltage:     {rx["vol_l"]:<8d}  {rx["vol_r"]:<8d}
@@ -52,76 +59,67 @@ def update_display(state, rx):
 """)
 
 
-def rx_all(proc):
+def handle_rx(rx):
     '''rx all msgs from can'''
-    rx = {}
-    line = proc.stdout.readline().decode().strip()
-    while line:
-        can_id, data = parse_line(line)
-
-        if can_id == can.SYSTEM_CAN_MESSAGE_MOTOR_CONTROLLER_MOTOR_CONTROLLER_VC:
-            rx["vol_l"] = struct.unpack("h", data[0:2])[0]
-            rx["cur_l"] = struct.unpack("h", data[2:4])[0]
-            rx["vol_r"] = struct.unpack("h", data[4:6])[0]
-            rx["cur_r"] = struct.unpack("h", data[6:8])[0]
-        elif can_id == can.SYSTEM_CAN_MESSAGE_MOTOR_CONTROLLER_MOTOR_VELOCITY:
-            rx["vel_l"] = struct.unpack("h", data[0:2])[0]
-            rx["vel_r"] = struct.unpack("h", data[2:4])[0]
-        elif can_id == can.SYSTEM_CAN_MESSAGE_MOTOR_CONTROLLER_MOTOR_STATUS:
-            rx["mts_l"] = struct.unpack("i", data[0:4])[0]
-            rx["mts_r"] = struct.unpack("i", data[4:8])[0]
-        elif can_id == can.SYSTEM_CAN_MESSAGE_MOTOR_CONTROLLER_MOTOR_SINK_TEMPS:
-            rx["mtt_l"] = struct.unpack("h", data[0:2])[0]
-            rx["hst_l"] = struct.unpack("h", data[2:4])[0]
-            rx["mtt_r"] = struct.unpack("h", data[4:6])[0]
-            rx["hst_r"] = struct.unpack("h", data[6:8])[0]
-        elif can_id == can.SYSTEM_CAN_MESSAGE_MOTOR_CONTROLLER_DSP_BOARD_TEMPS:
-            rx["dsp_l"] = struct.unpack("i", data[0:4])[0]
-            rx["dsp_r"] = struct.unpack("i", data[4:8])[0]
-        elif can_id == can.SYSTEM_CAN_MESSAGE_MOTOR_CONTROLLER_MC_STATUS:
-            rx["lbs_l"] = int(data[0])
-            rx["ebs_l"] = int(data[1])
-            rx["lbs_r"] = int(data[2])
-            rx["ebs_r"] = int(data[3])
-            rx["bfalt"] = int(data[4])
-            rx["ovtmp"] = int(data[5])
-            rx["prchg"] = int(data[6])
-
-        line = proc.stdout.readline().decode().strip()
-    return rx
-
+    with subprocess.Popen(['candump', CAN], stdout=subprocess.PIPE) as proc:
+        while True:
+            line = proc.stdout.readline().decode().strip()
+            can_id, data = parse_line(line)
+
+            if can_id == can_a.SYSTEM_CAN_MESSAGE_MOTOR_CONTROLLER_MOTOR_CONTROLLER_VC:
+                rx["vol_l"] = struct.unpack("h", data[0:2])[0]
+                rx["cur_l"] = struct.unpack("h", data[2:4])[0]
+                rx["vol_r"] = struct.unpack("h", data[4:6])[0]
+                rx["cur_r"] = struct.unpack("h", data[6:8])[0]
+            elif can_id == can_a.SYSTEM_CAN_MESSAGE_MOTOR_CONTROLLER_MOTOR_VELOCITY:
+                rx["vel_l"] = struct.unpack("h", data[0:2])[0]
+                rx["vel_r"] = struct.unpack("h", data[2:4])[0]
+            # elif can_id == can.SYSTEM_CAN_MESSAGE_MOTOR_CONTROLLER_MOTOR_STATUS:
+            #     rx["mts_l"] = struct.unpack("i", data[0:4])[0]
+            #     rx["mts_r"] = struct.unpack("i", data[4:8])[0]
+            elif can_id == can_a.SYSTEM_CAN_MESSAGE_MOTOR_CONTROLLER_MOTOR_SINK_TEMPS:
+                rx["mtt_l"] = struct.unpack("h", data[0:2])[0]
+                rx["hst_l"] = struct.unpack("h", data[2:4])[0]
+                rx["mtt_r"] = struct.unpack("h", data[4:6])[0]
+                rx["hst_r"] = struct.unpack("h", data[6:8])[0]
+            elif can_id == can_a.SYSTEM_CAN_MESSAGE_MOTOR_CONTROLLER_DSP_BOARD_TEMPS:
+                rx["dsp_l"] = struct.unpack("h", data[0:2])[0]
+                rx["dsp_r"] = struct.unpack("h", data[2:4])[0]
+            elif can_id == can_a.SYSTEM_CAN_MESSAGE_MOTOR_CONTROLLER_MC_STATUS:
+                rx["lbs_l"] = int(data[0])
+                rx["ebs_l"] = int(data[1])
+                rx["lbs_r"] = int(data[2])
+                rx["ebs_r"] = int(data[3])
+                rx["bfalt"] = int(data[4])
+                rx["ovtmp"] = int(data[5])
+                rx["prchg"] = int(data[6])
 
 def main():
+    # print("hello")
     '''main'''
-    with subprocess.Popen(['candump', 'vcan0'], stdout=subprocess.PIPE) as proc:
-        os.set_blocking(proc.stdout.fileno(), False)
-        os.set_blocking(sys.stdin.fileno(), False)
-        wake_time = time.time()
+    wake_time = time.time()
 
-        state = {
-            "vel": 0,
-            "state": 0,
-            "throttle": 0,
-            "brake": 0,
-        }
+    state = defaultdict(int)
+    state["state"] = 1
 
-        rx_data = defaultdict(int)
+    rx = defaultdict(int)
 
-        while True:
-            # std input
-            state |= handle_stdin()
-            # Can RX
-            rx_data |= rx_all(proc)
+    stdin_thread = threading.Thread(target=handle_stdin, args=(state,))
+    stdin_thread.start()
+
+    rx_thread = threading.Thread(target=handle_rx, args=(rx,))
+    rx_thread.start()
 
-            update_display(state, rx_data)
+    while True:
+        update_display(state, rx)
 
-            # every 200 ms
-            can.send_centre_console_drive_output(
-                state["vel"], state["state"], 1, 1, 1)
-            can.send_pedal_pedal_output(state["throttle"], state["brake"])
+        # every 200 ms
+        # can_a.send_centre_console_drive_output(
+        #     state["vel"], state["state"], state["cruise"], state["regen"], state["precharge"])
+        can_a.send_pedal_pedal_output(state["throttle"], state["brake"])
 
-            wake_time += 0.2
-            time.sleep(max(wake_time - time.time(), 0))
+        wake_time += 0.1
+        time.sleep(max(wake_time - time.time(), 0))
 
 
 if __name__ == "__main__":
diff --git a/py/motor_controller/readme.md b/py/motor_controller/readme.md
new file mode 100644
index 000000000..eeb63674c
--- /dev/null
+++ b/py/motor_controller/readme.md
@@ -0,0 +1,4 @@
+
+```
+
+```
\ No newline at end of file
diff --git a/py/wavesculptor/config.json b/py/wavesculptor/config.json
new file mode 100644
index 000000000..68c4a6bcc
--- /dev/null
+++ b/py/wavesculptor/config.json
@@ -0,0 +1,3 @@
+{
+  "no_lint": true
+}
\ No newline at end of file
diff --git a/py/wavesculptor/main.py b/py/wavesculptor/main.py
new file mode 100644
index 000000000..9ec24bd28
--- /dev/null
+++ b/py/wavesculptor/main.py
@@ -0,0 +1,153 @@
+'''
+Simulate a wavesculptor 22's can messages
+'''
+import subprocess
+import time
+import struct
+from collections import defaultdict
+import threading
+from can_a.util import set_output, parse_line
+
+DRIVER_CONTROL_BASE = 0x00
+MOTOR_CONTROLLER_BASE_L = 0x40
+MOTOR_CONTROLLER_BASE_R = 0x80
+CAN = "can0"
+
+
+def pack(num, size):
+    '''pack a variable into the given size in hex'''
+    if isinstance(num, float) and (size == 32):
+        return struct.pack("f", num).hex()
+    if size == 32:
+        return struct.pack("i", num).hex()
+    if size == 16:
+        return struct.pack("h", num).hex()
+    if size == 8:
+        return struct.pack("b", num).hex()
+    return ""
+
+
+def send_message(can_id, d1, d2):
+    '''send a message with two floats'''
+    data = pack(d1, 32) + pack(d2, 32)
+    subprocess.run(["cansend", CAN, f"{can_id:0>3x}#{data}"], check=False)
+
+
+def send_status(can_id, data):
+    '''send the status message for wavesculptor'''
+    subprocess.run(["cansend", CAN, f"{can_id:0>3x}#{data}"], check=False)
+
+
+def pack_status(rx_err, tx_err, active, err_flg, lim_flg):
+    '''pack the status into a hex string'''
+    return pack(rx_err, 8) + pack(tx_err, 8) + pack(active, 16) + \
+        pack(err_flg, 16) + pack(lim_flg, 16)
+
+
+def handle_stdin(state):
+    '''handle stdin commands'''
+
+
+def send_200ms_messages(current, velocity):
+    '''send can messages that are sent every 200ms'''
+    for i, base_addrs in enumerate([MOTOR_CONTROLLER_BASE_L, MOTOR_CONTROLLER_BASE_R]):
+        send_status(base_addrs + 0x01,
+                    pack_status(0, 0, 0, 0, 0))  # status
+        send_message(base_addrs + 0x02, current + i,
+                     current * 2 + i)  # bus current, bus voltage
+        send_message(base_addrs + 0x03, velocity + i,
+                     velocity * 2 + i)  # vehicle vel, motor rpm
+
+        # these are all ignored so values doens't matter,
+        # just make sure they don't break anything
+        # phase B cur, phase C cur
+        send_message(base_addrs + 0x04, 0.2, 0.2)
+        send_message(base_addrs + 0x05, 0.2, 0.2)  # vol vec Vd, Vq
+        send_message(base_addrs + 0x06, 0.2, 0.2)  # cur vec Id, Iq
+        send_message(base_addrs + 0x07, 0.1, 0.1)  # backEmf d, q
+
+
+def send_1s_messages(offset=0):
+    '''send can messages that are sent every 1s'''
+    for i, base_addrs in enumerate([MOTOR_CONTROLLER_BASE_L, MOTOR_CONTROLLER_BASE_R]):
+        send_message(base_addrs + 0x00, 0, 0)  # id
+        send_message(base_addrs + 0x08, 15.0 + offset + i, 0)  # 15V rail V
+        send_message(base_addrs + 0x09, 3.3 + offset + i,
+                     1.9 + offset + i)  # 3.3 rail V, 1.9 rail V
+        send_message(base_addrs + 0x0b, 30.0 + offset + i,
+                     31.0 + offset + i)  # heatsink temp, motor temp
+        send_message(base_addrs + 0x0c, 0,
+                     25.0 + offset + i)  # ---,  dsp temp
+        send_message(base_addrs + 0x0e, 45.0 + offset + i,
+                     15.0 + offset + i)  # DC bus amp hours, odometer
+
+
+def update_display(motor_velocity, rx):
+    '''update terminal output'''
+    set_output(f'''
+WAVESCULPTOR:
+motor_vel: {motor_velocity}
+current:   {rx["current"]}
+velocity:  {rx["velocity"]}
+''')
+
+
+def handle_rx(rx):
+    '''handle rx from can device'''
+    with subprocess.Popen(['candump', CAN], stdout=subprocess.PIPE) as proc:
+        while True:
+            line = proc.stdout.readline().decode().strip()
+            can_id, data = parse_line(line)
+
+            if can_id == DRIVER_CONTROL_BASE + 1:
+                rx["current"] = struct.unpack("<f", data[0:4])[0]
+                rx["velocity"] = struct.unpack("<f", data[4:8])[0]
+
+
+def main():
+    '''main'''
+    wake_time = time.time()
+    counter = 0
+    counter_2 = 0
+
+    motor_velocity = 0
+
+    state = defaultdict(int)
+    rx = defaultdict(int)
+
+    stdin_thread = threading.Thread(target=handle_stdin, args=(state,))
+    stdin_thread.start()
+
+    rx_thread = threading.Thread(target=handle_rx, args=(rx,))
+    rx_thread.start()
+
+    while True:
+        motor_velocity = motor_velocity * 0.95
+        if motor_velocity < rx["velocity"]:
+            motor_velocity += min(rx["current"],
+                                  rx["velocity"] - motor_velocity)
+        if motor_velocity > rx["velocity"]:
+            motor_velocity -= min(rx["current"],
+                                  motor_velocity - rx["velocity"])
+
+        # every 200 ms
+        send_200ms_messages(rx["current"], motor_velocity)
+        update_display(motor_velocity, rx)
+
+        counter += 1
+        if counter % 5:
+            # every 1 second
+            counter_2 += 1
+            if counter_2 % 10:
+                counter_2 = 0
+
+            send_1s_messages(counter_2 * 0.1)
+
+            counter = 0
+
+        wake_time += 0.2
+        time.sleep(max(wake_time - time.time(), 0))
+
+
+if __name__ == "__main__":
+    main()
diff --git a/smoke/spi/src/main.c b/smoke/spi/src/main.c
index 706a9947f..0cf6cb31e 100644
--- a/smoke/spi/src/main.c
+++ b/smoke/spi/src/main.c
@@ -1,4 +1,5 @@
 #include <stdio.h>
+#include <string.h>
 
 #include "delay.h"
 #include "gpio.h"
@@ -28,10 +29,8 @@ static const uint8_t bytes_to_write[] = { 0xFC };
 // Fill in this variable with the number of bytes to read.
 #define NUM_BYTES_TO_READ 1
 
-// ==== END OF PARAMETERS ====
-
 static SpiSettings spi_settings = {
-  .baudrate = 1000000,  // TO FIND
+  .baudrate = 10000000,  // 10 Mhz
   .mode = SPI_MODE_0,
   .mosi = { .port = GPIO_PORT_B, .pin = 15 },
   .miso = { .port = GPIO_PORT_B, .pin = 14 },
@@ -63,7 +62,6 @@ int main() {
   gpio_init();
   tasks_init();
   log_init();
-  // LOG_DEBUG("Welcome to TEST!");
 
   tasks_init_task(smoke_spi_task, TASK_PRIORITY(1), NULL);