diff --git a/.github/workflows/build-tests.yaml b/.github/workflows/build-tests.yaml
index b6fbfd09d..375c02434 100644
--- a/.github/workflows/build-tests.yaml
+++ b/.github/workflows/build-tests.yaml
@@ -18,7 +18,7 @@ jobs:
   build-and-test:
     name: Run all tests
     runs-on: "ubuntu-20.04"
-    timeout-minutes: 10
+    timeout-minutes: 20
     steps:
       - name: Checkout ot3-firmware repo
         uses: actions/checkout@v4
diff --git a/gantry/core/tasks_proto.cpp b/gantry/core/tasks_proto.cpp
index de020ffc3..965adbd0f 100644
--- a/gantry/core/tasks_proto.cpp
+++ b/gantry/core/tasks_proto.cpp
@@ -55,18 +55,20 @@ static auto eeprom_data_rev_update_builder =
 /**
  * Start gantry tasks.
  */
-void gantry::tasks::start_tasks(
+auto gantry::tasks::start_tasks(
     can::bus::CanBus& can_bus,
     motion_controller::MotionController<lms::BeltConfig>& motion_controller,
     spi::hardware::SpiDeviceBase& spi_device,
     tmc2130::configs::TMC2130DriverConfig& driver_configs,
     motor_hardware_task::MotorHardwareTask& mh_tsk,
     i2c::hardware::I2CBase& i2c2,
-    eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface) {
+    eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface)
+    -> interfaces::diag0_handler {
     auto& can_writer = can_task::start_writer(can_bus);
     can_task::start_reader(can_bus);
-    auto& motion = mc_task_builder.start(5, "motion controller",
-                                         motion_controller, ::queues, ::queues);
+    auto& motion =
+        mc_task_builder.start(5, "motion controller", motion_controller,
+                              ::queues, ::queues, ::queues, ::queues);
     auto& tmc2130_driver = motor_driver_task_builder.start(
         5, "tmc2130 driver", driver_configs, ::queues, spi_task_client);
     auto& move_group =
@@ -116,6 +118,15 @@ void gantry::tasks::start_tasks(
     ::queues.usage_storage_queue = &usage_storage_task.get_queue();
 
     mh_tsk.start_task();
+
+    return gantry::tasks::call_run_diag0_interrupt;
+}
+
+void gantry::tasks::call_run_diag0_interrupt() {
+    if (gantry::tasks::get_tasks().motion_controller) {
+        return gantry::tasks::get_tasks()
+            .motion_controller->run_diag0_interrupt();
+    }
 }
 
 gantry::queues::QueueClient::QueueClient(can::ids::NodeId this_fw)
@@ -131,6 +142,11 @@ void gantry::queues::QueueClient::send_motor_driver_queue(
     motor_driver_queue->try_write(m);
 }
 
+void gantry::queues::QueueClient::send_motor_driver_queue_isr(
+    const tmc2130::tasks::TaskMessage& m) {
+    static_cast<void>(motor_driver_queue->try_write_isr(m));
+}
+
 void gantry::queues::QueueClient::send_move_group_queue(
     const move_group_task::TaskMessage& m) {
     move_group_queue->try_write(m);
diff --git a/gantry/core/tasks_rev1.cpp b/gantry/core/tasks_rev1.cpp
index 95c81ba29..4ba6f156c 100644
--- a/gantry/core/tasks_rev1.cpp
+++ b/gantry/core/tasks_rev1.cpp
@@ -54,18 +54,20 @@ static auto tail_accessor = eeprom::dev_data::DevDataTailAccessor{queues};
 /**
  * Start gantry ::tasks.
  */
-void gantry::tasks::start_tasks(
+auto gantry::tasks::start_tasks(
     can::bus::CanBus& can_bus,
     motion_controller::MotionController<lms::BeltConfig>& motion_controller,
     spi::hardware::SpiDeviceBase& spi_device,
     tmc2160::configs::TMC2160DriverConfig& driver_configs,
     motor_hardware_task::MotorHardwareTask& mh_tsk,
     i2c::hardware::I2CBase& i2c2,
-    eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface) {
+    eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface)
+    -> interfaces::diag0_handler {
     auto& can_writer = can_task::start_writer(can_bus);
     can_task::start_reader(can_bus);
-    auto& motion = mc_task_builder.start(5, "motion controller",
-                                         motion_controller, ::queues, ::queues);
+    auto& motion =
+        mc_task_builder.start(5, "motion controller", motion_controller,
+                              ::queues, ::queues, ::queues, ::queues);
     auto& tmc2160_driver = motor_driver_task_builder.start(
         5, "tmc2160 driver", driver_configs, ::queues, spi_task_client);
     auto& move_group =
@@ -115,6 +117,15 @@ void gantry::tasks::start_tasks(
     ::queues.usage_storage_queue = &usage_storage_task.get_queue();
 
     mh_tsk.start_task();
+
+    return gantry::tasks::call_run_diag0_interrupt;
+}
+
+void gantry::tasks::call_run_diag0_interrupt() {
+    if (gantry::tasks::get_tasks().motion_controller) {
+        return gantry::tasks::get_tasks()
+            .motion_controller->run_diag0_interrupt();
+    }
 }
 
 gantry::queues::QueueClient::QueueClient(can::ids::NodeId this_fw)
@@ -130,6 +141,11 @@ void gantry::queues::QueueClient::send_motor_driver_queue(
     motor_driver_queue->try_write(m);
 }
 
+void gantry::queues::QueueClient::send_motor_driver_queue_isr(
+    const tmc2160::tasks::TaskMessage& m) {
+    static_cast<void>(motor_driver_queue->try_write_isr(m));
+}
+
 void gantry::queues::QueueClient::send_move_group_queue(
     const move_group_task::TaskMessage& m) {
     move_group_queue->try_write(m);
diff --git a/gantry/firmware/interfaces_proto.cpp b/gantry/firmware/interfaces_proto.cpp
index 584fd990e..0c142d197 100644
--- a/gantry/firmware/interfaces_proto.cpp
+++ b/gantry/firmware/interfaces_proto.cpp
@@ -86,10 +86,16 @@ struct motion_controller::HardwareConfig motor_pins_x {
             .port = GPIOB,
             .pin = GPIO_PIN_7,
             .active_setting = GPIO_PIN_RESET},
-    .estop_in = {
+    .estop_in =
+        {
+            // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
+            .port = GPIOA,
+            .pin = GPIO_PIN_10,
+            .active_setting = GPIO_PIN_RESET},
+    .diag0 = {
         // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
-        .port = GPIOA,
-        .pin = GPIO_PIN_10,
+        .port = GPIOC,
+        .pin = GPIO_PIN_5,
         .active_setting = GPIO_PIN_RESET}
 };
 
@@ -125,15 +131,21 @@ struct motion_controller::HardwareConfig motor_pins_y {
             .port = GPIOB,
             .pin = GPIO_PIN_5,
             .active_setting = GPIO_PIN_RESET},
-    .estop_in = {
+    .estop_in =
+        {
+            // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
+            .port = GPIOA,
+            .pin = GPIO_PIN_10,
+            .active_setting = GPIO_PIN_RESET},
+    .diag0 = {
         // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
-        .port = GPIOA,
-        .pin = GPIO_PIN_10,
+        .port = GPIOC,
+        .pin = GPIO_PIN_5,
         .active_setting = GPIO_PIN_RESET}
 };
 
 static tmc2130::configs::TMC2130DriverConfig gantry_x_driver_configs{
-    .registers = {.gconfig = {.en_pwm_mode = 1},
+    .registers = {.gconfig = {.en_pwm_mode = 1, .diag0_error = 1},
                   .ihold_irun = {.hold_current = 0x2,
                                  .run_current = 0x18,
                                  .hold_current_delay = 0x7},
@@ -156,7 +168,7 @@ static tmc2130::configs::TMC2130DriverConfig gantry_x_driver_configs{
     }};
 
 static tmc2130::configs::TMC2130DriverConfig gantry_y_driver_configs{
-    .registers = {.gconfig = {.en_pwm_mode = 1},
+    .registers = {.gconfig = {.en_pwm_mode = 1, .diag0_error = 1},
                   .ihold_irun = {.hold_current = 0x2,
                                  .run_current = 0x18,
                                  .hold_current_delay = 0x7},
@@ -232,8 +244,8 @@ static stall_check::StallCheck stallcheck(
  * Handler of motor interrupts.
  */
 static motor_handler::MotorInterruptHandler motor_interrupt(
-    motor_queue, gantry::queues::get_queues(), motor_hardware_iface, stallcheck,
-    update_position_queue);
+    motor_queue, gantry::queues::get_queues(), gantry::queues::get_queues(),
+    motor_hardware_iface, stallcheck, update_position_queue);
 
 static auto encoder_background_timer =
     motor_encoder::BackgroundTimer(motor_interrupt, motor_hardware_iface);
@@ -269,13 +281,14 @@ static constexpr auto can_bit_timings =
     can::bit_timings::BitTimings<170 * can::bit_timings::MHZ, 100,
                                  500 * can::bit_timings::KHZ, 800>{};
 
-void interfaces::initialize() {
+void interfaces::initialize(diag0_handler* call_diag0_handler) {
     // Initialize SPI
     if (initialize_spi(get_axis_type()) != HAL_OK) {
         Error_Handler();
     }
 
-    initialize_timer(call_motor_handler, enc_overflow_callback);
+    initialize_timer(call_motor_handler, call_diag0_handler,
+                     enc_overflow_callback);
 
     // Start the can bus
     canbus.start(can_bit_timings);
diff --git a/gantry/firmware/interfaces_rev1.cpp b/gantry/firmware/interfaces_rev1.cpp
index bf0de3359..d34b82c10 100644
--- a/gantry/firmware/interfaces_rev1.cpp
+++ b/gantry/firmware/interfaces_rev1.cpp
@@ -86,10 +86,16 @@ struct motion_controller::HardwareConfig motor_pins_x {
             .port = GPIOB,
             .pin = GPIO_PIN_7,
             .active_setting = GPIO_PIN_RESET},
-    .estop_in = {
+    .estop_in =
+        {
+            // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
+            .port = GPIOA,
+            .pin = GPIO_PIN_10,
+            .active_setting = GPIO_PIN_RESET},
+    .diag0 = {
         // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
-        .port = GPIOA,
-        .pin = GPIO_PIN_10,
+        .port = GPIOC,
+        .pin = GPIO_PIN_5,
         .active_setting = GPIO_PIN_RESET}
 };
 
@@ -125,15 +131,21 @@ struct motion_controller::HardwareConfig motor_pins_y {
             .port = GPIOB,
             .pin = GPIO_PIN_7,
             .active_setting = GPIO_PIN_RESET},
-    .estop_in = {
+    .estop_in =
+        {
+            // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
+            .port = GPIOA,
+            .pin = GPIO_PIN_10,
+            .active_setting = GPIO_PIN_RESET},
+    .diag0 = {
         // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
-        .port = GPIOA,
-        .pin = GPIO_PIN_10,
+        .port = GPIOC,
+        .pin = GPIO_PIN_5,
         .active_setting = GPIO_PIN_RESET}
 };
 
 static tmc2160::configs::TMC2160DriverConfig motor_driver_config_x{
-    .registers = {.gconfig = {.en_pwm_mode = 0},
+    .registers = {.gconfig = {.en_pwm_mode = 0, .diag0_error = 1},
                   .ihold_irun = {.hold_current = 16,
                                  .run_current = 31,
                                  .hold_current_delay = 0x7},
@@ -159,6 +171,7 @@ static tmc2160::configs::TMC2160DriverConfig motor_driver_config_x{
                               .freewheel = 0,
                               .pwm_reg = 0x7,
                               .pwm_lim = 0xC},
+                  .drvconf = {.ot_select = 0},
                   .glob_scale = {.global_scaler = 0xA7}},
     .current_config =
         {
@@ -172,7 +185,7 @@ static tmc2160::configs::TMC2160DriverConfig motor_driver_config_x{
     }};
 
 static tmc2160::configs::TMC2160DriverConfig motor_driver_config_y{
-    .registers = {.gconfig = {.en_pwm_mode = 0},
+    .registers = {.gconfig = {.en_pwm_mode = 0, .diag0_error = 1},
                   .ihold_irun = {.hold_current = 16,
                                  .run_current = 31,
                                  .hold_current_delay = 0x7},
@@ -198,6 +211,7 @@ static tmc2160::configs::TMC2160DriverConfig motor_driver_config_y{
                               .freewheel = 0,
                               .pwm_reg = 0x7,
                               .pwm_lim = 0xC},
+                  .drvconf = {.ot_select = 0},
                   .glob_scale = {.global_scaler = 0xA7}},
     .current_config =
         {
@@ -257,8 +271,8 @@ static stall_check::StallCheck stallcheck(
  * Handler of motor interrupts.
  */
 static motor_handler::MotorInterruptHandler motor_interrupt(
-    motor_queue, gantry::queues::get_queues(), motor_hardware_iface, stallcheck,
-    update_position_queue);
+    motor_queue, gantry::queues::get_queues(), gantry::queues::get_queues(),
+    motor_hardware_iface, stallcheck, update_position_queue);
 
 static auto encoder_background_timer =
     motor_encoder::BackgroundTimer(motor_interrupt, motor_hardware_iface);
@@ -295,13 +309,14 @@ static constexpr auto can_bit_timings =
     can::bit_timings::BitTimings<170 * can::bit_timings::MHZ, 100,
                                  500 * can::bit_timings::KHZ, 800>{};
 
-void interfaces::initialize() {
+void interfaces::initialize(diag0_handler* call_diag0_handler) {
     // Initialize SPI
     if (initialize_spi(get_axis_type()) != HAL_OK) {
         Error_Handler();
     }
 
-    initialize_timer(call_motor_handler, enc_overflow_callback);
+    initialize_timer(call_motor_handler, call_diag0_handler,
+                     enc_overflow_callback);
 
     // Start the can bus
     canbus.start(can_bit_timings);
diff --git a/gantry/firmware/main_proto.cpp b/gantry/firmware/main_proto.cpp
index cf7c26e03..55d10cc16 100644
--- a/gantry/firmware/main_proto.cpp
+++ b/gantry/firmware/main_proto.cpp
@@ -16,6 +16,8 @@
 #include "gantry/core/tasks_proto.hpp"
 #include "i2c/firmware/i2c_comms.hpp"
 
+static interfaces::diag0_handler call_diag0_handler = nullptr;
+
 static auto i2c_comms2 = i2c::hardware::I2C();
 static auto i2c_handles = I2CHandlerStruct{};
 
@@ -44,11 +46,11 @@ auto main() -> int {
 
     app_update_clear_flags();
 
-    interfaces::initialize();
+    interfaces::initialize(&call_diag0_handler);
     i2c_setup(&i2c_handles);
     i2c_comms2.set_handle(i2c_handles.i2c2);
 
-    gantry::tasks::start_tasks(
+    call_diag0_handler = gantry::tasks::start_tasks(
         interfaces::get_can_bus(), interfaces::get_motor().motion_controller,
         interfaces::get_spi(), interfaces::get_driver_config(),
         interfaces::get_motor_hardware_task(), i2c_comms2, eeprom_hw_iface);
diff --git a/gantry/firmware/main_rev1.cpp b/gantry/firmware/main_rev1.cpp
index 39fed3b73..77b198539 100644
--- a/gantry/firmware/main_rev1.cpp
+++ b/gantry/firmware/main_rev1.cpp
@@ -16,6 +16,8 @@
 #include "gantry/core/tasks_rev1.hpp"
 #include "i2c/firmware/i2c_comms.hpp"
 
+static interfaces::diag0_handler call_diag0_handler = nullptr;
+
 static auto i2c_comms2 = i2c::hardware::I2C();
 static auto i2c_handles = I2CHandlerStruct{};
 
@@ -44,11 +46,11 @@ auto main() -> int {
 
     app_update_clear_flags();
 
-    interfaces::initialize();
+    interfaces::initialize(&call_diag0_handler);
     i2c_setup(&i2c_handles);
     i2c_comms2.set_handle(i2c_handles.i2c2);
 
-    gantry::tasks::start_tasks(
+    call_diag0_handler = gantry::tasks::start_tasks(
         interfaces::get_can_bus(), interfaces::get_motor().motion_controller,
         interfaces::get_spi(), interfaces::get_driver_config(),
         interfaces::get_motor_hardware_task(), i2c_comms2, eeprom_hw_iface);
diff --git a/gantry/firmware/motor_hardware.c b/gantry/firmware/motor_hardware.c
index f48bcb6d7..637087c76 100644
--- a/gantry/firmware/motor_hardware.c
+++ b/gantry/firmware/motor_hardware.c
@@ -23,6 +23,8 @@ void HAL_SPI_MspInit(SPI_HandleTypeDef* hspi) {
         PC8   ------> Motor Step Pin
         Enable
         PA9   ------> Motor Enable Pin
+        DIAG0
+        PC5   ------> Motor Diag0 Pin
         */
         GPIO_InitStruct.Pin = GPIO_PIN_13 | GPIO_PIN_14 | GPIO_PIN_15;
         GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
@@ -54,6 +56,12 @@ void HAL_SPI_MspInit(SPI_HandleTypeDef* hspi) {
         GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
         HAL_GPIO_Init(GPIOA,  // NOLINT(cppcoreguidelines-pro-type-cstyle-cast)
                       &GPIO_InitStruct);
+
+        // Diag0
+        GPIO_InitStruct.Pin = GPIO_PIN_5;
+        GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING_FALLING;
+        GPIO_InitStruct.Pull = GPIO_NOPULL;
+        HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
     }
 }
 SPI_HandleTypeDef hspi2 = {
@@ -87,7 +95,7 @@ void HAL_SPI_MspDeInit(SPI_HandleTypeDef* hspi) {
         HAL_GPIO_DeInit(GPIOB, GPIO_PIN_12 | GPIO_PIN_13 | GPIO_PIN_14 |
                                    GPIO_PIN_15 | GPIO_PIN_1);
         HAL_GPIO_DeInit(GPIOA, GPIO_PIN_9);
-        HAL_GPIO_DeInit(GPIOC, GPIO_PIN_8);
+        HAL_GPIO_DeInit(GPIOC, GPIO_PIN_8 | GPIO_PIN_5);
     }
 }
 
@@ -119,6 +127,7 @@ HAL_StatusTypeDef initialize_spi(enum GantryAxisType gantry_type) {
 
 static motor_interrupt_callback timer_callback = NULL;
 static encoder_overflow_callback enc_overflow_callback = NULL;
+static diag0_interrupt_callback* diag0_callback = NULL;
 
 
 /**
@@ -140,6 +149,9 @@ void MX_GPIO_Init(void) {
     GPIO_InitStruct.Pull = GPIO_NOPULL;
     GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
     HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
+
+    HAL_NVIC_SetPriority(EXTI9_5_IRQn, 5, 0);
+    HAL_NVIC_EnableIRQ(EXTI9_5_IRQn);
 }
 
 // motor timer: 200kHz from
@@ -236,6 +248,16 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {
     }
 }
 
+void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
+    if (GPIO_Pin == GPIO_PIN_5) {
+        if (diag0_callback != NULL) {
+            if (*diag0_callback != NULL) {
+                (*diag0_callback)();
+            }
+        }
+    }
+}
+
 void HAL_TIM_Encoder_MspInit(TIM_HandleTypeDef *htim) {
     if (htim == &htim2) {
         /* Peripheral clock enable */
@@ -255,9 +277,10 @@ void HAL_TIM_Base_MspInit(TIM_HandleTypeDef *htim) {
     }
 }
 
-void initialize_timer(motor_interrupt_callback callback, encoder_overflow_callback enc_callback) {
+void initialize_timer(motor_interrupt_callback callback, diag0_interrupt_callback* diag0_int_callback, encoder_overflow_callback enc_callback) {
     timer_callback = callback;
     enc_overflow_callback = enc_callback;
+    diag0_callback = diag0_int_callback;
     MX_GPIO_Init();
     MX_TIM7_Init();
     Encoder_GPIO_Init();
diff --git a/gantry/firmware/motor_hardware.h b/gantry/firmware/motor_hardware.h
index fa2e0ac71..3a3024ac6 100644
--- a/gantry/firmware/motor_hardware.h
+++ b/gantry/firmware/motor_hardware.h
@@ -14,11 +14,12 @@ extern TIM_HandleTypeDef htim2;
 
 typedef void (*motor_interrupt_callback)();
 typedef void (*encoder_overflow_callback)(int32_t);
+typedef void (*diag0_interrupt_callback)();
 
 HAL_StatusTypeDef initialize_spi(enum GantryAxisType);
 void gantry_driver_CLK_init(enum GantryAxisType);
 
-void initialize_timer(motor_interrupt_callback callback, encoder_overflow_callback enc_callback);
+void initialize_timer(motor_interrupt_callback callback, diag0_interrupt_callback* diag0_int_callback, encoder_overflow_callback enc_callback);
 
 #ifdef __cplusplus
 }  // extern "C"
diff --git a/gantry/firmware/stm32g4xx_it.c b/gantry/firmware/stm32g4xx_it.c
index b4f589e8f..07463106c 100644
--- a/gantry/firmware/stm32g4xx_it.c
+++ b/gantry/firmware/stm32g4xx_it.c
@@ -147,6 +147,7 @@ void FDCAN1_IT0_IRQHandler(void) {
  */
 void TIM7_IRQHandler(void) { HAL_TIM_IRQHandler(&htim7); }
 void TIM2_IRQHandler(void) { HAL_TIM_IRQHandler(&htim2); }
+void EXTI9_5_IRQHandler(void) { HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_5); }
 
 
 extern void xPortSysTickHandler(void);
diff --git a/gantry/simulator/interfaces.cpp b/gantry/simulator/interfaces.cpp
index f1a59f0bd..a22db5105 100644
--- a/gantry/simulator/interfaces.cpp
+++ b/gantry/simulator/interfaces.cpp
@@ -89,14 +89,16 @@ static stall_check::StallCheck stallcheck(
  * Handler of motor interrupts.
  */
 static motor_handler::MotorInterruptHandler motor_interrupt(
-    motor_queue, gantry::queues::get_queues(), motor_interface, stallcheck,
-    update_position_queue);
+    motor_queue, gantry::queues::get_queues(), gantry::queues::get_queues(),
+    motor_interface, stallcheck, update_position_queue);
 
 static motor_interrupt_driver::MotorInterruptDriver A(motor_queue,
                                                       motor_interrupt,
                                                       motor_interface,
                                                       update_position_queue);
-void interfaces::initialize() {}
+void interfaces::initialize(diag0_handler* call_diag0_handler) {
+    static_cast<void>(call_diag0_handler);
+}
 
 static po::variables_map options{};
 
diff --git a/gantry/simulator/main.cpp b/gantry/simulator/main.cpp
index d78b931e2..77a2d985d 100644
--- a/gantry/simulator/main.cpp
+++ b/gantry/simulator/main.cpp
@@ -12,6 +12,8 @@ void signal_handler(int signum) {
     exit(signum);
 }
 
+static interfaces::diag0_handler call_diag0_handler = NULL;
+
 int main(int argc, char** argv) {
     signal(SIGINT, signal_handler);
 
@@ -21,10 +23,10 @@ int main(int argc, char** argv) {
             return pcTaskGetName(xTaskGetCurrentTaskHandle());
         });
 
-    interfaces::initialize();
+    interfaces::initialize(&call_diag0_handler);
     interfaces::initialize_sim(argc, argv);
 
-    gantry::tasks::start_tasks(
+    call_diag0_handler = gantry::tasks::start_tasks(
         interfaces::get_can_bus(), interfaces::get_motor().motion_controller,
         interfaces::get_spi(), interfaces::get_driver_config(),
         interfaces::get_motor_hardware_task(), *interfaces::get_sim_i2c2(),
diff --git a/gripper/core/tasks.cpp b/gripper/core/tasks.cpp
index 9c9ea66ed..ae3dd7d2b 100644
--- a/gripper/core/tasks.cpp
+++ b/gripper/core/tasks.cpp
@@ -62,7 +62,7 @@ static auto eeprom_data_rev_update_builder =
 /**
  * Start gripper tasks.
  */
-void gripper_tasks::start_tasks(
+auto gripper_tasks::start_tasks(
     can::bus::CanBus& can_bus,
     motor_class::Motor<lms::LeadScrewConfig>& z_motor,
     brushed_motor::BrushedMotor<lms::GearBoxConfig>& grip_motor,
@@ -72,7 +72,8 @@ void gripper_tasks::start_tasks(
     sensors::hardware::SensorHardwareBase& sensor_hardware,
     eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface,
     motor_hardware_task::MotorHardwareTask& zmh_tsk,
-    motor_hardware_task::MotorHardwareTask& gmh_tsk) {
+    motor_hardware_task::MotorHardwareTask& gmh_tsk)
+    -> z_motor_iface::diag0_handler {
     auto& can_writer = can_task::start_writer(can_bus);
     can_task::start_reader(can_bus);
     tasks.can_writer = &can_writer;
@@ -131,8 +132,8 @@ void gripper_tasks::start_tasks(
     queues.capacitive_sensor_queue_rear =
         &capacitive_sensor_task_rear.get_queue();
 
-    z_tasks::start_task(z_motor, spi_device, driver_configs, tasks, queues,
-                        tail_accessor);
+    auto diag0_handler = z_tasks::start_task(
+        z_motor, spi_device, driver_configs, tasks, queues, tail_accessor);
 
     g_tasks::start_task(grip_motor, tasks, queues, tail_accessor);
 
@@ -141,6 +142,8 @@ void gripper_tasks::start_tasks(
 
     zmh_tsk.start_task();
     gmh_tsk.start_task();
+
+    return diag0_handler;
 }
 
 gripper_tasks::QueueClient::QueueClient(can::ids::NodeId this_fw)
diff --git a/gripper/core/tasks_z.cpp b/gripper/core/tasks_z.cpp
index 43432ea46..ce5f9b306 100644
--- a/gripper/core/tasks_z.cpp
+++ b/gripper/core/tasks_z.cpp
@@ -34,15 +34,16 @@ static auto z_usage_storage_task_builder =
     freertos_task::TaskStarter<512, usage_storage_task::UsageStorageTask>{};
 #endif
 
-void z_tasks::start_task(
+auto z_tasks::start_task(
     motor_class::Motor<lms::LeadScrewConfig>& z_motor,
     spi::hardware::SpiDeviceBase& spi_device,
     tmc2130::configs::TMC2130DriverConfig& driver_configs,
     AllTask& gripper_tasks, gripper_tasks::QueueClient& main_queues,
     eeprom::dev_data::DevDataTailAccessor<gripper_tasks::QueueClient>&
-        tail_accessor) {
-    auto& motion = mc_task_builder.start(5, "z mc", z_motor.motion_controller,
-                                         z_queues, z_queues);
+        tail_accessor) -> z_motor_iface::diag0_handler {
+    auto& motion =
+        mc_task_builder.start(5, "z mc", z_motor.motion_controller, z_queues,
+                              z_queues, z_queues, z_queues);
     auto& move_group =
         move_group_task_builder.start(5, "move group", z_queues, z_queues);
     auto& tmc2130_driver = motor_driver_task_builder.start(
@@ -77,6 +78,14 @@ void z_tasks::start_task(
     z_queues.z_usage_storage_queue = &z_usage_storage_task.get_queue();
 #endif
     spi_task_client.set_queue(&spi_task.get_queue());
+
+    return z_tasks::call_run_diag0_interrupt;
+}
+
+void z_tasks::call_run_diag0_interrupt() {
+    if (get_all_tasks().motion_controller) {
+        return get_all_tasks().motion_controller->run_diag0_interrupt();
+    }
 }
 
 z_tasks::QueueClient::QueueClient()
@@ -92,6 +101,11 @@ void z_tasks::QueueClient::send_motor_driver_queue(
     tmc2130_driver_queue->try_write(m);
 }
 
+void z_tasks::QueueClient::send_motor_driver_queue_isr(
+    const tmc2130::tasks::TaskMessage& m) {
+    static_cast<void>(tmc2130_driver_queue->try_write_isr(m));
+}
+
 void z_tasks::QueueClient::send_move_group_queue(
     const move_group_task::TaskMessage& m) {
     move_group_queue->try_write(m);
diff --git a/gripper/firmware/interfaces_z_motor.cpp b/gripper/firmware/interfaces_z_motor.cpp
index 69405dfde..a979ffca0 100644
--- a/gripper/firmware/interfaces_z_motor.cpp
+++ b/gripper/firmware/interfaces_z_motor.cpp
@@ -103,7 +103,13 @@ struct motion_controller::HardwareConfig motor_pins {
             .port = ESTOP_IN_PORT,
             .pin = ESTOP_IN_PIN,
             .active_setting = GPIO_PIN_RESET},
-    .ebrake = ebrake,
+    .diag0 =
+        {
+            // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
+            .port = GPIOB,
+            .pin = GPIO_PIN_2,
+            .active_setting = GPIO_PIN_RESET},
+    .ebrake = ebrake
 };
 
 /**
@@ -119,6 +125,7 @@ static motor_hardware::MotorHardware motor_hardware_iface(motor_pins, &htim7,
  */
 static tmc2130::configs::TMC2130DriverConfig MotorDriverConfigurations{
     .registers = {.gconfig = {.en_pwm_mode = 0x0,
+                              .diag0_error = 1,
                               .stop_enable = use_stop_enable},
                   .ihold_irun = {.hold_current = 0x2,  // 0.177A
                                  .run_current = 0xA,   // 0.648A
@@ -196,8 +203,9 @@ static motor_class::Motor z_motor{
  * Handler of motor interrupts.
  */
 static motor_handler::MotorInterruptHandler motor_interrupt(
-    motor_queue, gripper_tasks::z_tasks::get_queues(), motor_hardware_iface,
-    stallcheck, update_position_queue);
+    motor_queue, gripper_tasks::z_tasks::get_queues(),
+    gripper_tasks::z_tasks::get_queues(), motor_hardware_iface, stallcheck,
+    update_position_queue);
 
 static auto encoder_background_timer =
     motor_encoder::BackgroundTimer(motor_interrupt, motor_hardware_iface);
@@ -210,12 +218,13 @@ extern "C" void call_enc_handler(int32_t direction) {
     motor_hardware_iface.encoder_overflow(direction);
 }
 
-void z_motor_iface::initialize() {
+void z_motor_iface::initialize(diag0_handler* call_diag0_handler) {
     if (initialize_spi() != HAL_OK) {
         Error_Handler();
     }
     initialize_hardware_z();
-    set_z_motor_timer_callback(call_motor_handler, call_enc_handler);
+    set_z_motor_timer_callback(call_motor_handler, call_diag0_handler,
+                               call_enc_handler);
     encoder_background_timer.start();
 }
 
diff --git a/gripper/firmware/main_proto.cpp b/gripper/firmware/main_proto.cpp
index 96328dca9..c4a249b0c 100644
--- a/gripper/firmware/main_proto.cpp
+++ b/gripper/firmware/main_proto.cpp
@@ -25,6 +25,8 @@
 #include "i2c/firmware/i2c_comms.hpp"
 #include "sensors/firmware/sensor_hardware.hpp"
 
+static z_motor_iface::diag0_handler call_diag0_handler = nullptr;
+
 static auto iWatchdog = iwdg::IndependentWatchDog{};
 
 /**
@@ -100,7 +102,7 @@ auto main() -> int {
 
     app_update_clear_flags();
 
-    z_motor_iface::initialize();
+    z_motor_iface::initialize(&call_diag0_handler);
     grip_motor_iface::initialize();
 
     i2c_setup(&i2c_handles);
@@ -108,7 +110,7 @@ auto main() -> int {
     i2c_comms3.set_handle(i2c_handles.i2c3);
 
     canbus.start(can_bit_timings);
-    gripper_tasks::start_tasks(
+    call_diag0_handler = gripper_tasks::start_tasks(
         canbus, z_motor_iface::get_z_motor(),
         grip_motor_iface::get_grip_motor(), z_motor_iface::get_spi(),
         z_motor_iface::get_tmc2130_driver_configs(), i2c_comms2, i2c_comms3,
diff --git a/gripper/firmware/main_rev1.cpp b/gripper/firmware/main_rev1.cpp
index b15eafec9..5e351ee53 100644
--- a/gripper/firmware/main_rev1.cpp
+++ b/gripper/firmware/main_rev1.cpp
@@ -26,6 +26,8 @@
 #include "i2c/firmware/i2c_comms.hpp"
 #include "sensors/firmware/sensor_hardware.hpp"
 
+static z_motor_iface::diag0_handler call_diag0_handler = nullptr;
+
 static auto iWatchdog = iwdg::IndependentWatchDog{};
 
 /**
@@ -111,7 +113,7 @@ auto main() -> int {
 
     app_update_clear_flags();
 
-    z_motor_iface::initialize();
+    z_motor_iface::initialize(&call_diag0_handler);
     grip_motor_iface::initialize();
 
     i2c_setup(&i2c_handles);
@@ -119,7 +121,7 @@ auto main() -> int {
     i2c_comms3.set_handle(i2c_handles.i2c3);
 
     canbus.start(can_bit_timings);
-    gripper_tasks::start_tasks(
+    call_diag0_handler = gripper_tasks::start_tasks(
         canbus, z_motor_iface::get_z_motor(),
         grip_motor_iface::get_grip_motor(), z_motor_iface::get_spi(),
         z_motor_iface::get_tmc2130_driver_configs(), i2c_comms2, i2c_comms3,
diff --git a/gripper/firmware/motor_hardware.h b/gripper/firmware/motor_hardware.h
index 37bce25ca..4df6dbd0a 100644
--- a/gripper/firmware/motor_hardware.h
+++ b/gripper/firmware/motor_hardware.h
@@ -32,6 +32,7 @@ extern TIM_HandleTypeDef htim8;
 
 typedef void (*motor_interrupt_callback)();
 typedef void (*z_encoder_overflow_callback)(int32_t);
+typedef void (*diag0_interrupt_callback)();
 
 
 HAL_StatusTypeDef initialize_spi();
@@ -39,6 +40,7 @@ void initialize_hardware_z();
 
 void set_z_motor_timer_callback(
         motor_interrupt_callback callback,
+        diag0_interrupt_callback* diag0_int_callback,
         z_encoder_overflow_callback enc_callback);
 
 
diff --git a/gripper/firmware/motor_hardware_shared.c b/gripper/firmware/motor_hardware_shared.c
index 997322797..f6118af8b 100644
--- a/gripper/firmware/motor_hardware_shared.c
+++ b/gripper/firmware/motor_hardware_shared.c
@@ -3,6 +3,7 @@
 
 static motor_interrupt_callback timer_callback = NULL;
 static z_encoder_overflow_callback z_enc_overflow_callback = NULL;
+static diag0_interrupt_callback* diag0_callback = NULL;
 static brushed_motor_interrupt_callback brushed_timer_callback = NULL;
 static encoder_overflow_callback gripper_enc_overflow_callback = NULL;
 static encoder_idle_state_callback gripper_enc_idle_state_overflow_callback =
@@ -10,11 +11,19 @@ static encoder_idle_state_callback gripper_enc_idle_state_overflow_callback =
 static stopwatch_overflow_callback gripper_force_stopwatch_overflow_callback = NULL;
 
 
+void MX_GPIO_Init(void) {
+    HAL_NVIC_SetPriority(EXTI2_IRQn, 5, 0);
+    HAL_NVIC_EnableIRQ(EXTI2_IRQn);
+}
+
 void set_z_motor_timer_callback(
         motor_interrupt_callback callback,
+        diag0_interrupt_callback* diag0_int_callback,
         z_encoder_overflow_callback enc_callback) {
     timer_callback = callback;
     z_enc_overflow_callback = enc_callback;
+    diag0_callback = diag0_int_callback;
+    MX_GPIO_Init();
 }
 
 void set_brushed_motor_timer_callback(
@@ -212,9 +221,14 @@ void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef* htim) {
     }
 }
 
-
 void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
-    if (GPIO_Pin == ESTOP_IN_PIN) {
+    if (GPIO_Pin == GPIO_PIN_2) {
+        if (diag0_callback != NULL) {
+            if (*diag0_callback != NULL) {
+                (*diag0_callback)();
+            }
+        }
+    } else if (GPIO_Pin == ESTOP_IN_PIN) {
         #if PCBA_PRIMARY_REVISION != 'b' && PCBA_PRIMARY_REVISION != 'a'
             HAL_GPIO_WritePin(EBRAKE_PORT, EBRAKE_PIN, GPIO_PIN_RESET);
         #endif
diff --git a/gripper/firmware/stm32g4xx_it.c b/gripper/firmware/stm32g4xx_it.c
index 2051ebc6b..6a90023e6 100644
--- a/gripper/firmware/stm32g4xx_it.c
+++ b/gripper/firmware/stm32g4xx_it.c
@@ -198,6 +198,11 @@ void TIM7_IRQHandler(void) {
     call_motor_handler();
 }
 
+__attribute__((section(".ccmram")))
+void EXTI2_IRQHandler(void) {
+    HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_2);
+}
+
 /**
   * @brief This function handles EXTI line[15:10] interrupts.
   */
diff --git a/gripper/firmware/utility_gpio.c b/gripper/firmware/utility_gpio.c
index 551f5cc12..9c25e0b46 100644
--- a/gripper/firmware/utility_gpio.c
+++ b/gripper/firmware/utility_gpio.c
@@ -108,6 +108,8 @@ static void z_motor_gpio_init(void) {
     PB1   ------> Motor Step Pin
     Enable
     PA9   ------> Motor Enable Pin
+    Diag0
+    PB2   ------> Motor Diag0 Pin
     */
     GPIO_InitStruct.Pin = Z_MOT_DIR_PIN | Z_MOT_STEP_PIN;
     GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
@@ -119,6 +121,11 @@ static void z_motor_gpio_init(void) {
     GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
     HAL_GPIO_Init(Z_MOT_ENABLE_PORT,  // NOLINT(cppcoreguidelines-pro-type-cstyle-cast)
                   &GPIO_InitStruct);
+
+    GPIO_InitStruct.Pin = Z_MOT_DIAG0_PIN;
+    GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING_FALLING;
+    GPIO_InitStruct.Pull = GPIO_NOPULL;
+    HAL_GPIO_Init(Z_MOT_STEPDIR_PORT, &GPIO_InitStruct);
 }
 
 #if PCBA_PRIMARY_REVISION != 'b' && PCBA_PRIMARY_REVISION != 'a'
diff --git a/gripper/simulator/interfaces.cpp b/gripper/simulator/interfaces.cpp
index 085c4ad77..104d0575a 100644
--- a/gripper/simulator/interfaces.cpp
+++ b/gripper/simulator/interfaces.cpp
@@ -101,8 +101,9 @@ static stall_check::StallCheck stallcheck(
  * Handler of motor interrupts.
  */
 static motor_handler::MotorInterruptHandler motor_interrupt(
-    motor_queue, gripper_tasks::z_tasks::get_queues(), motor_interface,
-    stallcheck, update_position_queue);
+    motor_queue, gripper_tasks::z_tasks::get_queues(),
+    gripper_tasks::z_tasks::get_queues(), motor_interface, stallcheck,
+    update_position_queue);
 
 static motor_interrupt_driver::MotorInterruptDriver A(motor_queue,
                                                       motor_interrupt,
@@ -142,7 +143,8 @@ static brushed_motor_handler::BrushedMotorInterruptHandler
 static brushed_motor_interrupt_driver::BrushedMotorInterruptDriver G(
     brushed_motor_queue, brushed_motor_interrupt, brushed_motor_hardware_iface);
 
-void z_motor_iface::initialize() {
+void z_motor_iface::initialize(diag0_handler* call_diag0_handler) {
+    static_cast<void>(call_diag0_handler);
     motor_interface.provide_mech_config(z_motor_sys_config);
 };
 
diff --git a/gripper/simulator/main.cpp b/gripper/simulator/main.cpp
index 23ff9edff..6b2a93db5 100644
--- a/gripper/simulator/main.cpp
+++ b/gripper/simulator/main.cpp
@@ -21,6 +21,8 @@
 
 namespace po = boost::program_options;
 
+static z_motor_iface::diag0_handler call_diag0_handler = nullptr;
+
 void signal_handler(int signum) {
     LOG("Interrupt signal (%d) received.", signum);
     exit(signum);
@@ -114,9 +116,9 @@ int main(int argc, char** argv) {
     auto i2c3 = std::make_shared<i2c::hardware::SimI2C>(i2c_device_map);
     static auto canbus =
         can::sim::bus::SimCANBus(can::sim::transport::create(options));
-    z_motor_iface::initialize();
+    z_motor_iface::initialize(&call_diag0_handler);
     grip_motor_iface::initialize();
-    gripper_tasks::start_tasks(
+    call_diag0_handler = gripper_tasks::start_tasks(
         canbus, z_motor_iface::get_z_motor(),
         grip_motor_iface::get_grip_motor(), z_motor_iface::get_spi(),
         z_motor_iface::get_tmc2130_driver_configs(), i2c2, *i2c3,
diff --git a/head/core/can_task.cpp b/head/core/can_task.cpp
index 6cc432b07..dcb1f3359 100644
--- a/head/core/can_task.cpp
+++ b/head/core/can_task.cpp
@@ -28,7 +28,8 @@ using MotorDispatchTarget = can::dispatch::DispatchParseTarget<
     can::message_handlers::motor::MotorHandler<head_tasks::MotorQueueClient>,
     can::messages::ReadMotorDriverRegister,
     can::messages::WriteMotorDriverRegister,
-    can::messages::WriteMotorCurrentRequest>;
+    can::messages::WriteMotorCurrentRequest,
+    can::messages::ReadMotorDriverErrorStatusRequest>;
 using MoveGroupDispatchTarget = can::dispatch::DispatchParseTarget<
     can::message_handlers::move_group::MoveGroupHandler<
         head_tasks::MotorQueueClient>,
diff --git a/head/core/tasks_proto.cpp b/head/core/tasks_proto.cpp
index 2690550ca..c3c09d968 100644
--- a/head/core/tasks_proto.cpp
+++ b/head/core/tasks_proto.cpp
@@ -78,10 +78,25 @@ static auto eeprom_data_rev_update_builder =
     freertos_task::TaskStarter<512, eeprom::data_rev_task::UpdateDataRevTask>{};
 
 static auto tail_accessor = eeprom::dev_data::DevDataTailAccessor{head_queues};
+
+void call_run_diag0_z_interrupt() {
+    if (head_tasks::get_left_tasks().motion_controller) {
+        return head_tasks::get_left_tasks()
+            .motion_controller->run_diag0_interrupt();
+    }
+}
+
+void call_run_diag0_a_interrupt() {
+    if (head_tasks::get_right_tasks().motion_controller) {
+        return head_tasks::get_right_tasks()
+            .motion_controller->run_diag0_interrupt();
+    }
+}
+
 /**
  * Start head tasks.
  */
-void head_tasks::start_tasks(
+auto head_tasks::start_tasks(
     can::bus::CanBus& can_bus,
     motion_controller::MotionController<lms::LeadScrewConfig>&
         left_motion_controller,
@@ -95,7 +110,8 @@ void head_tasks::start_tasks(
     motor_hardware_task::MotorHardwareTask& rmh_tsk,
     motor_hardware_task::MotorHardwareTask& lmh_tsk,
     i2c::hardware::I2CBase& i2c3,
-    eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface) {
+    eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface)
+    -> std::tuple<diag0_handler, diag0_handler> {
     // Start the head tasks
     auto& can_writer = can_task::start_writer(can_bus);
     can_task::start_reader(can_bus);
@@ -141,7 +157,8 @@ void head_tasks::start_tasks(
 
     // Start the left motor tasks
     auto& left_motion = left_mc_task_builder.start(
-        5, "left mc", left_motion_controller, left_queues, left_queues);
+        5, "left mc", left_motion_controller, left_queues, left_queues,
+        left_queues, left_queues);
     auto& left_tmc2130_driver = left_motor_driver_task_builder.start(
         5, "left motor driver", left_driver_configs, left_queues,
         spi3_task_client);
@@ -170,7 +187,8 @@ void head_tasks::start_tasks(
 
     // Start the right motor tasks
     auto& right_motion = right_mc_task_builder.start(
-        5, "right mc", right_motion_controller, right_queues, right_queues);
+        5, "right mc", right_motion_controller, right_queues, right_queues,
+        right_queues, right_queues);
     auto& right_tmc2130_driver = right_motor_driver_task_builder.start(
         5, "right motor driver", right_driver_configs, right_queues,
         spi2_task_client);
@@ -199,6 +217,9 @@ void head_tasks::start_tasks(
     right_queues.move_status_report_queue =
         &right_move_status_reporter.get_queue();
     right_queues.usage_storage_queue = &right_usage_storage_task.get_queue();
+
+    return std::make_tuple(call_run_diag0_z_interrupt,
+                           call_run_diag0_a_interrupt);
 }
 
 // Implementation of HeadQueueClient
@@ -231,6 +252,11 @@ void head_tasks::MotorQueueClient::send_motor_driver_queue(
     motor_queue->try_write(m);
 }
 
+void head_tasks::MotorQueueClient::send_motor_driver_queue_isr(
+    const tmc2130::tasks::TaskMessage& m) {
+    static_cast<void>(motor_queue->try_write_isr(m));
+}
+
 void head_tasks::MotorQueueClient::send_move_group_queue(
     const move_group_task::TaskMessage& m) {
     move_group_queue->try_write(m);
diff --git a/head/core/tasks_rev1.cpp b/head/core/tasks_rev1.cpp
index 2cf2d803d..3e417f116 100644
--- a/head/core/tasks_rev1.cpp
+++ b/head/core/tasks_rev1.cpp
@@ -83,10 +83,25 @@ static auto right_usage_storage_task_builder =
 static auto eeprom_data_rev_update_builder =
     freertos_task::TaskStarter<512, eeprom::data_rev_task::UpdateDataRevTask>{};
 #endif
+
+void call_run_diag0_z_interrupt() {
+    if (head_tasks::get_left_tasks().motion_controller) {
+        return head_tasks::get_left_tasks()
+            .motion_controller->run_diag0_interrupt();
+    }
+}
+
+void call_run_diag0_a_interrupt() {
+    if (head_tasks::get_right_tasks().motion_controller) {
+        return head_tasks::get_right_tasks()
+            .motion_controller->run_diag0_interrupt();
+    }
+}
+
 /**
  * Start head tasks.
  */
-void head_tasks::start_tasks(
+auto head_tasks::start_tasks(
     can::bus::CanBus& can_bus,
     motion_controller::MotionController<lms::LeadScrewConfig>&
         left_motion_controller,
@@ -100,7 +115,8 @@ void head_tasks::start_tasks(
     motor_hardware_task::MotorHardwareTask& rmh_tsk,
     motor_hardware_task::MotorHardwareTask& lmh_tsk,
     i2c::hardware::I2CBase& i2c3,
-    eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface) {
+    eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface)
+    -> std::tuple<head_tasks::diag0_handler, head_tasks::diag0_handler> {
     // Start the head tasks
     auto& can_writer = can_task::start_writer(can_bus);
     can_task::start_reader(can_bus);
@@ -146,7 +162,8 @@ void head_tasks::start_tasks(
 #endif
     // Start the left motor tasks
     auto& left_motion = left_mc_task_builder.start(
-        5, "left mc", left_motion_controller, left_queues, left_queues);
+        5, "left mc", left_motion_controller, left_queues, left_queues,
+        left_queues, left_queues);
     auto& left_tmc2160_driver = left_motor_driver_task_builder.start(
         5, "left motor driver", left_driver_configs, left_queues,
         spi3_task_client);
@@ -183,7 +200,8 @@ void head_tasks::start_tasks(
 
     // Start the right motor tasks
     auto& right_motion = right_mc_task_builder.start(
-        5, "right mc", right_motion_controller, right_queues, right_queues);
+        5, "right mc", right_motion_controller, right_queues, right_queues,
+        right_queues, right_queues);
     auto& right_tmc2160_driver = right_motor_driver_task_builder.start(
         5, "right motor driver", right_driver_configs, right_queues,
         spi2_task_client);
@@ -220,6 +238,9 @@ void head_tasks::start_tasks(
 #if PCBA_PRIMARY_REVISION != 'b'
     right_queues.usage_storage_queue = &right_usage_storage_task.get_queue();
 #endif
+
+    return std::make_tuple(call_run_diag0_z_interrupt,
+                           call_run_diag0_a_interrupt);
 }
 
 // Implementation of HeadQueueClient
@@ -256,6 +277,11 @@ void head_tasks::MotorQueueClient::send_motor_driver_queue(
     motor_queue->try_write(m);
 }
 
+void head_tasks::MotorQueueClient::send_motor_driver_queue_isr(
+    const tmc::tasks::TaskMessage& m) {
+    static_cast<void>(motor_queue->try_write_isr(m));
+}
+
 void head_tasks::MotorQueueClient::send_move_group_queue(
     const move_group_task::TaskMessage& m) {
     move_group_queue->try_write(m);
diff --git a/head/firmware/main_proto.cpp b/head/firmware/main_proto.cpp
index d3ba07641..614af1142 100644
--- a/head/firmware/main_proto.cpp
+++ b/head/firmware/main_proto.cpp
@@ -1,6 +1,7 @@
 #include <array>
 #include <cstdio>
 #include <cstring>
+#include <tuple>
 
 // clang-format off
 #include "FreeRTOS.h"
@@ -40,6 +41,9 @@
 
 static auto iWatchdog = iwdg::IndependentWatchDog{};
 
+static head_tasks::diag0_handler call_diag0_z_handler = nullptr;
+static head_tasks::diag0_handler call_diag0_a_handler = nullptr;
+
 static auto can_bus_1 = can::hal::bus::HalCanBus(
     can_get_device_handle(),
     gpio::PinConfig{// NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
@@ -142,10 +146,16 @@ struct motor_hardware::HardwareConfig pin_configurations_left {
             .port = GPIOA,
             .pin = GPIO_PIN_8,
             .active_setting = GPIO_PIN_RESET},
-    .estop_in = {
+    .estop_in =
+        {
+            // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
+            .port = GPIOB,
+            .pin = GPIO_PIN_4,
+            .active_setting = GPIO_PIN_RESET},
+    .diag0 = {
         // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
-        .port = GPIOB,
-        .pin = GPIO_PIN_4,
+        .port = GPIOC,
+        .pin = GPIO_PIN_13,
         .active_setting = GPIO_PIN_RESET}
 };
 
@@ -186,10 +196,16 @@ struct motor_hardware::HardwareConfig pin_configurations_right {
             .port = GPIOA,
             .pin = GPIO_PIN_8,
             .active_setting = GPIO_PIN_RESET},
-    .estop_in = {
+    .estop_in =
+        {
+            // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
+            .port = GPIOB,
+            .pin = GPIO_PIN_4,
+            .active_setting = GPIO_PIN_RESET},
+    .diag0 = {
         // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
-        .port = GPIOB,
-        .pin = GPIO_PIN_4,
+        .port = GPIOC,
+        .pin = GPIO_PIN_15,
         .active_setting = GPIO_PIN_RESET}
 };
 
@@ -197,7 +213,7 @@ struct motor_hardware::HardwareConfig pin_configurations_right {
 static tmc2130::configs::TMC2130DriverConfig motor_driver_configs_right{
     .registers =
         {
-            .gconfig = {.en_pwm_mode = 1},
+            .gconfig = {.en_pwm_mode = 1, .diag0_error = 1},
             .ihold_irun = {.hold_current = 0xB,
                            .run_current = 0x19,
                            .hold_current_delay = 0x7},
@@ -224,7 +240,7 @@ static tmc2130::configs::TMC2130DriverConfig motor_driver_configs_right{
 static tmc2130::configs::TMC2130DriverConfig motor_driver_configs_left{
     .registers =
         {
-            .gconfig = {.en_pwm_mode = 1},
+            .gconfig = {.en_pwm_mode = 1, .diag0_error = 1},
             .ihold_irun = {.hold_current = 0xB,
                            .run_current = 0x19,
                            .hold_current_delay = 0x7},
@@ -268,8 +284,9 @@ static stall_check::StallCheck stallcheck_right(
 static motor_hardware::MotorHardware motor_hardware_right(
     pin_configurations_right, &htim7, &htim2, right_usage_config);
 static motor_handler::MotorInterruptHandler motor_interrupt_right(
-    motor_queue_right, head_tasks::get_right_queues(), motor_hardware_right,
-    stallcheck_right, update_position_queue_right);
+    motor_queue_right, head_tasks::get_right_queues(),
+    head_tasks::get_right_queues(), motor_hardware_right, stallcheck_right,
+    update_position_queue_right);
 
 static auto encoder_background_timer_right =
     motor_encoder::BackgroundTimer(motor_interrupt_right, motor_hardware_right);
@@ -289,8 +306,9 @@ static stall_check::StallCheck stallcheck_left(
 static motor_hardware::MotorHardware motor_hardware_left(
     pin_configurations_left, &htim7, &htim3, left_usage_config);
 static motor_handler::MotorInterruptHandler motor_interrupt_left(
-    motor_queue_left, head_tasks::get_left_queues(), motor_hardware_left,
-    stallcheck_left, update_position_queue_left);
+    motor_queue_left, head_tasks::get_left_queues(),
+    head_tasks::get_left_queues(), motor_hardware_left, stallcheck_left,
+    update_position_queue_left);
 
 static auto encoder_background_timer_left =
     motor_encoder::BackgroundTimer(motor_interrupt_left, motor_hardware_left);
@@ -393,7 +411,8 @@ auto main() -> int {
 
     app_update_clear_flags();
     initialize_timer(motor_callback_glue, left_enc_overflow_callback_glue,
-                     right_enc_overflow_callback_glue);
+                     right_enc_overflow_callback_glue, &call_diag0_z_handler,
+                     &call_diag0_a_handler);
 
     if (initialize_spi(&hspi2) != HAL_OK) {
         Error_Handler();
@@ -407,12 +426,12 @@ auto main() -> int {
 
     i2c_setup(&i2c_handles);
     i2c_comms3.set_handle(i2c_handles.i2c3);
-
-    head_tasks::start_tasks(can_bus_1, motor_left.motion_controller,
-                            motor_right.motion_controller, psd, spi_comms2,
-                            spi_comms3, motor_driver_configs_left,
-                            motor_driver_configs_right, rmh_tsk, lmh_tsk,
-                            i2c_comms3, eeprom_hw_iface);
+    std::tie(call_diag0_z_handler, call_diag0_a_handler) =
+        head_tasks::start_tasks(can_bus_1, motor_left.motion_controller,
+                                motor_right.motion_controller, psd, spi_comms2,
+                                spi_comms3, motor_driver_configs_left,
+                                motor_driver_configs_right, rmh_tsk, lmh_tsk,
+                                i2c_comms3, eeprom_hw_iface);
 
     timer_for_notifier.start();
 
diff --git a/head/firmware/main_rev1.cpp b/head/firmware/main_rev1.cpp
index ce5849129..e6d17776f 100644
--- a/head/firmware/main_rev1.cpp
+++ b/head/firmware/main_rev1.cpp
@@ -1,6 +1,7 @@
 #include <array>
 #include <cstdio>
 #include <cstring>
+#include <tuple>
 
 // clang-format off
 #include "FreeRTOS.h"
@@ -40,6 +41,9 @@
 
 static auto iWatchdog = iwdg::IndependentWatchDog{};
 
+static head_tasks::diag0_handler call_diag0_z_handler = nullptr;
+static head_tasks::diag0_handler call_diag0_a_handler = nullptr;
+
 static auto can_bus_1 = can::hal::bus::HalCanBus(
     can_get_device_handle(),
     gpio::PinConfig{// NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
@@ -154,6 +158,12 @@ struct motor_hardware::HardwareConfig pin_configurations_left {
             .port = GPIOB,
             .pin = GPIO_PIN_4,
             .active_setting = GPIO_PIN_RESET},
+    .diag0 =
+        {
+            // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
+            .port = GPIOC,
+            .pin = GPIO_PIN_13,
+            .active_setting = GPIO_PIN_RESET},
     .ebrake = gpio::PinConfig {
         // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
         .port = GPIOB, .pin = GPIO_PIN_5, .active_setting = GPIO_PIN_RESET
@@ -198,6 +208,12 @@ struct motor_hardware::HardwareConfig pin_configurations_right {
             .port = GPIOB,
             .pin = GPIO_PIN_4,
             .active_setting = GPIO_PIN_RESET},
+    .diag0 =
+        {
+            // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
+            .port = GPIOC,
+            .pin = GPIO_PIN_15,
+            .active_setting = GPIO_PIN_RESET},
     .ebrake = gpio::PinConfig {
         // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
         .port = GPIOB, .pin = GPIO_PIN_0, .active_setting = GPIO_PIN_RESET
@@ -206,7 +222,7 @@ struct motor_hardware::HardwareConfig pin_configurations_right {
 
 // TODO clean up the head main file by using interfaces.
 static tmc2160::configs::TMC2160DriverConfig motor_driver_configs_right{
-    .registers = {.gconfig = {.en_pwm_mode = 0},
+    .registers = {.gconfig = {.en_pwm_mode = 0, .diag0_error = 1},
                   .ihold_irun = {.hold_current = 16,
                                  .run_current = 31,
                                  .hold_current_delay = 0x7},
@@ -232,7 +248,7 @@ static tmc2160::configs::TMC2160DriverConfig motor_driver_configs_right{
     }};
 
 static tmc2160::configs::TMC2160DriverConfig motor_driver_configs_left{
-    .registers = {.gconfig = {.en_pwm_mode = 0},
+    .registers = {.gconfig = {.en_pwm_mode = 0, .diag0_error = 1},
                   .ihold_irun = {.hold_current = 16,
                                  .run_current = 31,
                                  .hold_current_delay = 0x7},
@@ -286,8 +302,9 @@ static stall_check::StallCheck stallcheck_right(
 static motor_hardware::MotorHardware motor_hardware_right(
     pin_configurations_right, &htim7, &htim2, right_usage_config);
 static motor_handler::MotorInterruptHandler motor_interrupt_right(
-    motor_queue_right, head_tasks::get_right_queues(), motor_hardware_right,
-    stallcheck_right, update_position_queue_right);
+    motor_queue_right, head_tasks::get_right_queues(),
+    head_tasks::get_right_queues(), motor_hardware_right, stallcheck_right,
+    update_position_queue_right);
 
 static auto encoder_background_timer_right =
     motor_encoder::BackgroundTimer(motor_interrupt_right, motor_hardware_right);
@@ -311,8 +328,9 @@ static stall_check::StallCheck stallcheck_left(
 static motor_hardware::MotorHardware motor_hardware_left(
     pin_configurations_left, &htim7, &htim3, left_usage_config);
 static motor_handler::MotorInterruptHandler motor_interrupt_left(
-    motor_queue_left, head_tasks::get_left_queues(), motor_hardware_left,
-    stallcheck_left, update_position_queue_left);
+    motor_queue_left, head_tasks::get_left_queues(),
+    head_tasks::get_left_queues(), motor_hardware_left, stallcheck_left,
+    update_position_queue_left);
 
 static auto encoder_background_timer_left =
     motor_encoder::BackgroundTimer(motor_interrupt_left, motor_hardware_left);
@@ -418,7 +436,8 @@ auto main() -> int {
     app_update_clear_flags();
 
     initialize_timer(motor_callback_glue, left_enc_overflow_callback_glue,
-                     right_enc_overflow_callback_glue);
+                     right_enc_overflow_callback_glue, &call_diag0_z_handler,
+                     &call_diag0_a_handler);
 
     i2c_setup(&i2c_handles);
     i2c_comms3.set_handle(i2c_handles.i2c3);
@@ -432,11 +451,12 @@ auto main() -> int {
 
     utility_gpio_init();
     can_bus_1.start(can_bit_timings);
-    head_tasks::start_tasks(can_bus_1, motor_left.motion_controller,
-                            motor_right.motion_controller, psd, spi_comms2,
-                            spi_comms3, motor_driver_configs_left,
-                            motor_driver_configs_right, rmh_tsk, lmh_tsk,
-                            i2c_comms3, eeprom_hw_iface);
+    std::tie(call_diag0_z_handler, call_diag0_a_handler) =
+        head_tasks::start_tasks(can_bus_1, motor_left.motion_controller,
+                                motor_right.motion_controller, psd, spi_comms2,
+                                spi_comms3, motor_driver_configs_left,
+                                motor_driver_configs_right, rmh_tsk, lmh_tsk,
+                                i2c_comms3, eeprom_hw_iface);
 
     timer_for_notifier.start();
 
diff --git a/head/firmware/motor_hardware.h b/head/firmware/motor_hardware.h
index e01e68d85..2cce7382f 100644
--- a/head/firmware/motor_hardware.h
+++ b/head/firmware/motor_hardware.h
@@ -14,11 +14,14 @@ extern TIM_HandleTypeDef htim3;
 
 typedef void (*motor_interrupt_callback)();
 typedef void (*encoder_overflow_callback)(int32_t);
+typedef void (*diag0_interrupt_callback)();
 
 HAL_StatusTypeDef initialize_spi(SPI_HandleTypeDef* hspi);
 void initialize_timer(motor_interrupt_callback callback,
                       encoder_overflow_callback left_enc_overflow_callback,
-                      encoder_overflow_callback right_enc_overflow_callback);
+                      encoder_overflow_callback right_enc_overflow_callback,
+                      diag0_interrupt_callback* diag0_z_int_callback,
+                      diag0_interrupt_callback* diag0_a_int_callback);
 void initialize_rev_specific_pins();
 
 #ifdef __cplusplus
diff --git a/head/firmware/motor_hardware_common.c b/head/firmware/motor_hardware_common.c
index d1313df4d..e895252fe 100644
--- a/head/firmware/motor_hardware_common.c
+++ b/head/firmware/motor_hardware_common.c
@@ -21,6 +21,8 @@ TIM_HandleTypeDef htim3 = {
 motor_interrupt_callback motor_callback = NULL;
 encoder_overflow_callback left_enc_overflow_callback = NULL;
 encoder_overflow_callback right_enc_overflow_callback = NULL;
+static diag0_interrupt_callback* diag0_z_callback = NULL;
+static diag0_interrupt_callback* diag0_a_callback = NULL;
 
 void HAL_SPI_MspInit(SPI_HandleTypeDef *hspi) {
     GPIO_InitTypeDef GPIO_InitStruct = {0};
@@ -55,6 +57,14 @@ void HAL_SPI_MspInit(SPI_HandleTypeDef *hspi) {
         GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
         HAL_GPIO_Init(GPIOC,  // NOLINT(cppcoreguidelines-pro-type-cstyle-cast)
                       &GPIO_InitStruct);
+
+        // A motor diag0
+        GPIO_InitStruct.Pin = GPIO_PIN_15;
+        GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING_FALLING;
+        GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+        GPIO_InitStruct.Pull = GPIO_NOPULL;
+        HAL_GPIO_Init(GPIOC,  // NOLINT(cppcoreguidelines-pro-type-cstyle-cast)
+                      &GPIO_InitStruct);
     }
 
     else if (hspi->Instance == SPI3) {
@@ -91,6 +101,14 @@ void HAL_SPI_MspInit(SPI_HandleTypeDef *hspi) {
         GPIO_InitStruct.Pull = GPIO_NOPULL;
         HAL_GPIO_Init(GPIOC,  // NOLINT(cppcoreguidelines-pro-type-cstyle-cast)
                       &GPIO_InitStruct);
+
+        // Z motor diag0
+        GPIO_InitStruct.Pin = GPIO_PIN_13;
+        GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING_FALLING;
+        GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+        GPIO_InitStruct.Pull = GPIO_NOPULL;
+        HAL_GPIO_Init(GPIOC,  // NOLINT(cppcoreguidelines-pro-type-cstyle-cast)
+                      &GPIO_InitStruct);
     }
 }
 
@@ -229,6 +247,9 @@ void MX_GPIO_Init(void) {
     __HAL_RCC_GPIOA_CLK_ENABLE();
     __HAL_RCC_GPIOB_CLK_ENABLE();
     initialize_rev_specific_pins();
+
+    HAL_NVIC_SetPriority(EXTI15_10_IRQn, 5, 0);
+    HAL_NVIC_EnableIRQ(EXTI15_10_IRQn);
 }
 
 #if PCBA_PRIMARY_REVISION == 'b' || PCBA_PRIMARY_REVISION == 'a'
@@ -345,15 +366,31 @@ void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
         // disable both left and right enable pins
         HAL_GPIO_WritePin(GPIOC, GPIO_PIN_4, GPIO_PIN_RESET);
         HAL_GPIO_WritePin(GPIOB, GPIO_PIN_11, GPIO_PIN_RESET);
+    } else if (GPIO_Pin == GPIO_PIN_13) {
+        if (diag0_z_callback != NULL) {
+            if (*diag0_z_callback != NULL) {
+                (*diag0_z_callback)();
+            }
+        }
+    } else if (GPIO_Pin == GPIO_PIN_15) {
+        if (diag0_a_callback != NULL) {
+            if (*diag0_a_callback != NULL) {
+                (*diag0_a_callback)();
+            }
+        }
     }
 }
 
 void initialize_timer(motor_interrupt_callback callback,
                       encoder_overflow_callback l_f_callback,
-                      encoder_overflow_callback r_f_callback) {
+                      encoder_overflow_callback r_f_callback,
+                      diag0_interrupt_callback* diag0_z_int_callback,
+                      diag0_interrupt_callback* diag0_a_int_callback) {
     motor_callback = callback;
     left_enc_overflow_callback = l_f_callback;
     right_enc_overflow_callback = r_f_callback;
+    diag0_z_callback = diag0_z_int_callback;
+    diag0_a_callback = diag0_a_int_callback;
     MX_GPIO_Init();
     Encoder_GPIO_Init();
     encoder_init(&htim2);
diff --git a/head/firmware/stm32g4xx_it.c b/head/firmware/stm32g4xx_it.c
index 6e031a2bf..929586b89 100644
--- a/head/firmware/stm32g4xx_it.c
+++ b/head/firmware/stm32g4xx_it.c
@@ -177,6 +177,14 @@ void TIM2_IRQHandler(void) { HAL_TIM_IRQHandler(&htim2); }
  */
 void TIM3_IRQHandler(void) { HAL_TIM_IRQHandler(&htim3); }
 
+void EXTI15_10_IRQHandler(void) {
+    if (__HAL_GPIO_EXTI_GET_IT(GPIO_PIN_13)) {
+        HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_13);
+    } else if (__HAL_GPIO_EXTI_GET_IT(GPIO_PIN_15)) {
+        HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_15);
+    }
+}
+
 extern void xPortSysTickHandler(void);
 void SysTick_Handler(void) {
     HAL_IncTick();
diff --git a/head/simulator/main.cpp b/head/simulator/main.cpp
index 79db8f736..907f60bdb 100644
--- a/head/simulator/main.cpp
+++ b/head/simulator/main.cpp
@@ -94,8 +94,9 @@ static stall_check::StallCheck stallcheck_right(
     linear_config.get_usteps_per_mm() / 1000.0F, utils::STALL_THRESHOLD_UM);
 
 static motor_handler::MotorInterruptHandler motor_interrupt_right(
-    motor_queue_right, head_tasks::get_right_queues(), motor_interface_right,
-    stallcheck_right, update_position_queue_right);
+    motor_queue_right, head_tasks::get_right_queues(),
+    head_tasks::get_right_queues(), motor_interface_right, stallcheck_right,
+    update_position_queue_right);
 
 static stall_check::StallCheck stallcheck_left(
     linear_config.get_encoder_pulses_per_mm() / 1000.0F,
@@ -117,8 +118,9 @@ static motor_class::Motor motor_right{
     motor_queue_right, update_position_queue_right};
 
 static motor_handler::MotorInterruptHandler motor_interrupt_left(
-    motor_queue_left, head_tasks::get_left_queues(), motor_interface_left,
-    stallcheck_left, update_position_queue_left);
+    motor_queue_left, head_tasks::get_left_queues(),
+    head_tasks::get_left_queues(), motor_interface_left, stallcheck_left,
+    update_position_queue_left);
 
 static motor_class::Motor motor_left{
     motor_sys_config, motor_interface_left,
diff --git a/include/can/core/ids.hpp b/include/can/core/ids.hpp
index 636a70cdd..d5b3a2d48 100644
--- a/include/can/core/ids.hpp
+++ b/include/can/core/ids.hpp
@@ -64,6 +64,8 @@ enum class MessageId {
     write_motor_current_request = 0x33,
     read_motor_current_request = 0x34,
     read_motor_current_response = 0x35,
+    read_motor_driver_error_status_request = 0x36,
+    read_motor_driver_error_status_response = 0x37,
     set_brushed_motor_vref_request = 0x40,
     set_brushed_motor_pwm_request = 0x41,
     gripper_grip_request = 0x42,
@@ -169,11 +171,13 @@ enum class ErrorCode {
     over_pressure = 0xd,
     door_open = 0xe,
     reed_open = 0xf,
+    motor_driver_error_detected = 0x10,
     safety_relay_inactive = 0x11,
 };
 
 /** Error Severity levels. */
 enum class ErrorSeverity {
+    none = 0x0,
     warning = 0x1,
     recoverable = 0x2,
     unrecoverable = 0x3,
diff --git a/include/can/core/messages.hpp b/include/can/core/messages.hpp
index 3ad1da7c5..71654f7de 100644
--- a/include/can/core/messages.hpp
+++ b/include/can/core/messages.hpp
@@ -646,6 +646,41 @@ struct ReadMotorDriverRegisterResponse
         -> bool = default;
 };
 
+struct ReadMotorDriverErrorStatusRequest
+    : BaseMessage<MessageId::read_motor_driver_error_status_request> {
+    uint32_t message_index;
+
+    template <bit_utils::ByteIterator Input, typename Limit>
+    static auto parse(Input body, Limit limit)
+        -> ReadMotorDriverErrorStatusRequest {
+        uint32_t msg_ind = 0;
+
+        body = bit_utils::bytes_to_int(body, limit, msg_ind);
+        return ReadMotorDriverErrorStatusRequest{.message_index = msg_ind};
+    }
+
+    auto operator==(const ReadMotorDriverErrorStatusRequest& other) const
+        -> bool = default;
+};
+
+struct ReadMotorDriverErrorStatusResponse
+    : BaseMessage<MessageId::read_motor_driver_error_status_response> {
+    uint32_t message_index;
+    uint8_t reg_address;
+    uint32_t data;
+
+    template <bit_utils::ByteIterator Output, typename Limit>
+    auto serialize(Output body, Limit limit) const -> uint8_t {
+        auto iter = bit_utils::int_to_bytes(message_index, body, limit);
+        iter = bit_utils::int_to_bytes(reg_address, iter, limit);
+        iter = bit_utils::int_to_bytes(data, iter, limit);
+        return iter - body;
+    }
+
+    auto operator==(const ReadMotorDriverErrorStatusResponse& other) const
+        -> bool = default;
+};
+
 struct WriteMotorCurrentRequest
     : BaseMessage<MessageId::write_motor_current_request> {
     uint32_t message_index;
@@ -1851,6 +1886,7 @@ using ResponseMessageType = std::variant<
     UpdateMotorPositionEstimationResponse, BaselineSensorResponse,
     PushTipPresenceNotification, GetMotorUsageResponse, GripperJawStateResponse,
     GripperJawHoldoffResponse, HepaUVInfoResponse, GetHepaFanStateResponse,
-    GetHepaUVStateResponse, MotorStatusResponse, GearMotorStatusResponse>;
+    GetHepaUVStateResponse, MotorStatusResponse, GearMotorStatusResponse,
+    ReadMotorDriverErrorStatusResponse>;
 
 }  // namespace can::messages
diff --git a/include/gantry/core/can_task.hpp b/include/gantry/core/can_task.hpp
index 2c4918fff..53bf495f9 100644
--- a/include/gantry/core/can_task.hpp
+++ b/include/gantry/core/can_task.hpp
@@ -21,7 +21,8 @@ using MotorDispatchTarget = can::dispatch::DispatchParseTarget<
     can::message_handlers::motor::MotorHandler<gantry::queues::QueueClient>,
     can::messages::ReadMotorDriverRegister,
     can::messages::WriteMotorDriverRegister,
-    can::messages::WriteMotorCurrentRequest>;
+    can::messages::WriteMotorCurrentRequest,
+    can::messages::ReadMotorDriverErrorStatusRequest>;
 using MoveGroupDispatchTarget = can::dispatch::DispatchParseTarget<
     can::message_handlers::move_group::MoveGroupHandler<
         gantry::queues::QueueClient>,
diff --git a/include/gantry/core/interfaces_proto.hpp b/include/gantry/core/interfaces_proto.hpp
index d58ea63fd..ad72967b2 100644
--- a/include/gantry/core/interfaces_proto.hpp
+++ b/include/gantry/core/interfaces_proto.hpp
@@ -9,10 +9,12 @@
 
 namespace interfaces {
 
+extern "C" using diag0_handler = void(*)();
+
 /**
  * Initialize the hardware portability layer.
  */
-void initialize();
+void initialize(diag0_handler *call_diag0_handler);
 
 /**
  * Get the CAN bus interface.
diff --git a/include/gantry/core/interfaces_rev1.hpp b/include/gantry/core/interfaces_rev1.hpp
index 129d2ac68..fa276cbff 100644
--- a/include/gantry/core/interfaces_rev1.hpp
+++ b/include/gantry/core/interfaces_rev1.hpp
@@ -9,10 +9,12 @@
 
 namespace interfaces {
 
+extern "C" using diag0_handler = void(*)();
+
 /**
  * Initialize the hardware portability layer.
  */
-void initialize();
+void initialize(diag0_handler *call_diag0_handler);
 
 /**
  * Get the CAN bus interface.
diff --git a/include/gantry/core/queues.hpp b/include/gantry/core/queues.hpp
index 3007e49cb..42884aa01 100644
--- a/include/gantry/core/queues.hpp
+++ b/include/gantry/core/queues.hpp
@@ -32,6 +32,8 @@ struct QueueClient : can::message_writer::MessageWriter {
 
     void send_motor_driver_queue(const tmc::tasks::TaskMessage& m);
 
+    void send_motor_driver_queue_isr(const tmc::tasks::TaskMessage& m);
+
     void send_move_group_queue(const move_group_task::TaskMessage& m);
 
     void send_move_status_reporter_queue(
diff --git a/include/gantry/core/tasks_proto.hpp b/include/gantry/core/tasks_proto.hpp
index c9fe5be64..8fe6846d4 100644
--- a/include/gantry/core/tasks_proto.hpp
+++ b/include/gantry/core/tasks_proto.hpp
@@ -6,6 +6,7 @@
 #include "eeprom/core/hardware_iface.hpp"
 #include "eeprom/core/task.hpp"
 #include "eeprom/core/update_data_rev_task.hpp"
+#include "gantry/core/interfaces_proto.hpp"
 #include "i2c/core/hardware_iface.hpp"
 #include "i2c/core/tasks/i2c_poller_task.hpp"
 #include "i2c/core/tasks/i2c_task.hpp"
@@ -28,14 +29,17 @@ namespace tasks {
 /**
  * Start gantry tasks.
  */
-void start_tasks(
+auto start_tasks(
     can::bus::CanBus& can_bus,
     motion_controller::MotionController<lms::BeltConfig>& motion_controller,
     spi::hardware::SpiDeviceBase& spi_device,
     tmc2130::configs::TMC2130DriverConfig& driver_configs,
     motor_hardware_task::MotorHardwareTask& mh_tsk,
     i2c::hardware::I2CBase& i2c2,
-    eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface);
+    eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface)
+    -> interfaces::diag0_handler;
+
+void call_run_diag0_interrupt();
 
 /**
  * Access to all tasks in the system.
diff --git a/include/gantry/core/tasks_rev1.hpp b/include/gantry/core/tasks_rev1.hpp
index 0a90e559d..ea086c412 100644
--- a/include/gantry/core/tasks_rev1.hpp
+++ b/include/gantry/core/tasks_rev1.hpp
@@ -6,6 +6,7 @@
 #include "eeprom/core/hardware_iface.hpp"
 #include "eeprom/core/task.hpp"
 #include "eeprom/core/update_data_rev_task.hpp"
+#include "gantry/core/interfaces_rev1.hpp"
 #include "i2c/core/hardware_iface.hpp"
 #include "i2c/core/tasks/i2c_poller_task.hpp"
 #include "i2c/core/tasks/i2c_task.hpp"
@@ -28,14 +29,17 @@ namespace tasks {
 /**
  * Start gantry tasks.
  */
-void start_tasks(
+auto start_tasks(
     can::bus::CanBus& can_bus,
     motion_controller::MotionController<lms::BeltConfig>& motion_controller,
     spi::hardware::SpiDeviceBase& spi_device,
     tmc2160::configs::TMC2160DriverConfig& driver_configs,
     motor_hardware_task::MotorHardwareTask& mh_tsk,
     i2c::hardware::I2CBase& i2c2,
-    eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface);
+    eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface)
+    -> interfaces::diag0_handler;
+
+void call_run_diag0_interrupt();
 
 /**
  * Access to all tasks in the system.
diff --git a/include/gripper/core/can_task.hpp b/include/gripper/core/can_task.hpp
index c31568ded..49a70ad44 100644
--- a/include/gripper/core/can_task.hpp
+++ b/include/gripper/core/can_task.hpp
@@ -18,7 +18,8 @@ using MotorDispatchTarget = can::dispatch::DispatchParseTarget<
     can::message_handlers::motor::MotorHandler<z_tasks::QueueClient>,
     can::messages::ReadMotorDriverRegister,
     can::messages::WriteMotorDriverRegister,
-    can::messages::WriteMotorCurrentRequest>;
+    can::messages::WriteMotorCurrentRequest,
+    can::messages::ReadMotorDriverErrorStatusRequest>;
 #ifdef USE_SENSOR_MOVE
 using MoveGroupDispatchTarget = can::dispatch::DispatchParseTarget<
     can::message_handlers::move_group::MoveGroupHandler<z_tasks::QueueClient>,
diff --git a/include/gripper/core/interfaces.hpp b/include/gripper/core/interfaces.hpp
index f11dac6ef..5430d5801 100644
--- a/include/gripper/core/interfaces.hpp
+++ b/include/gripper/core/interfaces.hpp
@@ -11,7 +11,9 @@
 
 namespace z_motor_iface {
 
-void initialize();
+extern "C" using diag0_handler = void(*)();
+
+void initialize(diag0_handler *call_diag0_handler);
 
 /**
  * Access to the z motor.
diff --git a/include/gripper/core/tasks.hpp b/include/gripper/core/tasks.hpp
index c61ef164b..1de5f7c06 100644
--- a/include/gripper/core/tasks.hpp
+++ b/include/gripper/core/tasks.hpp
@@ -7,6 +7,7 @@
 #include "eeprom/core/hardware_iface.hpp"
 #include "eeprom/core/task.hpp"
 #include "eeprom/core/update_data_rev_task.hpp"
+#include "gripper/core/interfaces.hpp"
 #include "i2c/core/hardware_iface.hpp"
 #include "i2c/core/tasks/i2c_poller_task.hpp"
 #include "i2c/core/tasks/i2c_task.hpp"
@@ -36,7 +37,7 @@ namespace gripper_tasks {
 /**
  * Start gripper tasks.
  */
-void start_tasks(can::bus::CanBus& can_bus,
+auto start_tasks(can::bus::CanBus& can_bus,
                  motor_class::Motor<lms::LeadScrewConfig>& z_motor,
                  brushed_motor::BrushedMotor<lms::GearBoxConfig>& grip_motor,
                  spi::hardware::SpiDeviceBase& spi_device,
@@ -45,7 +46,8 @@ void start_tasks(can::bus::CanBus& can_bus,
                  sensors::hardware::SensorHardwareBase& sensor_hardware,
                  eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface,
                  motor_hardware_task::MotorHardwareTask& zmh_tsk,
-                 motor_hardware_task::MotorHardwareTask& gmh_tsk);
+                 motor_hardware_task::MotorHardwareTask& gmh_tsk)
+    -> z_motor_iface::diag0_handler;
 
 /**
  * Access to all the message queues in the system.
@@ -184,13 +186,15 @@ struct AllTask {
 
 namespace z_tasks {
 
-void start_task(
+auto start_task(
     motor_class::Motor<lms::LeadScrewConfig>& z_motor,
     spi::hardware::SpiDeviceBase& spi_device,
     tmc2130::configs::TMC2130DriverConfig& driver_configs, AllTask& tasks,
     gripper_tasks::QueueClient& main_queues,
     eeprom::dev_data::DevDataTailAccessor<gripper_tasks::QueueClient>&
-        tail_accessor);
+        tail_accessor) -> z_motor_iface::diag0_handler;
+
+void call_run_diag0_interrupt();
 
 struct QueueClient : can::message_writer::MessageWriter {
     QueueClient();
@@ -200,6 +204,8 @@ struct QueueClient : can::message_writer::MessageWriter {
 
     void send_motor_driver_queue(const tmc2130::tasks::TaskMessage& m);
 
+    void send_motor_driver_queue_isr(const tmc2130::tasks::TaskMessage& m);
+
     void send_move_group_queue(const move_group_task::TaskMessage& m);
 
     void send_move_status_reporter_queue(
diff --git a/include/gripper/firmware/utility_gpio.h b/include/gripper/firmware/utility_gpio.h
index 4cb96d005..c0267033a 100644
--- a/include/gripper/firmware/utility_gpio.h
+++ b/include/gripper/firmware/utility_gpio.h
@@ -53,6 +53,7 @@ void utility_gpio_init();
 // z motor pins
 #define Z_MOT_DIR_PIN GPIO_PIN_10
 #define Z_MOT_STEP_PIN GPIO_PIN_1
+#define Z_MOT_DIAG0_PIN GPIO_PIN_2
 #define Z_MOT_STEPDIR_PORT GPIOB
 #define Z_MOT_ENABLE_PIN GPIO_PIN_9
 #define Z_MOT_ENABLE_PORT GPIOA
diff --git a/include/gripper/simulation/sim_interfaces.hpp b/include/gripper/simulation/sim_interfaces.hpp
index 4de28bad4..a3750ab63 100644
--- a/include/gripper/simulation/sim_interfaces.hpp
+++ b/include/gripper/simulation/sim_interfaces.hpp
@@ -4,6 +4,8 @@
 
 namespace z_motor_iface {
 
+extern "C" using diag0_handler = void(*)();
+
 auto get_z_motor_interface()
     -> sim_motor_hardware_iface::SimMotorHardwareIface&;
 
diff --git a/include/head/core/queues.hpp b/include/head/core/queues.hpp
index 2552275ff..6a07d9cd1 100644
--- a/include/head/core/queues.hpp
+++ b/include/head/core/queues.hpp
@@ -55,6 +55,8 @@ struct MotorQueueClient : can::message_writer::MessageWriter {
 
     void send_motor_driver_queue(const tmc::tasks::TaskMessage& m);
 
+    void send_motor_driver_queue_isr(const tmc::tasks::TaskMessage& m);
+
     void send_move_group_queue(const move_group_task::TaskMessage& m);
 
     void send_move_status_reporter_queue(
diff --git a/include/head/core/tasks_proto.hpp b/include/head/core/tasks_proto.hpp
index 6f87e5286..8be17643e 100644
--- a/include/head/core/tasks_proto.hpp
+++ b/include/head/core/tasks_proto.hpp
@@ -24,11 +24,12 @@
 
 namespace head_tasks {
 
+extern "C" using diag0_handler = void(*)();
+
 /**
  * Start head tasks.
  */
-
-void start_tasks(
+auto start_tasks(
     can::bus::CanBus& can_bus,
     motion_controller::MotionController<lms::LeadScrewConfig>&
         left_motion_controller,
@@ -42,7 +43,8 @@ void start_tasks(
     motor_hardware_task::MotorHardwareTask& right_motor_hardware,
     motor_hardware_task::MotorHardwareTask& left_motor_hardware,
     i2c::hardware::I2CBase& i2c3,
-    eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface);
+    eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface)
+    -> std::tuple<diag0_handler, diag0_handler>;
 
 /**
  * Access to all tasks not associated with a motor. This will be a singleton.
diff --git a/include/head/core/tasks_rev1.hpp b/include/head/core/tasks_rev1.hpp
index bbb375586..430c89cf4 100644
--- a/include/head/core/tasks_rev1.hpp
+++ b/include/head/core/tasks_rev1.hpp
@@ -24,11 +24,12 @@
 
 namespace head_tasks {
 
+extern "C" using diag0_handler = void(*)();
+
 /**
  * Start head tasks.
  */
-
-void start_tasks(
+auto start_tasks(
     can::bus::CanBus& can_bus,
     motion_controller::MotionController<lms::LeadScrewConfig>&
         left_motion_controller,
@@ -42,7 +43,8 @@ void start_tasks(
     motor_hardware_task::MotorHardwareTask& right_motor_hardware,
     motor_hardware_task::MotorHardwareTask& left_motor_hardware,
     i2c::hardware::I2CBase& i2c3,
-    eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface);
+    eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface)
+    -> std::tuple<diag0_handler, diag0_handler>;
 
 /**
  * Access to all tasks not associated with a motor. This will be a singleton.
diff --git a/include/motor-control/core/brushed_motor/brushed_motion_controller.hpp b/include/motor-control/core/brushed_motor/brushed_motion_controller.hpp
index 00ec0dddf..113f56214 100644
--- a/include/motor-control/core/brushed_motor/brushed_motion_controller.hpp
+++ b/include/motor-control/core/brushed_motor/brushed_motion_controller.hpp
@@ -101,12 +101,15 @@ class MotionController {
         enabled = false;
     }
 
-    void stop() {
+    void stop(
+        can::ids::ErrorSeverity error_severity =
+            can::ids::ErrorSeverity::warning,
+        can::ids::ErrorCode error_code = can::ids::ErrorCode::stop_requested) {
         queue.reset();
         // if we're gripping something we need to flag this so we don't drop it
         if (!hardware.get_stay_enabled()) {
             if (hardware.is_timer_interrupt_running()) {
-                hardware.request_cancel();
+                hardware.set_cancel_request(error_severity, error_code);
             }
         }
     }
diff --git a/include/motor-control/core/brushed_motor/brushed_motor_interrupt_handler.hpp b/include/motor-control/core/brushed_motor/brushed_motor_interrupt_handler.hpp
index 436311fad..634249d8c 100644
--- a/include/motor-control/core/brushed_motor/brushed_motor_interrupt_handler.hpp
+++ b/include/motor-control/core/brushed_motor/brushed_motor_interrupt_handler.hpp
@@ -180,7 +180,7 @@ class BrushedMotorInterruptHandler {
         } else if (in_estop) {
             // if we've received a stop request during this time we can clear
             // that flag since there isn't anything running
-            std::ignore = hardware.has_cancel_request();
+            std::ignore = hardware.get_cancel_request();
             // return out of error state once the estop is disabled
             if (!estop_triggered()) {
                 in_estop = false;
@@ -197,7 +197,9 @@ class BrushedMotorInterruptHandler {
         } else if (estop_triggered()) {
             in_estop = true;
             cancel_and_clear_moves(can::ids::ErrorCode::estop_detected);
-        } else if (hardware.has_cancel_request()) {
+        } else if (static_cast<can::ids::ErrorCode>(
+                       hardware.get_cancel_request().code) !=
+                   can::ids::ErrorCode::ok) {
             if (!hardware.get_stay_enabled()) {
                 hardware.set_motor_state(BrushedMotorState::UNHOMED);
             }
diff --git a/include/motor-control/core/motor_hardware_interface.hpp b/include/motor-control/core/motor_hardware_interface.hpp
index 9a979143d..647603250 100644
--- a/include/motor-control/core/motor_hardware_interface.hpp
+++ b/include/motor-control/core/motor_hardware_interface.hpp
@@ -72,6 +72,11 @@ class UsageEEpromConfig {
     size_t num_keys = 0;
 };
 
+struct __attribute__((packed)) CancelRequest {
+    uint8_t severity;
+    uint8_t code;
+};
+
 class MotorHardwareIface {
   public:
     MotorHardwareIface() = default;
@@ -90,6 +95,7 @@ class MotorHardwareIface {
     virtual void read_limit_switch() = 0;
     virtual void read_estop_in() = 0;
     virtual void read_sync_in() = 0;
+    virtual auto read_tmc_diag0() -> bool = 0;
     virtual auto get_encoder_pulses() -> int32_t = 0;
     virtual void reset_encoder_pulses() = 0;
     virtual void start_timer_interrupt() = 0;
@@ -98,8 +104,10 @@ class MotorHardwareIface {
     virtual void enable_encoder() = 0;
     virtual void disable_encoder() = 0;
 
-    virtual auto has_cancel_request() -> bool = 0;
-    virtual void request_cancel() = 0;
+    virtual auto get_cancel_request() -> CancelRequest = 0;
+    virtual void set_cancel_request(can::ids::ErrorSeverity error_severity,
+                                    can::ids::ErrorCode error_code) = 0;
+    virtual void clear_cancel_request() = 0;
     virtual auto get_usage_eeprom_config() -> const UsageEEpromConfig& = 0;
 
     // This variable can remain public because the only public methods
diff --git a/include/motor-control/core/stepper_motor/motion_controller.hpp b/include/motor-control/core/stepper_motor/motion_controller.hpp
index cc22efc48..6f3267fcc 100644
--- a/include/motor-control/core/stepper_motor/motion_controller.hpp
+++ b/include/motor-control/core/stepper_motor/motion_controller.hpp
@@ -12,6 +12,9 @@
 #include "motor-control/core/types.hpp"
 #include "motor-control/core/utils.hpp"
 
+constexpr uint32_t DIAG0_DEBOUNCE_REPS = 9;
+constexpr uint32_t DIAG0_DEBOUNCE_DELAY = 100;
+
 namespace motion_controller {
 
 using namespace motor_messages;
@@ -184,13 +187,22 @@ class MotionController {
         return update_queue.try_write(can_msg);
     }
 
-    void stop() {
+    void stop(
+        can::ids::ErrorSeverity error_severity =
+            can::ids::ErrorSeverity::warning,
+        can::ids::ErrorCode error_code = can::ids::ErrorCode::stop_requested) {
         queue.reset();
         if (hardware.is_timer_interrupt_running()) {
-            hardware.request_cancel();
+            hardware.set_cancel_request(error_severity, error_code);
         }
     }
 
+    void clear_cancel_request() { hardware.clear_cancel_request(); }
+
+    auto is_timer_interrupt_running() -> bool {
+        return hardware.is_timer_interrupt_running();
+    }
+
     auto read_limit_switch() -> bool { return hardware.check_limit_switch(); }
 
     [[nodiscard]] auto read_motor_position() const {
@@ -205,6 +217,8 @@ class MotionController {
 
     auto check_read_sync_line() -> bool { return hardware.check_sync_in(); }
 
+    auto read_tmc_diag0() -> bool { return hardware.read_tmc_diag0(); }
+
     void enable_motor() {
         hardware.activate_motor();
         hardware.start_timer_interrupt();
@@ -339,20 +353,31 @@ class PipetteMotionController {
         return false;
     }
 
-    void stop() {
+    void stop(
+        can::ids::ErrorSeverity error_severity =
+            can::ids::ErrorSeverity::warning,
+        can::ids::ErrorCode error_code = can::ids::ErrorCode::stop_requested) {
         queue.reset();
         // if the timer interrupt is running, cancel it. if it isn't running,
         // don't submit a cancel because then the cancel won't be read until
         // the timer starts the next time.
         if (hardware.is_timer_interrupt_running()) {
-            hardware.request_cancel();
+            hardware.set_cancel_request(error_severity, error_code);
         }
     }
 
+    void clear_cancel_request() { hardware.clear_cancel_request(); }
+
+    auto is_timer_interrupt_running() -> bool {
+        return hardware.is_timer_interrupt_running();
+    }
+
     auto read_limit_switch() -> bool { return hardware.check_limit_switch(); }
 
     auto check_read_sync_line() -> bool { return hardware.check_sync_in(); }
 
+    auto read_tmc_diag0() -> bool { return hardware.read_tmc_diag0(); }
+
     void enable_motor() {
         hardware.start_timer_interrupt();
         hardware.activate_motor();
diff --git a/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp b/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp
index 5196a7f2d..fab6d550c 100644
--- a/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp
+++ b/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp
@@ -49,7 +49,7 @@ using namespace motor_messages;
  */
 
 template <template <class> class QueueImpl, class StatusClient,
-          typename MotorMoveMessage, typename MotorHardware>
+          class DriverClient, typename MotorMoveMessage, typename MotorHardware>
 requires MessageQueue<QueueImpl<MotorMoveMessage>, MotorMoveMessage> &&
     std::is_base_of_v<motor_hardware::MotorHardwareIface, MotorHardware>
 class MotorInterruptHandler {
@@ -60,11 +60,13 @@ class MotorInterruptHandler {
     MotorInterruptHandler() = delete;
     MotorInterruptHandler(MoveQueue& incoming_move_queue,
                           StatusClient& outgoing_queue,
+                          DriverClient& driver_queue,
                           MotorHardware& hardware_iface,
                           stall_check::StallCheck& stall,
                           UpdatePositionQueue& incoming_update_position_queue)
         : move_queue(incoming_move_queue),
           status_queue_client(outgoing_queue),
+          driver_client(driver_queue),
           hardware(hardware_iface),
           stall_checker{stall},
           update_position_queue(incoming_update_position_queue) {
@@ -191,6 +193,8 @@ class MotorInterruptHandler {
 
     void run_interrupt() {
         // handle various error states
+        motor_hardware::CancelRequest cancel_request =
+            hardware.get_cancel_request();
         std::ignore = hardware.get_encoder_pulses();
         if (clear_queue_until_empty) {
             // If we were executing a move when estop asserted, and
@@ -210,9 +214,11 @@ class MotorInterruptHandler {
         } else if (estop_triggered()) {
             cancel_and_clear_moves(can::ids::ErrorCode::estop_detected);
             in_estop = true;
-        } else if (hardware.has_cancel_request()) {
-            cancel_and_clear_moves(can::ids::ErrorCode::stop_requested,
-                                   can::ids::ErrorSeverity::warning);
+        } else if (static_cast<can::ids::ErrorCode>(cancel_request.code) !=
+                   can::ids::ErrorCode::ok) {
+            cancel_and_clear_moves(
+                static_cast<can::ids::ErrorCode>(cancel_request.code),
+                static_cast<can::ids::ErrorSeverity>(cancel_request.severity));
         } else {
             // Normal Move logic
             run_normal_interrupt();
@@ -469,6 +475,11 @@ class MotorInterruptHandler {
             can::messages::ErrorMessage{.message_index = message_index,
                                         .severity = severity,
                                         .error_code = err_code});
+        if (err_code == can::ids::ErrorCode::motor_driver_error_detected) {
+            driver_client.send_motor_driver_queue_isr(
+                can::messages::ReadMotorDriverErrorStatusRequest{
+                    .message_index = message_index});
+        }
         if (err_code == can::ids::ErrorCode::collision_detected) {
             build_and_send_ack(AckMessageId::position_error);
         }
@@ -675,6 +686,7 @@ class MotorInterruptHandler {
     q31_31 position_tracker{0};
     MoveQueue& move_queue;
     StatusClient& status_queue_client;
+    DriverClient& driver_client;
     MotorHardware& hardware;
     stall_check::StallCheck& stall_checker;
     UpdatePositionQueue& update_position_queue;
diff --git a/include/motor-control/core/stepper_motor/tmc2130_driver.hpp b/include/motor-control/core/stepper_motor/tmc2130_driver.hpp
index fefe1f049..7c3a74e7d 100644
--- a/include/motor-control/core/stepper_motor/tmc2130_driver.hpp
+++ b/include/motor-control/core/stepper_motor/tmc2130_driver.hpp
@@ -52,10 +52,11 @@ class TMC2130 {
     auto operator=(const TMC2130&& c) = delete;
     ~TMC2130() = default;
 
-    auto read(Registers addr, uint32_t command_data, uint32_t message_index)
-        -> void {
+    auto read(Registers addr, uint32_t command_data, uint32_t message_index,
+              uint8_t tags = 0) -> void {
         auto converted_addr = static_cast<uint8_t>(addr);
-        _spi_manager.read(converted_addr, command_data, _task_queue, _cs_intf,
+        uint32_t token = spi::utils::build_token(converted_addr, tags);
+        _spi_manager.read(token, command_data, _task_queue, _cs_intf,
                           message_index);
     }
 
diff --git a/include/motor-control/core/stepper_motor/tmc2160.hpp b/include/motor-control/core/stepper_motor/tmc2160.hpp
index 5f656e3f5..1590535fb 100644
--- a/include/motor-control/core/stepper_motor/tmc2160.hpp
+++ b/include/motor-control/core/stepper_motor/tmc2160.hpp
@@ -505,6 +505,21 @@ struct __attribute__((packed, __may_alias__)) DriveStatus {
     uint32_t stst : 1 = 0;
 };
 
+struct __attribute__((packed, __may_alias__)) DriveConf {
+    static constexpr Registers address = Registers::DRV_CONF;
+    static constexpr bool writable = true;
+    static constexpr uint32_t value_mask = (1 << 22) - 1;
+
+    uint32_t bbm_time : 5 = 0;
+    uint32_t bit_padding_1 : 3 = 0;
+    uint32_t bbm_clks : 4 = 0;
+    uint32_t bit_padding_2 : 4 = 0;
+    // 00 = 150*C, 01 = 143*C, 10 = 136*C, 11 = 120*C
+    uint32_t ot_select : 2 = 0;
+    uint32_t drv_strength : 2 = 0;
+    uint32_t filt_isense : 2 = 0;
+};
+
 /**
  * This register sets the control current for voltage PWM mode stealth chop.
  */
@@ -544,6 +559,7 @@ struct TMC2160RegisterMap {
     StealthChop pwmconf = {};
     DriveStatus drvstatus = {};
     GStatus gstat = {};
+    DriveConf drvconf = {};
     GlobalScaler glob_scale = {};
 };
 
diff --git a/include/motor-control/core/stepper_motor/tmc2160_driver.hpp b/include/motor-control/core/stepper_motor/tmc2160_driver.hpp
index bc8574cee..3958b917c 100644
--- a/include/motor-control/core/stepper_motor/tmc2160_driver.hpp
+++ b/include/motor-control/core/stepper_motor/tmc2160_driver.hpp
@@ -53,10 +53,11 @@ class TMC2160 {
     auto operator=(const TMC2160&& c) = delete;
     ~TMC2160() = default;
 
-    auto read(Registers addr, uint32_t command_data, uint32_t message_index)
-        -> void {
+    auto read(Registers addr, uint32_t command_data, uint32_t message_index,
+              uint8_t tags = 0) -> void {
         auto converted_addr = static_cast<uint8_t>(addr);
-        _spi_manager.read(converted_addr, command_data, _task_queue, _cs_intf,
+        uint32_t token = spi::utils::build_token(converted_addr, tags);
+        _spi_manager.read(token, command_data, _task_queue, _cs_intf,
                           message_index);
     }
 
@@ -105,6 +106,9 @@ class TMC2160 {
         if (!set_glob_scaler(_registers.glob_scale)) {
             return false;
         }
+        if (!set_drv_conf(_registers.drvconf)) {
+            return false;
+        }
         _initialized = true;
         return true;
     }
@@ -272,6 +276,22 @@ class TMC2160 {
         return false;
     }
 
+    /**
+     * @brief Update DRV_CONF register
+     * @param reg New configuration register to set
+     * @param policy Instance of abstraction policy to use
+     * @return True if new register was set succesfully, false otherwise
+     */
+    auto set_drv_conf(DriveConf reg) -> bool {
+        reg.bit_padding_1 = 0;
+        reg.bit_padding_2 = 0;
+        if (set_register(reg)) {
+            _registers.drvconf = reg;
+            return true;
+        }
+        return false;
+    }
+
     /**
      * @brief Update GlobalScaler register
      * @param reg New configuration register to set
diff --git a/include/motor-control/core/tasks/gear_tmc2160_motor_driver_task.hpp b/include/motor-control/core/tasks/gear_tmc2160_motor_driver_task.hpp
index ce55d848c..d9593d395 100644
--- a/include/motor-control/core/tasks/gear_tmc2160_motor_driver_task.hpp
+++ b/include/motor-control/core/tasks/gear_tmc2160_motor_driver_task.hpp
@@ -59,12 +59,24 @@ class MotorDriverMessageHandler {
             auto data = driver.handle_spi_read(
                 tmc2160::registers::Registers(static_cast<uint8_t>(m.id.token)),
                 m.rxBuffer);
-            can::messages::ReadMotorDriverRegisterResponse response_msg{
-                .message_index = m.id.message_index,
-                .reg_address = static_cast<uint8_t>(m.id.token),
-                .data = data,
-            };
-            can_client.send_can_message(can::ids::NodeId::host, response_msg);
+            if (spi::utils::tag_in_token(
+                    m.id.token, spi::utils::ResponseTag::IS_ERROR_RESPONSE)) {
+                can::messages::ReadMotorDriverErrorStatusResponse response_msg{
+                    .message_index = m.id.message_index,
+                    .reg_address = static_cast<uint8_t>(m.id.token),
+                    .data = data,
+                };
+                can_client.send_can_message(can::ids::NodeId::host,
+                                            response_msg);
+            } else {
+                can::messages::ReadMotorDriverRegisterResponse response_msg{
+                    .message_index = m.id.message_index,
+                    .reg_address = static_cast<uint8_t>(m.id.token),
+                    .data = data,
+                };
+                can_client.send_can_message(can::ids::NodeId::host,
+                                            response_msg);
+            }
         }
     }
 
@@ -87,6 +99,15 @@ class MotorDriverMessageHandler {
         }
     }
 
+    void handle(const can::messages::ReadMotorDriverErrorStatusRequest& m) {
+        LOG("Received read motor driver error register request");
+        uint32_t data = 0;
+        std::array tags{spi::utils::ResponseTag::IS_ERROR_RESPONSE};
+        uint8_t tag_byte = spi::utils::byte_from_tags(tags);
+        driver.read(tmc2160::registers::Registers::DRVSTATUS, data,
+                    m.message_index, tag_byte);
+    }
+
     void handle(const can::messages::GearWriteMotorCurrentRequest& m) {
         LOG("Received gear write motor current request: hold_current=%d, "
             "run_current=%d",
diff --git a/include/motor-control/core/tasks/messages.hpp b/include/motor-control/core/tasks/messages.hpp
index 759a28308..dc3203f7f 100644
--- a/include/motor-control/core/tasks/messages.hpp
+++ b/include/motor-control/core/tasks/messages.hpp
@@ -6,6 +6,11 @@
 
 namespace motor_control_task_messages {
 
+struct RouteMotorDriverInterrupt {
+    uint32_t message_index;
+    uint8_t debounce_count;
+};
+
 #ifdef USE_SENSOR_MOVE
 using MotionControlTaskMessage = std::variant<
     std::monostate, can::messages::AddLinearMoveRequest,
@@ -16,7 +21,7 @@ using MotionControlTaskMessage = std::variant<
     can::messages::HomeRequest,
     can::messages::UpdateMotorPositionEstimationRequest,
     can::messages::GetMotorUsageRequest, can::messages::MotorStatusRequest,
-    can::messages::AddSensorMoveRequest>;
+    RouteMotorDriverInterrupt, can::messages::AddSensorMoveRequest>;
 
 using MoveGroupTaskMessage =
     std::variant<std::monostate, can::messages::AddLinearMoveRequest,
@@ -34,7 +39,8 @@ using MotionControlTaskMessage = std::variant<
     can::messages::MotorPositionRequest, can::messages::ReadLimitSwitchRequest,
     can::messages::HomeRequest,
     can::messages::UpdateMotorPositionEstimationRequest,
-    can::messages::GetMotorUsageRequest, can::messages::MotorStatusRequest>;
+    can::messages::GetMotorUsageRequest, can::messages::MotorStatusRequest,
+    RouteMotorDriverInterrupt>;
 
 using MoveGroupTaskMessage =
     std::variant<std::monostate, can::messages::AddLinearMoveRequest,
@@ -47,7 +53,8 @@ using MoveGroupTaskMessage =
 using MotorDriverTaskMessage =
     std::variant<std::monostate, can::messages::ReadMotorDriverRegister,
                  can::messages::WriteMotorDriverRegister,
-                 can::messages::WriteMotorCurrentRequest>;
+                 can::messages::WriteMotorCurrentRequest,
+                 can::messages::ReadMotorDriverErrorStatusRequest>;
 
 using MoveStatusReporterTaskMessage = std::variant<
     std::monostate, motor_messages::Ack, motor_messages::UpdatePositionResponse,
diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp
index f8838703b..74f6e6ca7 100644
--- a/include/motor-control/core/tasks/motion_controller_task.hpp
+++ b/include/motor-control/core/tasks/motion_controller_task.hpp
@@ -8,26 +8,43 @@
 #include "motor-control/core/linear_motion_system.hpp"
 #include "motor-control/core/stepper_motor/motion_controller.hpp"
 #include "motor-control/core/tasks/messages.hpp"
+#include "motor-control/core/tasks/tmc_motor_driver_common.hpp"
 
 namespace motion_controller_task {
 
 using TaskMessage = motor_control_task_messages::MotionControlTaskMessage;
 
+/**
+ * Concept describing a class that can message this task.
+ * @tparam Client
+ */
+template <typename Client>
+concept TaskClient = requires(Client client, const TaskMessage& m) {
+    {client.send_motion_controller_queue(m)};
+};
+
 /**
  * The message queue message handler.
  */
 template <lms::MotorMechanicalConfig MEConfig,
           can::message_writer_task::TaskClient CanClient,
-          usage_storage_task::TaskClient UsageClient>
+          usage_storage_task::TaskClient UsageClient,
+          tmc::tasks::TaskClient DriverClient, TaskClient MotionClient>
 class MotionControllerMessageHandler {
   public:
     using MotorControllerType = motion_controller::MotionController<MEConfig>;
     MotionControllerMessageHandler(MotorControllerType& controller,
                                    CanClient& can_client,
-                                   UsageClient& usage_client)
+                                   UsageClient& usage_client,
+                                   DriverClient& driver_client,
+                                   MotionClient& motion_client,
+                                   std::atomic<bool>& diag0_debounced)
         : controller{controller},
           can_client{can_client},
-          usage_client{usage_client} {}
+          usage_client{usage_client},
+          driver_client{driver_client},
+          motion_client{motion_client},
+          diag0_debounced{diag0_debounced} {}
     MotionControllerMessageHandler(const MotionControllerMessageHandler& c) =
         delete;
     MotionControllerMessageHandler(const MotionControllerMessageHandler&& c) =
@@ -52,9 +69,19 @@ class MotionControllerMessageHandler {
 
     void handle(const can::messages::EnableMotorRequest& m) {
         LOG("Received enable motor request");
-        controller.enable_motor();
-        can_client.send_can_message(can::ids::NodeId::host,
-                                    can::messages::ack_from_request(m));
+        if (controller.read_tmc_diag0()) {
+            can_client.send_can_message(
+                can::ids::NodeId::host,
+                can::messages::ErrorMessage{
+                    .message_index = m.message_index,
+                    .severity = can::ids::ErrorSeverity::unrecoverable,
+                    .error_code =
+                        can::ids::ErrorCode::motor_driver_error_detected});
+        } else {
+            controller.enable_motor();
+            can_client.send_can_message(can::ids::NodeId::host,
+                                        can::messages::ack_from_request(m));
+        }
     }
 
     void handle(const can::messages::DisableMotorRequest& m) {
@@ -92,7 +119,17 @@ class MotionControllerMessageHandler {
             "groupid=%d, seqid=%d, duration=%d, stopcondition=%d",
             m.velocity, m.acceleration, m.group_id, m.seq_id, m.duration,
             m.request_stop_condition);
-        controller.move(m);
+        if (controller.read_tmc_diag0()) {
+            can_client.send_can_message(
+                can::ids::NodeId::host,
+                can::messages::ErrorMessage{
+                    .message_index = m.message_index,
+                    .severity = can::ids::ErrorSeverity::unrecoverable,
+                    .error_code =
+                        can::ids::ErrorCode::motor_driver_error_detected});
+        } else {
+            controller.move(m);
+        }
     }
 
 #ifdef USE_SENSOR_MOVE
@@ -109,7 +146,17 @@ class MotionControllerMessageHandler {
         LOG("Motion Controller Received home request: velocity=%d, "
             "groupid=%d, seqid=%d\n",
             m.velocity, m.group_id, m.seq_id);
-        controller.move(m);
+        if (controller.read_tmc_diag0()) {
+            can_client.send_can_message(
+                can::ids::NodeId::host,
+                can::messages::ErrorMessage{
+                    .message_index = m.message_index,
+                    .severity = can::ids::ErrorSeverity::unrecoverable,
+                    .error_code =
+                        can::ids::ErrorCode::motor_driver_error_detected});
+        } else {
+            controller.move(m);
+        }
     }
 
     void handle(const can::messages::ReadLimitSwitchRequest& m) {
@@ -146,10 +193,47 @@ class MotionControllerMessageHandler {
             can_client.send_can_message(can::ids::NodeId::host, response);
         }
     }
+
     void handle(const can::messages::GetMotorUsageRequest& m) {
         controller.send_usage_data(m.message_index, usage_client);
     }
 
+    void handle(
+        const motor_control_task_messages::RouteMotorDriverInterrupt& m) {
+        if (m.debounce_count > DIAG0_DEBOUNCE_REPS) {
+            if (controller.read_tmc_diag0()) {
+                controller.stop(
+                    can::ids::ErrorSeverity::unrecoverable,
+                    can::ids::ErrorCode::motor_driver_error_detected);
+                if (!controller.is_timer_interrupt_running()) {
+                    can_client.send_can_message(
+                        can::ids::NodeId::host,
+                        can::messages::ErrorMessage{
+                            .message_index = m.message_index,
+                            .severity = can::ids::ErrorSeverity::unrecoverable,
+                            .error_code = can::ids::ErrorCode::
+                                motor_driver_error_detected});
+                    driver_client.send_motor_driver_queue(
+                        can::messages::ReadMotorDriverErrorStatusRequest{
+                            .message_index = m.message_index});
+                }
+            }
+            diag0_debounced = false;
+        } else {
+            vTaskDelay(pdMS_TO_TICKS(DIAG0_DEBOUNCE_DELAY));
+            motion_client.send_motion_controller_queue(
+                increment_message_debounce_count(m));
+        }
+    }
+
+    auto increment_message_debounce_count(
+        const motor_control_task_messages::RouteMotorDriverInterrupt& m)
+        -> motor_control_task_messages::RouteMotorDriverInterrupt {
+        return motor_control_task_messages::RouteMotorDriverInterrupt{
+            .message_index = m.message_index,
+            .debounce_count = static_cast<uint8_t>(m.debounce_count + 1)};
+    }
+
     void handle(const can::messages::MotorStatusRequest& m) {
         auto response = static_cast<uint8_t>(controller.is_motor_enabled());
         can::messages::MotorStatusResponse msg{.message_index = m.message_index,
@@ -160,6 +244,9 @@ class MotionControllerMessageHandler {
     MotorControllerType& controller;
     CanClient& can_client;
     UsageClient& usage_client;
+    DriverClient& driver_client;
+    MotionClient& motion_client;
+    std::atomic<bool>& diag0_debounced;
 };
 
 /**
@@ -183,12 +270,15 @@ class MotionControllerTask {
      */
     template <lms::MotorMechanicalConfig MEConfig,
               can::message_writer_task::TaskClient CanClient,
-              usage_storage_task::TaskClient UsageClient>
+              usage_storage_task::TaskClient UsageClient,
+              tmc::tasks::TaskClient DriverClient, TaskClient MotionClient>
     [[noreturn]] void operator()(
         motion_controller::MotionController<MEConfig>* controller,
-        CanClient* can_client, UsageClient* usage_client) {
-        auto handler = MotionControllerMessageHandler{*controller, *can_client,
-                                                      *usage_client};
+        CanClient* can_client, UsageClient* usage_client,
+        DriverClient* driver_client, MotionClient* motion_client) {
+        auto handler = MotionControllerMessageHandler{
+            *controller,    *can_client,    *usage_client,
+            *driver_client, *motion_client, diag0_debounced};
         TaskMessage message{};
         bool first_run = true;
         for (;;) {
@@ -204,17 +294,18 @@ class MotionControllerTask {
 
     [[nodiscard]] auto get_queue() const -> QueueType& { return queue; }
 
+    void run_diag0_interrupt() {
+        if (!diag0_debounced) {
+            static_cast<void>(queue.try_write_isr(
+                motor_control_task_messages::RouteMotorDriverInterrupt{
+                    .message_index = 0, .debounce_count = 0}));
+            diag0_debounced = true;
+        }
+    }
+
   private:
     QueueType& queue;
-};
-
-/**
- * Concept describing a class that can message this task.
- * @tparam Client
- */
-template <typename Client>
-concept TaskClient = requires(Client client, const TaskMessage& m) {
-    {client.send_motion_controller_queue(m)};
+    std::atomic<bool> diag0_debounced = false;
 };
 
 }  // namespace motion_controller_task
diff --git a/include/motor-control/core/tasks/tmc2130_motor_driver_task.hpp b/include/motor-control/core/tasks/tmc2130_motor_driver_task.hpp
index 82cff0e01..526b0ebaf 100644
--- a/include/motor-control/core/tasks/tmc2130_motor_driver_task.hpp
+++ b/include/motor-control/core/tasks/tmc2130_motor_driver_task.hpp
@@ -58,12 +58,24 @@ class MotorDriverMessageHandler {
             auto data = driver.handle_spi_read(
                 tmc2130::registers::Registers(static_cast<uint8_t>(m.id.token)),
                 m.rxBuffer);
-            can::messages::ReadMotorDriverRegisterResponse response_msg{
-                .message_index = m.id.message_index,
-                .reg_address = static_cast<uint8_t>(m.id.token),
-                .data = data,
-            };
-            can_client.send_can_message(can::ids::NodeId::host, response_msg);
+            if (spi::utils::tag_in_token(
+                    m.id.token, spi::utils::ResponseTag::IS_ERROR_RESPONSE)) {
+                can::messages::ReadMotorDriverErrorStatusResponse response_msg{
+                    .message_index = m.id.message_index,
+                    .reg_address = static_cast<uint8_t>(m.id.token),
+                    .data = data,
+                };
+                can_client.send_can_message(can::ids::NodeId::host,
+                                            response_msg);
+            } else {
+                can::messages::ReadMotorDriverRegisterResponse response_msg{
+                    .message_index = m.id.message_index,
+                    .reg_address = static_cast<uint8_t>(m.id.token),
+                    .data = data,
+                };
+                can_client.send_can_message(can::ids::NodeId::host,
+                                            response_msg);
+            }
         }
     }
 
@@ -86,6 +98,15 @@ class MotorDriverMessageHandler {
         }
     }
 
+    void handle(const can::messages::ReadMotorDriverErrorStatusRequest& m) {
+        LOG("Received read motor driver error register request");
+        uint32_t data = 0;
+        std::array tags{spi::utils::ResponseTag::IS_ERROR_RESPONSE};
+        uint8_t tag_byte = spi::utils::byte_from_tags(tags);
+        driver.read(tmc2130::registers::Registers::DRVSTATUS, data,
+                    m.message_index, tag_byte);
+    }
+
     void handle(const can::messages::WriteMotorCurrentRequest& m) {
         LOG("Received write motor current request: hold_current=%d, "
             "run_current=%d",
@@ -154,6 +175,7 @@ class MotorDriverTask {
 template <typename Client>
 concept TaskClient = requires(Client client, const TaskMessage& m) {
     {client.send_motor_driver_queue(m)};
+    {client.send_motor_driver_queue_isr(m)};
 };
 
 }  // namespace tasks
diff --git a/include/motor-control/core/tasks/tmc2160_motor_driver_task.hpp b/include/motor-control/core/tasks/tmc2160_motor_driver_task.hpp
index 9f4e69179..aec315ea7 100644
--- a/include/motor-control/core/tasks/tmc2160_motor_driver_task.hpp
+++ b/include/motor-control/core/tasks/tmc2160_motor_driver_task.hpp
@@ -10,6 +10,7 @@
 #include "motor-control/core/tasks/messages.hpp"
 #include "motor-control/core/tasks/tmc_motor_driver_common.hpp"
 #include "spi/core/messages.hpp"
+#include "spi/core/utils.hpp"
 
 namespace tmc2160 {
 
@@ -57,12 +58,24 @@ class MotorDriverMessageHandler {
             auto data = driver.handle_spi_read(
                 tmc2160::registers::Registers(static_cast<uint8_t>(m.id.token)),
                 m.rxBuffer);
-            can::messages::ReadMotorDriverRegisterResponse response_msg{
-                .message_index = m.id.message_index,
-                .reg_address = static_cast<uint8_t>(m.id.token),
-                .data = data,
-            };
-            can_client.send_can_message(can::ids::NodeId::host, response_msg);
+            if (spi::utils::tag_in_token(
+                    m.id.token, spi::utils::ResponseTag::IS_ERROR_RESPONSE)) {
+                can::messages::ReadMotorDriverErrorStatusResponse response_msg{
+                    .message_index = m.id.message_index,
+                    .reg_address = static_cast<uint8_t>(m.id.token),
+                    .data = data,
+                };
+                can_client.send_can_message(can::ids::NodeId::host,
+                                            response_msg);
+            } else {
+                can::messages::ReadMotorDriverRegisterResponse response_msg{
+                    .message_index = m.id.message_index,
+                    .reg_address = static_cast<uint8_t>(m.id.token),
+                    .data = data,
+                };
+                can_client.send_can_message(can::ids::NodeId::host,
+                                            response_msg);
+            }
         }
     }
 
@@ -85,6 +98,15 @@ class MotorDriverMessageHandler {
         }
     }
 
+    void handle(const can::messages::ReadMotorDriverErrorStatusRequest& m) {
+        LOG("Received read motor driver error register request");
+        uint32_t data = 0;
+        std::array tags{spi::utils::ResponseTag::IS_ERROR_RESPONSE};
+        uint8_t tag_byte = spi::utils::byte_from_tags(tags);
+        driver.read(tmc2160::registers::Registers::DRVSTATUS, data,
+                    m.message_index, tag_byte);
+    }
+
     void handle(const can::messages::WriteMotorCurrentRequest& m) {
         LOG("Received write motor current request: hold_current=%d, "
             "run_current=%d",
@@ -146,6 +168,16 @@ class MotorDriverTask {
     QueueType& queue;
 };
 
+/**
+ * Concept describing a class that can message this task.
+ * @tparam Client
+ */
+template <typename Client>
+concept TaskClient = requires(Client client, const TaskMessage& m) {
+    {client.send_motor_driver_queue(m)};
+    {client.send_motor_driver_queue_isr(m)};
+};
+
 }  // namespace tasks
 
 }  // namespace tmc2160
diff --git a/include/motor-control/core/tasks/tmc_motor_driver_common.hpp b/include/motor-control/core/tasks/tmc_motor_driver_common.hpp
index 191373466..0b0839348 100644
--- a/include/motor-control/core/tasks/tmc_motor_driver_common.hpp
+++ b/include/motor-control/core/tasks/tmc_motor_driver_common.hpp
@@ -8,11 +8,14 @@ namespace tmc {
 namespace tasks {
 
 using SpiResponseMessage = std::tuple<spi::messages::TransactResponse>;
-using CanMessageTuple = std::tuple<can::messages::ReadMotorDriverRegister,
-                                   can::messages::WriteMotorDriverRegister,
-                                   can::messages::WriteMotorCurrentRequest>;
+using CanMessageTuple =
+    std::tuple<can::messages::ReadMotorDriverRegister,
+               can::messages::WriteMotorDriverRegister,
+               can::messages::WriteMotorCurrentRequest,
+               can::messages::ReadMotorDriverErrorStatusRequest>;
 using GearCanMessageTuple =
     std::tuple<can::messages::GearReadMotorDriverRegister,
+               can::messages::ReadMotorDriverErrorStatusRequest,
                can::messages::GearWriteMotorDriverRegister,
                can::messages::GearWriteMotorCurrentRequest>;
 using CanMessage =
@@ -37,6 +40,7 @@ using GearTaskMessage = typename ::utils::VariantCat<
 template <typename Client>
 concept TaskClient = requires(Client client, const TaskMessage& m) {
     {client.send_motor_driver_queue(m)};
+    {client.send_motor_driver_queue_isr(m)};
 };
 
 /**
@@ -46,6 +50,7 @@ concept TaskClient = requires(Client client, const TaskMessage& m) {
 template <typename Client>
 concept GearTaskClient = requires(Client client, const GearTaskMessage& m) {
     {client.send_motor_driver_queue(m)};
+    {client.send_motor_driver_queue_isr(m)};
 };
 };  // namespace tasks
 };  // namespace tmc
diff --git a/include/motor-control/firmware/brushed_motor/brushed_motor_hardware.hpp b/include/motor-control/firmware/brushed_motor/brushed_motor_hardware.hpp
index 986c9fe1d..3049d8f41 100644
--- a/include/motor-control/firmware/brushed_motor/brushed_motor_hardware.hpp
+++ b/include/motor-control/firmware/brushed_motor/brushed_motor_hardware.hpp
@@ -62,6 +62,7 @@ class BrushedMotorHardware : public BrushedMotorHardwareIface {
     void read_limit_switch() final;
     void read_estop_in() final;
     void read_sync_in() final;
+    auto read_tmc_diag0() -> bool final;
     void grip() final;
     void ungrip() final;
     void stop_pwm() final;
@@ -78,10 +79,21 @@ class BrushedMotorHardware : public BrushedMotorHardwareIface {
     void reset_control() final;
     void set_stay_enabled(bool state) final { stay_enabled = state; }
     auto get_stay_enabled() -> bool final { return stay_enabled; }
-    auto has_cancel_request() -> bool final {
-        return cancel_request.exchange(false);
+    auto get_cancel_request() -> CancelRequest final {
+        CancelRequest exchange_request = {};
+        return cancel_request.exchange(exchange_request);
+    }
+    void set_cancel_request(can::ids::ErrorSeverity error_severity,
+                            can::ids::ErrorCode error_code) final {
+        CancelRequest update_request{
+            .severity = static_cast<uint8_t>(error_severity),
+            .code = static_cast<uint8_t>(error_code)};
+        cancel_request.store(update_request);
+    }
+    void clear_cancel_request() final {
+        CancelRequest clear_request = {};
+        cancel_request.store(clear_request);
     }
-    void request_cancel() final { cancel_request.store(true); }
     auto get_usage_eeprom_config() -> const UsageEEpromConfig& final {
         return eeprom_config;
     }
@@ -101,7 +113,7 @@ class BrushedMotorHardware : public BrushedMotorHardwareIface {
     int32_t motor_encoder_overflow_count = 0;
     ot_utils::pid::PID controller_loop;
     std::atomic<ControlDirection> control_dir = ControlDirection::unset;
-    std::atomic<bool> cancel_request = false;
+    std::atomic<CancelRequest> cancel_request = {};
     const UsageEEpromConfig& eeprom_config;
     void* stopwatch_handle;
     BrushedMotorState motor_state = BrushedMotorState::UNHOMED;
diff --git a/include/motor-control/firmware/stepper_motor/motor_hardware.hpp b/include/motor-control/firmware/stepper_motor/motor_hardware.hpp
index e750a4f02..ca7be4974 100644
--- a/include/motor-control/firmware/stepper_motor/motor_hardware.hpp
+++ b/include/motor-control/firmware/stepper_motor/motor_hardware.hpp
@@ -18,6 +18,7 @@ struct HardwareConfig {
     gpio::PinConfig led;
     gpio::PinConfig sync_in;
     gpio::PinConfig estop_in;
+    gpio::PinConfig diag0;
     std::optional<gpio::PinConfig> ebrake = std::nullopt;
 };
 
@@ -50,16 +51,28 @@ class MotorHardware : public StepperMotorHardwareIface {
     void read_limit_switch() final;
     void read_estop_in() final;
     void read_sync_in() final;
+    auto read_tmc_diag0() -> bool final;
     void set_LED(bool status) final;
     auto get_encoder_pulses() -> int32_t final;
     void reset_encoder_pulses() final;
-    auto has_cancel_request() -> bool final {
-        return cancel_request.exchange(false);
-    }
     void disable_encoder() final;
     void enable_encoder() final;
 
-    void request_cancel() final { cancel_request.store(true); }
+    auto get_cancel_request() -> CancelRequest final {
+        CancelRequest exchange_request = {};
+        return cancel_request.exchange(exchange_request);
+    }
+    void set_cancel_request(can::ids::ErrorSeverity error_severity,
+                            can::ids::ErrorCode error_code) final {
+        CancelRequest update_request{
+            .severity = static_cast<uint8_t>(error_severity),
+            .code = static_cast<uint8_t>(error_code)};
+        cancel_request.store(update_request);
+    }
+    void clear_cancel_request() final {
+        CancelRequest clear_request = {};
+        cancel_request.store(clear_request);
+    }
 
     auto get_usage_eeprom_config() -> const UsageEEpromConfig& final {
         return eeprom_config;
@@ -67,6 +80,8 @@ class MotorHardware : public StepperMotorHardwareIface {
     // downward interface - call from timer overflow handler
     void encoder_overflow(int32_t direction);
 
+    auto get_pins() -> HardwareConfig { return pins; }
+
   private:
     debouncer::Debouncer estop = debouncer::Debouncer{};
     debouncer::Debouncer limit = debouncer::Debouncer{};
@@ -77,7 +92,7 @@ class MotorHardware : public StepperMotorHardwareIface {
     void* enc_handle;
     const UsageEEpromConfig& eeprom_config;
     std::atomic<int32_t> motor_encoder_overflow_count = 0;
-    std::atomic<bool> cancel_request = false;
+    std::atomic<CancelRequest> cancel_request = {};
     static constexpr uint32_t ENCODER_OVERFLOW_PULSES_BIT = 0x1 << 31;
 };
 
diff --git a/include/motor-control/simulation/motor_interrupt_driver.hpp b/include/motor-control/simulation/motor_interrupt_driver.hpp
index f7dbb193d..6fef39218 100644
--- a/include/motor-control/simulation/motor_interrupt_driver.hpp
+++ b/include/motor-control/simulation/motor_interrupt_driver.hpp
@@ -9,7 +9,8 @@
 
 namespace motor_interrupt_driver {
 
-template <class StatusClient, typename MotorMoveMessage, class MotorHardware>
+template <class StatusClient, class DriverClient, typename MotorMoveMessage,
+          class MotorHardware>
 class MotorInterruptDriver {
     using InterruptQueue =
         freertos_message_queue::FreeRTOSMessageQueue<MotorMoveMessage>;
@@ -18,7 +19,7 @@ class MotorInterruptDriver {
             can::messages::UpdateMotorPositionEstimationRequest>;
     using InterruptHandler = motor_handler::MotorInterruptHandler<
         freertos_message_queue::FreeRTOSMessageQueue, StatusClient,
-        MotorMoveMessage, MotorHardware>;
+        DriverClient, MotorMoveMessage, MotorHardware>;
 
   public:
     MotorInterruptDriver(InterruptQueue& q, InterruptHandler& h,
diff --git a/include/motor-control/simulation/sim_motor_hardware_iface.hpp b/include/motor-control/simulation/sim_motor_hardware_iface.hpp
index 2a8dd67cc..01f45fb21 100644
--- a/include/motor-control/simulation/sim_motor_hardware_iface.hpp
+++ b/include/motor-control/simulation/sim_motor_hardware_iface.hpp
@@ -51,6 +51,7 @@ class SimMotorHardwareIface : public motor_hardware::StepperMotorHardwareIface {
     void read_limit_switch() final {}
     void read_estop_in() final {}
     void read_sync_in() final {}
+    bool read_tmc_diag0() final { return false; }
     void set_LED(bool) final {}
     void trigger_limit_switch() { limit_switch_status = true; }
     bool check_sync_in() final {
@@ -87,10 +88,21 @@ class SimMotorHardwareIface : public motor_hardware::StepperMotorHardwareIface {
     bool check_estop_in() final { return estop_detected; }
 
     void set_estop(bool estop_pressed) { estop_detected = estop_pressed; }
-    auto has_cancel_request() -> bool final {
-        return cancel_request.exchange(false);
+    auto get_cancel_request() -> motor_hardware::CancelRequest final {
+        motor_hardware::CancelRequest exchange_request;
+        return cancel_request.exchange(exchange_request);
+    }
+    void set_cancel_request(can::ids::ErrorSeverity error_severity,
+                            can::ids::ErrorCode error_code) final {
+        motor_hardware::CancelRequest update_request{
+            .severity = static_cast<uint8_t>(error_severity),
+            .code = static_cast<uint8_t>(error_code)};
+        cancel_request.store(update_request);
+    }
+    void clear_cancel_request() final {
+        motor_hardware::CancelRequest clear_request;
+        cancel_request.store(clear_request);
     }
-    void request_cancel() final { cancel_request.store(true); }
     auto get_usage_eeprom_config()
         -> motor_hardware::UsageEEpromConfig & final {
         return eeprom_config;
@@ -106,7 +118,7 @@ class SimMotorHardwareIface : public motor_hardware::StepperMotorHardwareIface {
     Direction _direction = Direction::POSITIVE;
     float _encoder_ticks_per_pulse = 0;
     bool estop_detected = false;
-    std::atomic<bool> cancel_request = false;
+    std::atomic<motor_hardware::CancelRequest> cancel_request = {};
     motor_hardware::UsageEEpromConfig eeprom_config = {
         std::array<UsageRequestSet, 1>{UsageRequestSet{
             .eeprom_key = 0,
@@ -137,6 +149,7 @@ class SimBrushedMotorHardwareIface
     void read_limit_switch() final {}
     void read_estop_in() final {}
     void read_sync_in() final {}
+    bool read_tmc_diag0() final { return false; }
     void trigger_limit_switch() { limit_switch_status = true; }
     void grip() final {}
     void ungrip() final {}
@@ -177,11 +190,21 @@ class SimBrushedMotorHardwareIface
         return ret;
     }
 
-    auto has_cancel_request() -> bool final {
-        return cancel_request.exchange(false);
+    auto get_cancel_request() -> motor_hardware::CancelRequest final {
+        motor_hardware::CancelRequest exchange_request;
+        return cancel_request.exchange(exchange_request);
+    }
+    void set_cancel_request(can::ids::ErrorSeverity error_severity,
+                            can::ids::ErrorCode error_code) final {
+        motor_hardware::CancelRequest update_request{
+            .severity = static_cast<uint8_t>(error_severity),
+            .code = static_cast<uint8_t>(error_code)};
+        cancel_request.store(update_request);
+    }
+    void clear_cancel_request() final {
+        motor_hardware::CancelRequest clear_request;
+        cancel_request.store(clear_request);
     }
-    void request_cancel() final { cancel_request.store(true); }
-
     void disable_encoder() final {}
     void enable_encoder() final {}
 
@@ -201,7 +224,7 @@ class SimBrushedMotorHardwareIface
     StateManagerHandle _state_manager = nullptr;
     MoveMessageHardware _id;
     bool estop_detected = false;
-    std::atomic<bool> cancel_request = false;
+    std::atomic<motor_hardware::CancelRequest> cancel_request = {};
     BrushedMotorState motor_state = BrushedMotorState::UNHOMED;
     motor_hardware::UsageEEpromConfig eeprom_config{
         std::array<UsageRequestSet, 1>{UsageRequestSet{
@@ -235,6 +258,7 @@ class SimGearMotorHardwareIface
     void read_limit_switch() final {}
     void read_estop_in() final {}
     void read_sync_in() final {}
+    bool read_tmc_diag0() final { return false; }
     void set_LED(bool) final {}
     void trigger_limit_switch() { limit_switch_status = true; }
     bool check_sync_in() final {
@@ -267,10 +291,21 @@ class SimGearMotorHardwareIface
     bool check_estop_in() final { return estop_detected; }
 
     void set_estop(bool estop_pressed) { estop_detected = estop_pressed; }
-    auto has_cancel_request() -> bool final {
-        return cancel_request.exchange(false);
+    auto get_cancel_request() -> motor_hardware::CancelRequest final {
+        motor_hardware::CancelRequest exchange_request;
+        return cancel_request.exchange(exchange_request);
+    }
+    void set_cancel_request(can::ids::ErrorSeverity error_severity,
+                            can::ids::ErrorCode error_code) final {
+        motor_hardware::CancelRequest update_request{
+            .severity = static_cast<uint8_t>(error_severity),
+            .code = static_cast<uint8_t>(error_code)};
+        cancel_request.store(update_request);
+    }
+    void clear_cancel_request() final {
+        motor_hardware::CancelRequest clear_request;
+        cancel_request.store(clear_request);
     }
-    void request_cancel() final { cancel_request.store(true); }
 
     auto get_usage_eeprom_config()
         -> motor_hardware::UsageEEpromConfig & final {
@@ -287,7 +322,7 @@ class SimGearMotorHardwareIface
     Direction _direction = Direction::POSITIVE;
     float _encoder_ticks_per_pulse = 0;
     bool estop_detected = false;
-    std::atomic<bool> cancel_request = false;
+    std::atomic<motor_hardware::CancelRequest> cancel_request = {};
     motor_hardware::UsageEEpromConfig eeprom_config = {
         std::array<UsageRequestSet, 1>{UsageRequestSet{
             .eeprom_key = 0,
diff --git a/include/motor-control/tests/mock_brushed_motor_components.hpp b/include/motor-control/tests/mock_brushed_motor_components.hpp
index 410cc8632..ecfdd9b1d 100644
--- a/include/motor-control/tests/mock_brushed_motor_components.hpp
+++ b/include/motor-control/tests/mock_brushed_motor_components.hpp
@@ -42,6 +42,7 @@ class MockBrushedMotorHardware : public BrushedMotorHardwareIface {
     void stop_pwm() final { move_dir = PWM_DIRECTION::unset; }
     auto check_sync_in() -> bool final { return sync_val; }
     void read_sync_in() final {}
+    bool read_tmc_diag0() final { return diag0_val; }
 
     auto get_encoder_pulses() -> int32_t final {
         return (motor_encoder_overflow_count << 16) + enc_val;
@@ -71,12 +72,23 @@ class MockBrushedMotorHardware : public BrushedMotorHardwareIface {
     PWM_DIRECTION get_direction() { return move_dir; }
     void set_stay_enabled(bool state) { stay_enabled = state; }
     auto get_stay_enabled() -> bool { return stay_enabled; }
-    auto has_cancel_request() -> bool final {
-        bool old_request = cancel_request;
-        cancel_request = false;
+    auto get_cancel_request() -> CancelRequest final {
+        CancelRequest old_request = cancel_request;
+        CancelRequest exchange_request{};
+        cancel_request = exchange_request;
         return old_request;
     }
-    void request_cancel() final { cancel_request = true; }
+    void set_cancel_request(can::ids::ErrorSeverity error_severity,
+                            can::ids::ErrorCode error_code) final {
+        CancelRequest update_request{
+            .severity = static_cast<uint8_t>(error_severity),
+            .code = static_cast<uint8_t>(error_code)};
+        cancel_request = update_request;
+    }
+    void clear_cancel_request() final {
+        CancelRequest clear_request{};
+        cancel_request = clear_request;
+    }
     void set_timer_interrupt_running(bool is_running) {
         timer_interrupt_running = is_running;
     }
@@ -97,6 +109,7 @@ class MockBrushedMotorHardware : public BrushedMotorHardwareIface {
     int32_t motor_encoder_overflow_count = 0;
     bool ls_val = false;
     bool sync_val = false;
+    bool diag0_val = false;
     bool estop_in_val = false;
     bool is_gripping = false;
     bool motor_enabled = false;
@@ -108,7 +121,7 @@ class MockBrushedMotorHardware : public BrushedMotorHardwareIface {
     // when the "motor" instantly goes to top speed then instantly stops
     ot_utils::pid::PID controller_loop{0.008,         0.0045, 0.000015,
                                        1.F / 32000.0, 7,      -7};
-    bool cancel_request = false;
+    CancelRequest cancel_request = {};
     bool timer_interrupt_running = true;
     motor_hardware::UsageEEpromConfig eeprom_config =
         motor_hardware::UsageEEpromConfig{std::array<UsageRequestSet, 3>{
diff --git a/include/motor-control/tests/mock_motor_driver_client.hpp b/include/motor-control/tests/mock_motor_driver_client.hpp
new file mode 100644
index 000000000..b4fb715ca
--- /dev/null
+++ b/include/motor-control/tests/mock_motor_driver_client.hpp
@@ -0,0 +1,21 @@
+#pragma once
+
+#include <vector>
+
+#include "motor-control/core/tasks/tmc_motor_driver_common.hpp"
+
+namespace test_mocks {
+
+struct MockMotorDriverClient {
+    void send_motor_driver_queue(const tmc::tasks::TaskMessage& m) {
+        messages.push_back(m);
+    }
+
+    void send_motor_driver_queue_isr(const tmc::tasks::TaskMessage& m) {
+        messages.push_back(m);
+    }
+
+    std::vector<tmc::tasks::TaskMessage> messages{};
+};
+
+}  // namespace test_mocks
diff --git a/include/motor-control/tests/mock_motor_hardware.hpp b/include/motor-control/tests/mock_motor_hardware.hpp
index ff6e0bd69..fc04f03b2 100644
--- a/include/motor-control/tests/mock_motor_hardware.hpp
+++ b/include/motor-control/tests/mock_motor_hardware.hpp
@@ -30,6 +30,7 @@ class MockMotorHardware : public motor_hardware::StepperMotorHardwareIface {
     void read_limit_switch() final {}
     void read_estop_in() final {}
     void read_sync_in() final {}
+    bool read_tmc_diag0() final { return mock_diag0_value; }
     void set_LED(bool) final {}
     void set_mock_lim_sw(bool value) { mock_lim_sw_value = value; }
     void set_mock_estop_in(bool value) { mock_estop_in_value = value; }
@@ -39,12 +40,23 @@ class MockMotorHardware : public motor_hardware::StepperMotorHardwareIface {
     void reset_encoder_pulses() final { test_pulses = 0; }
     int32_t get_encoder_pulses() final { return test_pulses; }
     void sim_set_encoder_pulses(int32_t pulses) { test_pulses = pulses; }
-    auto has_cancel_request() -> bool final {
-        bool old_request = cancel_request;
-        cancel_request = false;
+    auto get_cancel_request() -> motor_hardware::CancelRequest final {
+        motor_hardware::CancelRequest old_request = cancel_request;
+        motor_hardware::CancelRequest exchange_request{};
+        cancel_request = exchange_request;
         return old_request;
     }
-    void request_cancel() final { cancel_request = true; }
+    void set_cancel_request(can::ids::ErrorSeverity error_severity,
+                            can::ids::ErrorCode error_code) final {
+        motor_hardware::CancelRequest update_request{
+            .severity = static_cast<uint8_t>(error_severity),
+            .code = static_cast<uint8_t>(error_code)};
+        cancel_request = update_request;
+    }
+    void clear_cancel_request() final {
+        motor_hardware::CancelRequest clear_request{};
+        cancel_request = clear_request;
+    }
     void sim_set_timer_interrupt_running(bool is_running) {
         mock_timer_interrupt_running = is_running;
     }
@@ -61,11 +73,12 @@ class MockMotorHardware : public motor_hardware::StepperMotorHardwareIface {
     bool mock_lim_sw_value = false;
     bool mock_estop_in_value = false;
     bool mock_sync_value = false;
+    bool mock_diag0_value = false;
     bool mock_sr_value = false;
     bool mock_dir_value = false;
     uint8_t finished_move_id = 0x0;
     int32_t test_pulses = 0x0;
-    bool cancel_request = false;
+    motor_hardware::CancelRequest cancel_request = {};
     bool mock_timer_interrupt_running = true;
     motor_hardware::UsageEEpromConfig eeprom_config =
         motor_hardware::UsageEEpromConfig{
diff --git a/include/pipettes/core/dispatch_builder.hpp b/include/pipettes/core/dispatch_builder.hpp
index 7bb94358b..c02cacd46 100644
--- a/include/pipettes/core/dispatch_builder.hpp
+++ b/include/pipettes/core/dispatch_builder.hpp
@@ -23,19 +23,22 @@ using TMC2130MotorDispatchTarget = can::dispatch::DispatchParseTarget<
         linear_motor_tasks::tmc2130_driver::QueueClient>,
     can::messages::ReadMotorDriverRegister,
     can::messages::WriteMotorDriverRegister,
-    can::messages::WriteMotorCurrentRequest>;
+    can::messages::WriteMotorCurrentRequest,
+    can::messages::ReadMotorDriverErrorStatusRequest>;
 
 using TMC2160MotorDispatchTarget = can::dispatch::DispatchParseTarget<
     can::message_handlers::motor::MotorHandler<
         linear_motor_tasks::tmc2160_driver::QueueClient>,
     can::messages::ReadMotorDriverRegister,
     can::messages::WriteMotorDriverRegister,
-    can::messages::WriteMotorCurrentRequest>;
+    can::messages::WriteMotorCurrentRequest,
+    can::messages::ReadMotorDriverErrorStatusRequest>;
 
 using GearMotorDispatchTarget = can::dispatch::DispatchParseTarget<
     can::message_handlers::motor::GearMotorHandler<
         gear_motor_tasks::QueueClient>,
     can::messages::GearReadMotorDriverRegister,
+    can::messages::ReadMotorDriverErrorStatusRequest,
     can::messages::GearWriteMotorDriverRegister,
     can::messages::GearWriteMotorCurrentRequest>;
 
diff --git a/include/pipettes/core/gear_motor_tasks.hpp b/include/pipettes/core/gear_motor_tasks.hpp
index 292f73b4d..6f4edc169 100644
--- a/include/pipettes/core/gear_motor_tasks.hpp
+++ b/include/pipettes/core/gear_motor_tasks.hpp
@@ -31,7 +31,7 @@ using CanWriterTask = can::message_writer_task::MessageWriterTask<
 using SPIWriterClient =
     spi::writer::Writer<freertos_message_queue::FreeRTOSMessageQueue>;
 
-void start_tasks(
+auto start_tasks(
     CanWriterTask& can_writer,
     interfaces::gear_motor::GearMotionControl& motion_controllers,
     SPIWriterClient& spi_writer,
@@ -39,7 +39,10 @@ void start_tasks(
     can::ids::NodeId id,
     interfaces::gear_motor::GearMotorHardwareTasks& gmh_tsks,
     eeprom::dev_data::DevDataTailAccessor<sensor_tasks::QueueClient>&
-        tail_accessor);
+        tail_accessor)
+    -> std::tuple<interfaces::diag0_handler, interfaces::diag0_handler>;
+
+void call_run_diag0_interrupt();
 
 /**
  * Access to all the gear motion tasks.
@@ -73,6 +76,9 @@ struct QueueClient : can::message_writer::MessageWriter {
 
     void send_motor_driver_queue(const tmc2160::tasks::gear::TaskMessage& m);
 
+    void send_motor_driver_queue_isr(
+        const tmc2160::tasks::gear::TaskMessage& m);
+
     void send_move_group_queue(
         const pipettes::tasks::move_group_task::TaskMessage& m);
 
diff --git a/include/pipettes/core/interfaces.hpp b/include/pipettes/core/interfaces.hpp
index 999e5f7ea..abca79a9f 100644
--- a/include/pipettes/core/interfaces.hpp
+++ b/include/pipettes/core/interfaces.hpp
@@ -36,6 +36,8 @@ struct HighThroughputInterruptQueues {
     UpdatePositionQueue left_update_queue;
 };
 
+extern "C" using diag0_handler = void(*)();
+
 namespace gear_motor {
 
 struct UnavailableGearHardware {};
diff --git a/include/pipettes/core/linear_motor_tasks.hpp b/include/pipettes/core/linear_motor_tasks.hpp
index ae20e52d8..c4511567a 100644
--- a/include/pipettes/core/linear_motor_tasks.hpp
+++ b/include/pipettes/core/linear_motor_tasks.hpp
@@ -12,6 +12,7 @@
 #include "motor-control/core/tasks/move_status_reporter_task.hpp"
 #include "motor-control/core/tasks/tmc2130_motor_driver_task.hpp"
 #include "motor-control/core/tasks/tmc2160_motor_driver_task.hpp"
+#include "pipettes/core/interfaces.hpp"
 #include "pipettes/core/sensor_tasks.hpp"
 #include "spi/core/writer.hpp"
 
@@ -31,7 +32,7 @@ using SPIWriterClient =
     spi::writer::Writer<freertos_message_queue::FreeRTOSMessageQueue>;
 
 // single channel/8 channel linear motor tasks
-void start_tasks(
+auto start_tasks(
     CanWriterTask& can_writer,
     motion_controller::MotionController<lms::LeadScrewConfig>&
         motion_controller,
@@ -39,10 +40,10 @@ void start_tasks(
     tmc2130::configs::TMC2130DriverConfig& linear_driver_configs,
     can::ids::NodeId, motor_hardware_task::MotorHardwareTask& lmh_tsk,
     eeprom::dev_data::DevDataTailAccessor<sensor_tasks::QueueClient>&
-        tail_accessor);
+        tail_accessor) -> interfaces::diag0_handler;
 
 // 96/384 linear motor tasks
-void start_tasks(
+auto start_tasks(
     CanWriterTask& can_writer,
     motion_controller::MotionController<lms::LeadScrewConfig>&
         motion_controller,
@@ -50,7 +51,9 @@ void start_tasks(
     tmc2160::configs::TMC2160DriverConfig& linear_driver_configs,
     can::ids::NodeId, motor_hardware_task::MotorHardwareTask& lmh_tsk,
     eeprom::dev_data::DevDataTailAccessor<sensor_tasks::QueueClient>&
-        tail_accessor);
+        tail_accessor) -> interfaces::diag0_handler;
+
+void call_run_diag0_interrupt();
 
 /**
  * Access to all the linear motion task queues on the pipette.
@@ -134,6 +137,11 @@ struct QueueClient : can::message_writer::MessageWriter {
         driver_queue->try_write(m);
     }
 
+    void send_motor_driver_queue_isr(
+        const tmc2130::tasks::TaskMessage& m) const {
+        static_cast<void>(driver_queue->try_write_isr(m));
+    }
+
     freertos_message_queue::FreeRTOSMessageQueue<tmc2130::tasks::TaskMessage>*
         driver_queue{nullptr};
 };
@@ -173,6 +181,11 @@ struct QueueClient : can::message_writer::MessageWriter {
         driver_queue->try_write(m);
     }
 
+    void send_motor_driver_queue_isr(
+        const tmc2160::tasks::TaskMessage& m) const {
+        static_cast<void>(driver_queue->try_write_isr(m));
+    }
+
     freertos_message_queue::FreeRTOSMessageQueue<tmc2160::tasks::TaskMessage>*
         driver_queue{nullptr};
 };
diff --git a/include/pipettes/core/tasks.hpp b/include/pipettes/core/tasks.hpp
index 2a53bc335..149beb395 100644
--- a/include/pipettes/core/tasks.hpp
+++ b/include/pipettes/core/tasks.hpp
@@ -49,6 +49,8 @@ struct QueueClient : can::message_writer::MessageWriter {
 
     void send_motor_driver_queue(const tmc2130::tasks::TaskMessage& m);
 
+    void send_motor_driver_queue_isr(const tmc2130::tasks::TaskMessage& m);
+
     void send_move_group_queue(const move_group_task::TaskMessage& m);
 
     void send_move_status_reporter_queue(
diff --git a/include/pipettes/core/tasks/messages.hpp b/include/pipettes/core/tasks/messages.hpp
index 7dcd6388d..ed827881f 100644
--- a/include/pipettes/core/tasks/messages.hpp
+++ b/include/pipettes/core/tasks/messages.hpp
@@ -10,6 +10,11 @@ namespace task_messages {
 
 namespace motor_control_task_messages {
 
+struct RouteMotorDriverInterrupt {
+    uint32_t message_index;
+    uint8_t debounce_count;
+};
+
 using MotionControlTaskMessage = std::variant<
     std::monostate, can::messages::TipActionRequest,
     can::messages::GearDisableMotorRequest,
@@ -17,7 +22,7 @@ using MotionControlTaskMessage = std::variant<
     can::messages::GetMotionConstraintsRequest,
     can::messages::SetMotionConstraints, can::messages::StopRequest,
     can::messages::ReadLimitSwitchRequest, can::messages::GetMotorUsageRequest,
-    can::messages::MotorStatusRequest>;
+    can::messages::MotorStatusRequest, RouteMotorDriverInterrupt>;
 
 using MoveStatusReporterTaskMessage =
     std::variant<std::monostate, motor_messages::GearMotorAck,
diff --git a/include/pipettes/core/tasks/motion_controller_task.hpp b/include/pipettes/core/tasks/motion_controller_task.hpp
index 71c3b3539..5c9db1062 100644
--- a/include/pipettes/core/tasks/motion_controller_task.hpp
+++ b/include/pipettes/core/tasks/motion_controller_task.hpp
@@ -7,6 +7,7 @@
 #include "common/core/logging.h"
 #include "motor-control/core/linear_motion_system.hpp"
 #include "motor-control/core/stepper_motor/motion_controller.hpp"
+#include "motor-control/core/tasks/tmc_motor_driver_common.hpp"
 #include "motor-control/core/tasks/usage_storage_task.hpp"
 #include "pipettes/core/tasks/messages.hpp"
 
@@ -19,22 +20,38 @@ namespace motion_controller_task {
 using TaskMessage = pipettes::task_messages::motor_control_task_messages::
     MotionControlTaskMessage;
 
+/**
+ * Concept describing a class that can message this task.
+ * @tparam Client
+ */
+template <typename Client>
+concept TaskClient = requires(Client client, const TaskMessage& m) {
+    {client.send_motion_controller_queue(m)};
+};
+
 /**
  * The message queue message handler.
  */
 template <lms::MotorMechanicalConfig MEConfig,
           can::message_writer_task::TaskClient CanClient,
-          usage_storage_task::TaskClient UsageClient>
+          usage_storage_task::TaskClient UsageClient,
+          tmc::tasks::GearTaskClient DriverClient, TaskClient MotionClient>
 class MotionControllerMessageHandler {
   public:
     using MotorControllerType =
         pipette_motion_controller::PipetteMotionController<MEConfig>;
     MotionControllerMessageHandler(MotorControllerType& controller,
                                    CanClient& can_client,
-                                   UsageClient& usage_client)
+                                   UsageClient& usage_client,
+                                   DriverClient& driver_client,
+                                   MotionClient& motion_client,
+                                   std::atomic<bool>& diag0_debounced)
         : controller{controller},
           can_client{can_client},
-          usage_client{usage_client} {}
+          usage_client{usage_client},
+          driver_client{driver_client},
+          motion_client{motion_client},
+          diag0_debounced{diag0_debounced} {}
     MotionControllerMessageHandler(const MotionControllerMessageHandler& c) =
         delete;
     MotionControllerMessageHandler(const MotionControllerMessageHandler&& c) =
@@ -61,9 +78,19 @@ class MotionControllerMessageHandler {
         LOG("Received enable motor request");
         // TODO only toggle the enable pin once since all motors share
         // a single enable pin line.
-        controller.enable_motor();
-        can_client.send_can_message(can::ids::NodeId::host,
-                                    can::messages::ack_from_request(m));
+        if (controller.read_tmc_diag0()) {
+            can_client.send_can_message(
+                can::ids::NodeId::host,
+                can::messages::ErrorMessage{
+                    .message_index = m.message_index,
+                    .severity = can::ids::ErrorSeverity::unrecoverable,
+                    .error_code =
+                        can::ids::ErrorCode::motor_driver_error_detected});
+        } else {
+            controller.enable_motor();
+            can_client.send_can_message(can::ids::NodeId::host,
+                                        can::messages::ack_from_request(m));
+        }
     }
 
     void handle(const can::messages::GearDisableMotorRequest& m) {
@@ -102,7 +129,17 @@ class MotionControllerMessageHandler {
         LOG("Motion Controller Received a tip action request: velocity=%d, "
             "acceleration=%d, groupid=%d, seqid=%d\n",
             m.velocity, m.acceleration, m.group_id, m.seq_id);
-        controller.move(m);
+        if (controller.read_tmc_diag0()) {
+            can_client.send_can_message(
+                can::ids::NodeId::host,
+                can::messages::ErrorMessage{
+                    .message_index = m.message_index,
+                    .severity = can::ids::ErrorSeverity::unrecoverable,
+                    .error_code =
+                        can::ids::ErrorCode::motor_driver_error_detected});
+        } else {
+            controller.move(m);
+        }
     }
 
     void handle(const can::messages::ReadLimitSwitchRequest& m) {
@@ -117,6 +154,44 @@ class MotionControllerMessageHandler {
         controller.send_usage_data(m.message_index, usage_client);
     }
 
+    void handle(const pipettes::task_messages::motor_control_task_messages::
+                    RouteMotorDriverInterrupt& m) {
+        if (m.debounce_count > DIAG0_DEBOUNCE_REPS) {
+            if (controller.read_tmc_diag0()) {
+                controller.stop(
+                    can::ids::ErrorSeverity::unrecoverable,
+                    can::ids::ErrorCode::motor_driver_error_detected);
+                if (!controller.is_timer_interrupt_running()) {
+                    can_client.send_can_message(
+                        can::ids::NodeId::host,
+                        can::messages::ErrorMessage{
+                            .message_index = m.message_index,
+                            .severity = can::ids::ErrorSeverity::unrecoverable,
+                            .error_code = can::ids::ErrorCode::
+                                motor_driver_error_detected});
+                    driver_client.send_motor_driver_queue(
+                        can::messages::ReadMotorDriverErrorStatusRequest{
+                            .message_index = m.message_index});
+                }
+            }
+            diag0_debounced = false;
+        } else {
+            vTaskDelay(pdMS_TO_TICKS(DIAG0_DEBOUNCE_DELAY));
+            motion_client.send_motion_controller_queue(
+                increment_message_debounce_count(m));
+        }
+    }
+
+    auto increment_message_debounce_count(
+        const pipettes::task_messages::motor_control_task_messages::
+            RouteMotorDriverInterrupt& m) -> pipettes::task_messages::
+        motor_control_task_messages::RouteMotorDriverInterrupt {
+        return pipettes::task_messages::motor_control_task_messages::
+            RouteMotorDriverInterrupt{
+                .message_index = m.message_index,
+                .debounce_count = static_cast<uint8_t>(m.debounce_count + 1)};
+    }
+
     void handle(const can::messages::MotorStatusRequest& m) {
         auto response = static_cast<uint8_t>(controller.is_motor_enabled());
         can::messages::GearMotorStatusResponse msg{
@@ -127,6 +202,9 @@ class MotionControllerMessageHandler {
     MotorControllerType& controller;
     CanClient& can_client;
     UsageClient& usage_client;
+    DriverClient& driver_client;
+    MotionClient& motion_client;
+    std::atomic<bool>& diag0_debounced;
 };
 
 /**
@@ -150,13 +228,16 @@ class MotionControllerTask {
      */
     template <lms::MotorMechanicalConfig MEConfig,
               can::message_writer_task::TaskClient CanClient,
-              usage_storage_task::TaskClient UsageClient>
+              usage_storage_task::TaskClient UsageClient,
+              tmc::tasks::GearTaskClient DriverClient, TaskClient MotionClient>
     [[noreturn]] void operator()(
         pipette_motion_controller::PipetteMotionController<MEConfig>*
             controller,
-        CanClient* can_client, UsageClient* usage_client) {
-        auto handler = MotionControllerMessageHandler{*controller, *can_client,
-                                                      *usage_client};
+        CanClient* can_client, UsageClient* usage_client,
+        DriverClient* driver_client, MotionClient* motion_client) {
+        auto handler = MotionControllerMessageHandler{
+            *controller,    *can_client,    *usage_client,
+            *driver_client, *motion_client, diag0_debounced};
         TaskMessage message{};
         for (;;) {
             if (queue.try_read(&message, queue.max_delay)) {
@@ -167,17 +248,19 @@ class MotionControllerTask {
 
     [[nodiscard]] auto get_queue() const -> QueueType& { return queue; }
 
+    void run_diag0_interrupt() {
+        if (!diag0_debounced) {
+            static_cast<void>(queue.try_write_isr(
+                pipettes::task_messages::motor_control_task_messages::
+                    RouteMotorDriverInterrupt{.message_index = 0,
+                                              .debounce_count = 0}));
+            diag0_debounced = true;
+        }
+    }
+
   private:
     QueueType& queue;
-};
-
-/**
- * Concept describing a class that can message this task.
- * @tparam Client
- */
-template <typename Client>
-concept TaskClient = requires(Client client, const TaskMessage& m) {
-    {client.send_motion_controller_queue(m)};
+    std::atomic<bool> diag0_debounced = false;
 };
 
 }  // namespace motion_controller_task
diff --git a/include/pipettes/firmware/interfaces.hpp b/include/pipettes/firmware/interfaces.hpp
index 2ff87fcc4..e39fa08fd 100644
--- a/include/pipettes/firmware/interfaces.hpp
+++ b/include/pipettes/firmware/interfaces.hpp
@@ -20,19 +20,19 @@
 namespace interfaces {
 
 #ifdef USE_SENSOR_MOVE
-template <typename Client>
+template <typename Client, typename DriverClient>
 using MotorInterruptHandlerType = motor_handler::MotorInterruptHandler<
-    freertos_message_queue::FreeRTOSMessageQueue, Client,
+    freertos_message_queue::FreeRTOSMessageQueue, Client, DriverClient,
     motor_messages::SensorSyncMove, motor_hardware::MotorHardware>;
 #else
-template <typename Client>
+template <typename Client, typename DriverClient>
 using MotorInterruptHandlerType = motor_handler::MotorInterruptHandler<
-    freertos_message_queue::FreeRTOSMessageQueue, Client, motor_messages::Move,
-    motor_hardware::MotorHardware>;
+    freertos_message_queue::FreeRTOSMessageQueue, Client, DriverClient,
+    motor_messages::Move, motor_hardware::MotorHardware>;
 #endif
 template <typename Client>
 using GearMotorInterruptHandlerType = motor_handler::MotorInterruptHandler<
-    freertos_message_queue::FreeRTOSMessageQueue, Client,
+    freertos_message_queue::FreeRTOSMessageQueue, Client, Client,
     motor_messages::GearMotorMove, motor_hardware::MotorHardware>;
 
 template <PipetteType P>
@@ -75,11 +75,15 @@ namespace linear_motor {
 auto get_interrupt(motor_hardware::MotorHardware& hw,
                    LowThroughputInterruptQueues& queues,
                    stall_check::StallCheck& stall)
-    -> MotorInterruptHandlerType<linear_motor_tasks::QueueClient>;
+    -> MotorInterruptHandlerType<
+        linear_motor_tasks::QueueClient,
+        linear_motor_tasks::tmc2130_driver::QueueClient>;
 auto get_interrupt(motor_hardware::MotorHardware& hw,
                    HighThroughputInterruptQueues& queues,
                    stall_check::StallCheck& stall)
-    -> MotorInterruptHandlerType<linear_motor_tasks::QueueClient>;
+    -> MotorInterruptHandlerType<
+        linear_motor_tasks::QueueClient,
+        linear_motor_tasks::tmc2160_driver::QueueClient>;
 auto get_motor_hardware(motor_configs::LowThroughputPipetteMotorHardware pins)
     -> motor_hardware::MotorHardware;
 auto get_motor_hardware(motor_configs::HighThroughputPipetteMotorHardware pins)
diff --git a/include/pipettes/firmware/interfaces_g4.hpp b/include/pipettes/firmware/interfaces_g4.hpp
index ecf9c0813..d50466777 100644
--- a/include/pipettes/firmware/interfaces_g4.hpp
+++ b/include/pipettes/firmware/interfaces_g4.hpp
@@ -29,19 +29,19 @@
 namespace interfaces {
 
 #ifdef USE_SENSOR_MOVE
-template <typename Client>
+template <typename Client, typename DriverClient>
 using MotorInterruptHandlerType = motor_handler::MotorInterruptHandler<
-    freertos_message_queue::FreeRTOSMessageQueue, Client,
+    freertos_message_queue::FreeRTOSMessageQueue, Client, DriverClient,
     motor_messages::SensorSyncMove, motor_hardware::MotorHardware>;
 #else
-template <typename Client>
+template <typename Client, typename DriverClient>
 using MotorInterruptHandlerType = motor_handler::MotorInterruptHandler<
-    freertos_message_queue::FreeRTOSMessageQueue, Client, motor_messages::Move,
-    motor_hardware::MotorHardware>;
+    freertos_message_queue::FreeRTOSMessageQueue, Client, DriverClient,
+    motor_messages::Move, motor_hardware::MotorHardware>;
 #endif
 template <typename Client>
 using GearMotorInterruptHandlerType = motor_handler::MotorInterruptHandler<
-    freertos_message_queue::FreeRTOSMessageQueue, Client,
+    freertos_message_queue::FreeRTOSMessageQueue, Client, Client,
     motor_messages::GearMotorMove, motor_hardware::MotorHardware>;
 
 template <PipetteType P>
@@ -84,11 +84,15 @@ namespace linear_motor {
 auto get_interrupt(motor_hardware::MotorHardware& hw,
                    LowThroughputInterruptQueues& queues,
                    stall_check::StallCheck& stall)
-    -> MotorInterruptHandlerType<linear_motor_tasks::QueueClient>;
+    -> MotorInterruptHandlerType<
+        linear_motor_tasks::QueueClient,
+        linear_motor_tasks::tmc2130_driver::QueueClient>;
 auto get_interrupt(motor_hardware::MotorHardware& hw,
                    HighThroughputInterruptQueues& queues,
                    stall_check::StallCheck& stall)
-    -> MotorInterruptHandlerType<linear_motor_tasks::QueueClient>;
+    -> MotorInterruptHandlerType<
+        linear_motor_tasks::QueueClient,
+        linear_motor_tasks::tmc2160_driver::QueueClient>;
 auto get_motor_hardware(motor_hardware::HardwareConfig pins)
     -> motor_hardware::MotorHardware;
 auto get_motion_control(motor_hardware::MotorHardware& hw,
diff --git a/include/pipettes/simulator/interfaces.hpp b/include/pipettes/simulator/interfaces.hpp
index 59ee3fbed..e4a9d3446 100644
--- a/include/pipettes/simulator/interfaces.hpp
+++ b/include/pipettes/simulator/interfaces.hpp
@@ -12,22 +12,22 @@
 
 namespace interfaces {
 
-template <typename Client>
+template <typename Client, typename DriverClient>
 using MotorInterruptHandlerType =
 #ifdef USE_SENSOR_MOVE
     motor_handler::MotorInterruptHandler<
-        freertos_message_queue::FreeRTOSMessageQueue, Client,
+        freertos_message_queue::FreeRTOSMessageQueue, Client, DriverClient,
         motor_messages::SensorSyncMove,
         sim_motor_hardware_iface::SimMotorHardwareIface>;
 #else
     motor_handler::MotorInterruptHandler<
-        freertos_message_queue::FreeRTOSMessageQueue, Client,
+        freertos_message_queue::FreeRTOSMessageQueue, Client, DriverClient,
         motor_messages::Move, sim_motor_hardware_iface::SimMotorHardwareIface>;
 #endif
 
 template <typename Client>
 using GearMotorInterruptHandlerType = motor_handler::MotorInterruptHandler<
-    freertos_message_queue::FreeRTOSMessageQueue, Client,
+    freertos_message_queue::FreeRTOSMessageQueue, Client, Client,
     motor_messages::GearMotorMove,
     sim_motor_hardware_iface::SimGearMotorHardwareIface>;
 
@@ -71,38 +71,52 @@ namespace linear_motor {
 auto get_interrupt(sim_motor_hardware_iface::SimMotorHardwareIface& hw,
                    LowThroughputInterruptQueues& queues,
                    stall_check::StallCheck& stall)
-    -> MotorInterruptHandlerType<linear_motor_tasks::QueueClient>;
+    -> MotorInterruptHandlerType<
+        linear_motor_tasks::QueueClient,
+        linear_motor_tasks::tmc2130_driver::QueueClient>;
 
 auto get_interrupt(sim_motor_hardware_iface::SimMotorHardwareIface& hw,
                    HighThroughputInterruptQueues& queues,
                    stall_check::StallCheck& stall)
-    -> MotorInterruptHandlerType<linear_motor_tasks::QueueClient>;
+    -> MotorInterruptHandlerType<
+        linear_motor_tasks::QueueClient,
+        linear_motor_tasks::tmc2160_driver::QueueClient>;
 
 auto get_interrupt_driver(
     sim_motor_hardware_iface::SimMotorHardwareIface& hw,
     LowThroughputInterruptQueues& queues,
-    MotorInterruptHandlerType<linear_motor_tasks::QueueClient>& handler) ->
+    MotorInterruptHandlerType<linear_motor_tasks::QueueClient,
+                              linear_motor_tasks::tmc2130_driver::QueueClient>&
+        handler) ->
 #ifdef USE_SENSOR_MOVE
     motor_interrupt_driver::MotorInterruptDriver<
-        linear_motor_tasks::QueueClient, motor_messages::SensorSyncMove,
+        linear_motor_tasks::QueueClient,
+        linear_motor_tasks::tmc2130_driver::QueueClient,
+        motor_messages::SensorSyncMove,
         sim_motor_hardware_iface::SimMotorHardwareIface>;
 #else
     motor_interrupt_driver::MotorInterruptDriver<
-        linear_motor_tasks::QueueClient, motor_messages::Move,
+        linear_motor_tasks::QueueClient,
+        linear_motor_tasks::tmc2130_driver::QueueClient, motor_messages::Move,
         sim_motor_hardware_iface::SimMotorHardwareIface>;
 #endif
 
 auto get_interrupt_driver(
     sim_motor_hardware_iface::SimMotorHardwareIface& hw,
     HighThroughputInterruptQueues& queues,
-    MotorInterruptHandlerType<linear_motor_tasks::QueueClient>& handler) ->
+    MotorInterruptHandlerType<linear_motor_tasks::QueueClient,
+                              linear_motor_tasks::tmc2160_driver::QueueClient>&
+        handler) ->
 #ifdef USE_SENSOR_MOVE
     motor_interrupt_driver::MotorInterruptDriver<
-        linear_motor_tasks::QueueClient, motor_messages::SensorSyncMove,
+        linear_motor_tasks::QueueClient,
+        linear_motor_tasks::tmc2160_driver::QueueClient,
+        motor_messages::SensorSyncMove,
         sim_motor_hardware_iface::SimMotorHardwareIface>;
 #else
     motor_interrupt_driver::MotorInterruptDriver<
-        linear_motor_tasks::QueueClient, motor_messages::Move,
+        linear_motor_tasks::QueueClient,
+        linear_motor_tasks::tmc2130_driver::QueueClient, motor_messages::Move,
         sim_motor_hardware_iface::SimMotorHardwareIface>;
 #endif
 
@@ -126,11 +140,13 @@ struct GearInterruptHandlers {
 
 struct GearInterruptDrivers {
     motor_interrupt_driver::MotorInterruptDriver<
-        gear_motor_tasks::QueueClient, motor_messages::GearMotorMove,
+        gear_motor_tasks::QueueClient, gear_motor_tasks::QueueClient,
+        motor_messages::GearMotorMove,
         sim_motor_hardware_iface::SimGearMotorHardwareIface>
         left;
     motor_interrupt_driver::MotorInterruptDriver<
-        gear_motor_tasks::QueueClient, motor_messages::GearMotorMove,
+        gear_motor_tasks::QueueClient, gear_motor_tasks::QueueClient,
+        motor_messages::GearMotorMove,
         sim_motor_hardware_iface::SimGearMotorHardwareIface>
         right;
 };
diff --git a/include/spi/core/utils.hpp b/include/spi/core/utils.hpp
index e41b4468f..0f28e89fa 100644
--- a/include/spi/core/utils.hpp
+++ b/include/spi/core/utils.hpp
@@ -16,6 +16,39 @@ struct ChipSelectInterface {
     void* GPIO_handle;
 };
 
+// Bit positions to pack in an 8 bit response tag
+enum class ResponseTag : size_t {
+    IS_ERROR_RESPONSE = 0,
+};
+
+[[nodiscard]] constexpr auto byte_from_tag(ResponseTag tag) -> uint8_t {
+    return (1 << static_cast<size_t>(tag));
+}
+
+template <std::ranges::range Tags>
+[[nodiscard]] auto byte_from_tags(const Tags& tags) -> uint8_t {
+    uint8_t result = 0;
+    for (auto tag : tags) {
+        result |= byte_from_tag(tag);
+    }
+    return result;
+}
+
+[[nodiscard]] inline constexpr auto tag_in_token(uint32_t token,
+                                                 ResponseTag tag) -> bool {
+    return bool(token &
+                (1 << (static_cast<size_t>(tag) + static_cast<size_t>(8))));
+}
+
+[[nodiscard]] inline constexpr auto build_token(uint8_t reg, uint8_t tags = 0)
+    -> uint32_t {
+    return (static_cast<uint32_t>(tags) << 8) | (reg);
+}
+
+template <typename RegType>
+[[nodiscard]] inline constexpr auto reg_from_token(uint32_t id) -> RegType {
+    return static_cast<RegType>(static_cast<uint8_t>(id & 0xff));
+}
 }  // namespace utils
 
-}  // namespace spi
\ No newline at end of file
+}  // namespace spi
diff --git a/include/spi/core/writer.hpp b/include/spi/core/writer.hpp
index a3e2e1710..cf887f4b6 100644
--- a/include/spi/core/writer.hpp
+++ b/include/spi/core/writer.hpp
@@ -46,13 +46,13 @@ class Writer {
      * @return A success boolean
      */
     template <OriginatingResponseQueue RQType>
-    auto read(uint8_t register_addr, uint32_t command_data,
-              RQType& response_queue, utils::ChipSelectInterface cs_intf,
-              uint32_t message_index) -> bool {
-        auto txBuffer = build_message(register_addr, spi::hardware::Mode::READ,
-                                      command_data);
+    auto read(uint32_t token, uint32_t command_data, RQType& response_queue,
+              utils::ChipSelectInterface cs_intf, uint32_t message_index)
+        -> bool {
+        auto txBuffer = build_message(utils::reg_from_token<uint8_t>(token),
+                                      spi::hardware::Mode::READ, command_data);
         TransactionIdentifier _transaction_id{
-            .token = register_addr,
+            .token = token,
             .command_type = static_cast<uint8_t>(spi::hardware::Mode::READ),
             .requires_response = false,
             .message_index = message_index};
@@ -83,7 +83,7 @@ class Writer {
         auto txBuffer = build_message(register_addr, spi::hardware::Mode::WRITE,
                                       command_data);
         TransactionIdentifier _transaction_id{
-            .token = register_addr,
+            .token = utils::build_token(register_addr),
             .command_type = static_cast<uint8_t>(spi::hardware::Mode::WRITE),
             .requires_response = true};
         Transact message{
diff --git a/motor-control/firmware/brushed_motor/brushed_motor_hardware.cpp b/motor-control/firmware/brushed_motor/brushed_motor_hardware.cpp
index 2b5d1314d..1d7690cfd 100644
--- a/motor-control/firmware/brushed_motor/brushed_motor_hardware.cpp
+++ b/motor-control/firmware/brushed_motor/brushed_motor_hardware.cpp
@@ -58,6 +58,8 @@ void BrushedMotorHardware::read_sync_in() {
     sync.debounce_update(gpio::is_set(pins.sync_in));
 }
 
+bool BrushedMotorHardware::read_tmc_diag0() { return 0; }
+
 int32_t BrushedMotorHardware::get_encoder_pulses() {
     if (!enc_handle) {
         return 0;
diff --git a/motor-control/firmware/stepper_motor/motor_hardware.cpp b/motor-control/firmware/stepper_motor/motor_hardware.cpp
index 76c64faa1..7cee57b34 100644
--- a/motor-control/firmware/stepper_motor/motor_hardware.cpp
+++ b/motor-control/firmware/stepper_motor/motor_hardware.cpp
@@ -50,6 +50,8 @@ void MotorHardware::read_estop_in() {
 
 void MotorHardware::read_sync_in() { sync = gpio::is_set(pins.sync_in); }
 
+bool MotorHardware::read_tmc_diag0() { return gpio::is_set(pins.diag0); }
+
 void MotorHardware::set_LED(bool status) {
     if (status) {
         gpio::set(pins.led);
diff --git a/motor-control/tests/test_brushed_motor_interrupt_handler.cpp b/motor-control/tests/test_brushed_motor_interrupt_handler.cpp
index 578754c46..9f1e9e7a7 100644
--- a/motor-control/tests/test_brushed_motor_interrupt_handler.cpp
+++ b/motor-control/tests/test_brushed_motor_interrupt_handler.cpp
@@ -523,7 +523,9 @@ SCENARIO("handler recovers from error state") {
         test_objs.handler.error_handled = true;
 
         WHEN("a cancel request is received") {
-            test_objs.hw.request_cancel();
+            test_objs.hw.set_cancel_request(
+                can::ids::ErrorSeverity::warning,
+                can::ids::ErrorCode::stop_requested);
             test_objs.handler.run_interrupt();
             THEN(
                 "motor state should become un-homed only if stay engaged is "
diff --git a/motor-control/tests/test_limit_switch.cpp b/motor-control/tests/test_limit_switch.cpp
index 8dbd107a2..962874bee 100644
--- a/motor-control/tests/test_limit_switch.cpp
+++ b/motor-control/tests/test_limit_switch.cpp
@@ -2,6 +2,7 @@
 #include "common/tests/mock_message_queue.hpp"
 #include "motor-control/core/motor_messages.hpp"
 #include "motor-control/core/stepper_motor/motor_interrupt_handler.hpp"
+#include "motor-control/tests/mock_motor_driver_client.hpp"
 #include "motor-control/tests/mock_motor_hardware.hpp"
 #include "motor-control/tests/mock_move_status_reporter_client.hpp"
 
@@ -16,11 +17,12 @@ struct HandlerContainer {
         can::messages::UpdateMotorPositionEstimationRequest>
         update_position_queue{};
     test_mocks::MockMoveStatusReporterClient reporter{};
+    test_mocks::MockMotorDriverClient driver{};
     stall_check::StallCheck stall{10, 10, 10};
-    MotorInterruptHandler<test_mocks::MockMessageQueue,
-                          test_mocks::MockMoveStatusReporterClient, Move,
-                          test_mocks::MockMotorHardware>
-        handler{queue, reporter, hw, stall, update_position_queue};
+    MotorInterruptHandler<
+        test_mocks::MockMessageQueue, test_mocks::MockMoveStatusReporterClient,
+        test_mocks::MockMotorDriverClient, Move, test_mocks::MockMotorHardware>
+        handler{queue, reporter, driver, hw, stall, update_position_queue};
 };
 
 static constexpr sq0_31 default_velocity =
diff --git a/motor-control/tests/test_limit_switch_backoff.cpp b/motor-control/tests/test_limit_switch_backoff.cpp
index c4967c890..df1e462a1 100644
--- a/motor-control/tests/test_limit_switch_backoff.cpp
+++ b/motor-control/tests/test_limit_switch_backoff.cpp
@@ -2,6 +2,7 @@
 #include "common/tests/mock_message_queue.hpp"
 #include "motor-control/core/motor_messages.hpp"
 #include "motor-control/core/stepper_motor/motor_interrupt_handler.hpp"
+#include "motor-control/tests/mock_motor_driver_client.hpp"
 #include "motor-control/tests/mock_motor_hardware.hpp"
 #include "motor-control/tests/mock_move_status_reporter_client.hpp"
 
@@ -16,11 +17,12 @@ struct HandlerContainer {
         can::messages::UpdateMotorPositionEstimationRequest>
         update_position_queue{};
     test_mocks::MockMoveStatusReporterClient reporter{};
+    test_mocks::MockMotorDriverClient driver{};
     stall_check::StallCheck stall{10, 10, 10};
-    MotorInterruptHandler<test_mocks::MockMessageQueue,
-                          test_mocks::MockMoveStatusReporterClient, Move,
-                          test_mocks::MockMotorHardware>
-        handler{queue, reporter, hw, stall, update_position_queue};
+    MotorInterruptHandler<
+        test_mocks::MockMessageQueue, test_mocks::MockMoveStatusReporterClient,
+        test_mocks::MockMotorDriverClient, Move, test_mocks::MockMotorHardware>
+        handler{queue, reporter, driver, hw, stall, update_position_queue};
 };
 
 SCENARIO(
diff --git a/motor-control/tests/test_motor_interrupt_handler.cpp b/motor-control/tests/test_motor_interrupt_handler.cpp
index cb83e8443..3c44db5f4 100644
--- a/motor-control/tests/test_motor_interrupt_handler.cpp
+++ b/motor-control/tests/test_motor_interrupt_handler.cpp
@@ -3,6 +3,7 @@
 #include "common/tests/mock_message_queue.hpp"
 #include "motor-control/core/motor_messages.hpp"
 #include "motor-control/core/stepper_motor/motor_interrupt_handler.hpp"
+#include "motor-control/tests/mock_motor_driver_client.hpp"
 #include "motor-control/tests/mock_motor_hardware.hpp"
 #include "motor-control/tests/mock_move_status_reporter_client.hpp"
 
@@ -15,11 +16,13 @@ struct MotorContainer {
         can::messages::UpdateMotorPositionEstimationRequest>
         update_position_queue{};
     test_mocks::MockMoveStatusReporterClient reporter{};
+    test_mocks::MockMotorDriverClient driver{};
     stall_check::StallCheck st{1, 1, 10};
     MotorInterruptHandler<test_mocks::MockMessageQueue,
                           test_mocks::MockMoveStatusReporterClient,
+                          test_mocks::MockMotorDriverClient,
                           motor_messages::Move, test_mocks::MockMotorHardware>
-        handler{queue, reporter, hw, st, update_position_queue};
+        handler{queue, reporter, driver, hw, st, update_position_queue};
 };
 
 SCENARIO("a move is cancelled due to a stop request") {
@@ -41,7 +44,9 @@ SCENARIO("a move is cancelled due to a stop request") {
             test_objs.handler.run_interrupt();
             test_objs.handler.run_interrupt();
             CHECK(test_objs.hw.steps_taken() == 1);
-            test_objs.hw.request_cancel();
+            test_objs.hw.set_cancel_request(
+                can::ids::ErrorSeverity::warning,
+                can::ids::ErrorCode::stop_requested);
             test_objs.handler.run_interrupt();
             THEN("An error and increase error count is sent") {
                 REQUIRE(test_objs.reporter.messages.size() == 2);
diff --git a/motor-control/tests/test_motor_interrupt_queue.cpp b/motor-control/tests/test_motor_interrupt_queue.cpp
index 9d3de068e..906da0ebd 100644
--- a/motor-control/tests/test_motor_interrupt_queue.cpp
+++ b/motor-control/tests/test_motor_interrupt_queue.cpp
@@ -2,6 +2,7 @@
 #include "catch2/catch.hpp"
 #include "common/tests/mock_message_queue.hpp"
 #include "motor-control/core/stepper_motor/motor_interrupt_handler.hpp"
+#include "motor-control/tests/mock_motor_driver_client.hpp"
 #include "motor-control/tests/mock_motor_hardware.hpp"
 #include "motor-control/tests/mock_move_status_reporter_client.hpp"
 
@@ -15,10 +16,11 @@ TEST_CASE("motor interrupt handler queue functionality") {
             can::messages::UpdateMotorPositionEstimationRequest>
             update_position_queue;
         test_mocks::MockMoveStatusReporterClient reporter{};
+        test_mocks::MockMotorDriverClient driver{};
         test_mocks::MockMotorHardware hardware;
         stall_check::StallCheck stall(10, 10, 10);
-        auto handler = MotorInterruptHandler(queue, reporter, hardware, stall,
-                                             update_position_queue);
+        auto handler = MotorInterruptHandler(queue, reporter, driver, hardware,
+                                             stall, update_position_queue);
 
         WHEN("add multiple moves to the queue") {
             THEN("all the moves should exist in order") {
diff --git a/motor-control/tests/test_motor_pulse.cpp b/motor-control/tests/test_motor_pulse.cpp
index 856edaad1..312aa86d2 100644
--- a/motor-control/tests/test_motor_pulse.cpp
+++ b/motor-control/tests/test_motor_pulse.cpp
@@ -1,6 +1,7 @@
 #include "catch2/catch.hpp"
 #include "common/tests/mock_message_queue.hpp"
 #include "motor-control/core/stepper_motor/motor_interrupt_handler.hpp"
+#include "motor-control/tests/mock_motor_driver_client.hpp"
 #include "motor-control/tests/mock_motor_hardware.hpp"
 #include "motor-control/tests/mock_move_status_reporter_client.hpp"
 
@@ -21,11 +22,12 @@ struct HandlerContainer {
         can::messages::UpdateMotorPositionEstimationRequest>
         update_position_queue{};
     test_mocks::MockMoveStatusReporterClient reporter{};
+    test_mocks::MockMotorDriverClient driver{};
     stall_check::StallCheck stall{tick_per_um, tick_per_um, stall_threshold_um};
-    MotorInterruptHandler<test_mocks::MockMessageQueue,
-                          test_mocks::MockMoveStatusReporterClient, Move,
-                          test_mocks::MockMotorHardware>
-        handler{queue, reporter, hw, stall, update_position_queue};
+    MotorInterruptHandler<
+        test_mocks::MockMessageQueue, test_mocks::MockMoveStatusReporterClient,
+        test_mocks::MockMotorDriverClient, Move, test_mocks::MockMotorHardware>
+        handler{queue, reporter, driver, hw, stall, update_position_queue};
 };
 
 sq0_31 convert_velocity(float f) {
diff --git a/motor-control/tests/test_motor_stall_handling.cpp b/motor-control/tests/test_motor_stall_handling.cpp
index 07ddb5006..f75b2db23 100644
--- a/motor-control/tests/test_motor_stall_handling.cpp
+++ b/motor-control/tests/test_motor_stall_handling.cpp
@@ -2,6 +2,7 @@
 #include "common/tests/mock_message_queue.hpp"
 #include "motor-control/core/stepper_motor/motor_interrupt_handler.hpp"
 #include "motor-control/core/usage_messages.hpp"
+#include "motor-control/tests/mock_motor_driver_client.hpp"
 #include "motor-control/tests/mock_motor_hardware.hpp"
 #include "motor-control/tests/mock_move_status_reporter_client.hpp"
 
@@ -21,11 +22,12 @@ struct HandlerContainer {
         can::messages::UpdateMotorPositionEstimationRequest>
         update_position_queue{};
     test_mocks::MockMoveStatusReporterClient reporter{};
+    test_mocks::MockMotorDriverClient driver{};
     stall_check::StallCheck stall{tick_per_um, tick_per_um, stall_threshold_um};
-    MotorInterruptHandler<test_mocks::MockMessageQueue,
-                          test_mocks::MockMoveStatusReporterClient, Move,
-                          test_mocks::MockMotorHardware>
-        handler{queue, reporter, hw, stall, update_position_queue};
+    MotorInterruptHandler<
+        test_mocks::MockMessageQueue, test_mocks::MockMoveStatusReporterClient,
+        test_mocks::MockMotorDriverClient, Move, test_mocks::MockMotorHardware>
+        handler{queue, reporter, driver, hw, stall, update_position_queue};
 };
 
 SCENARIO("motor handler stall detection") {
diff --git a/motor-control/tests/test_sync_handling.cpp b/motor-control/tests/test_sync_handling.cpp
index aa59325a5..f35e21d0c 100644
--- a/motor-control/tests/test_sync_handling.cpp
+++ b/motor-control/tests/test_sync_handling.cpp
@@ -2,6 +2,7 @@
 #include "common/tests/mock_message_queue.hpp"
 #include "motor-control/core/motor_messages.hpp"
 #include "motor-control/core/stepper_motor/motor_interrupt_handler.hpp"
+#include "motor-control/tests/mock_motor_driver_client.hpp"
 #include "motor-control/tests/mock_motor_hardware.hpp"
 #include "motor-control/tests/mock_move_status_reporter_client.hpp"
 
@@ -16,11 +17,12 @@ struct HandlerContainer {
         can::messages::UpdateMotorPositionEstimationRequest>
         update_position_queue{};
     test_mocks::MockMoveStatusReporterClient reporter{};
+    test_mocks::MockMotorDriverClient driver{};
     stall_check::StallCheck stall{10, 10, 10};
-    MotorInterruptHandler<test_mocks::MockMessageQueue,
-                          test_mocks::MockMoveStatusReporterClient, Move,
-                          test_mocks::MockMotorHardware>
-        handler{queue, reporter, hw, stall, update_position_queue};
+    MotorInterruptHandler<
+        test_mocks::MockMessageQueue, test_mocks::MockMoveStatusReporterClient,
+        test_mocks::MockMotorDriverClient, Move, test_mocks::MockMotorHardware>
+        handler{queue, reporter, driver, hw, stall, update_position_queue};
 };
 
 static constexpr sq0_31 default_velocity =
diff --git a/pipettes/core/gear_motor_tasks.cpp b/pipettes/core/gear_motor_tasks.cpp
index c5caabc5d..43e4d5585 100644
--- a/pipettes/core/gear_motor_tasks.cpp
+++ b/pipettes/core/gear_motor_tasks.cpp
@@ -37,7 +37,21 @@ static auto move_status_task_builder_right = freertos_task::TaskStarter<
 static auto left_usage_storage_task_builder =
     freertos_task::TaskStarter<256, usage_storage_task::UsageStorageTask>{};
 
-void gear_motor_tasks::start_tasks(
+void call_run_diag0_left_interrupt() {
+    if (gear_motor_tasks::get_left_gear_tasks().motion_controller) {
+        return gear_motor_tasks::get_left_gear_tasks()
+            .motion_controller->run_diag0_interrupt();
+    }
+}
+
+void call_run_diag0_right_interrupt() {
+    if (gear_motor_tasks::get_right_gear_tasks().motion_controller) {
+        return gear_motor_tasks::get_right_gear_tasks()
+            .motion_controller->run_diag0_interrupt();
+    }
+}
+
+auto gear_motor_tasks::start_tasks(
     gear_motor_tasks::CanWriterTask& can_writer,
     interfaces::gear_motor::GearMotionControl& motion_controllers,
     gear_motor_tasks::SPIWriterClient& spi_writer,
@@ -45,7 +59,8 @@ void gear_motor_tasks::start_tasks(
     can::ids::NodeId id,
     interfaces::gear_motor::GearMotorHardwareTasks& gmh_tsks,
     eeprom::dev_data::DevDataTailAccessor<sensor_tasks::QueueClient>&
-        tail_accessor) {
+        tail_accessor)
+    -> std::tuple<interfaces::diag0_handler, interfaces::diag0_handler> {
     left_queue_client.set_node_id(id);
     right_queue_client.set_node_id(id);
 
@@ -55,9 +70,9 @@ void gear_motor_tasks::start_tasks(
     auto& right_tasks = gear_motor_tasks::get_right_gear_tasks();
 
     // Left Gear Motor Tasks
-    auto& motion_left = mc_task_builder_left.start(5, "motion controller",
-                                                   motion_controllers.left,
-                                                   left_queues, left_queues);
+    auto& motion_left = mc_task_builder_left.start(
+        5, "motion controller", motion_controllers.left, left_queues,
+        left_queues, left_queues, left_queues);
     auto& tmc2160_driver_left = tmc2160_driver_task_builder_left.start(
         5, "tmc2160 driver", gear_driver_configs.left_gear_motor, left_queues,
         spi_writer);
@@ -87,7 +102,7 @@ void gear_motor_tasks::start_tasks(
     // Right Gear Motor Tasks
     auto& motion_right = mc_task_builder_right.start(
         5, "motion controller", motion_controllers.right, right_queues,
-        right_queues);
+        right_queues, right_queues, right_queues);
     auto& tmc2160_driver_right = tmc2160_driver_task_builder_right.start(
         5, "tmc2160 driver", gear_driver_configs.right_gear_motor, right_queues,
         spi_writer);
@@ -116,6 +131,9 @@ void gear_motor_tasks::start_tasks(
 
     gmh_tsks.left.start_task();
     gmh_tsks.right.start_task();
+
+    return std::make_tuple(call_run_diag0_left_interrupt,
+                           call_run_diag0_right_interrupt);
 }
 
 gear_motor_tasks::QueueClient::QueueClient()
@@ -133,6 +151,11 @@ void gear_motor_tasks::QueueClient::send_motor_driver_queue(
     driver_queue->try_write(m);
 }
 
+void gear_motor_tasks::QueueClient::send_motor_driver_queue_isr(
+    const tmc2160::tasks::gear::TaskMessage& m) {
+    static_cast<void>(driver_queue->try_write_isr(m));
+}
+
 void gear_motor_tasks::QueueClient::send_move_group_queue(
     const pipettes::tasks::move_group_task::TaskMessage& m) {
     move_group_queue->try_write(m);
diff --git a/pipettes/core/linear_motor_tasks.cpp b/pipettes/core/linear_motor_tasks.cpp
index 67909454b..86bd526e3 100644
--- a/pipettes/core/linear_motor_tasks.cpp
+++ b/pipettes/core/linear_motor_tasks.cpp
@@ -31,7 +31,7 @@ static auto linear_usage_storage_task_builder =
 static auto eeprom_data_rev_update_builder =
     freertos_task::TaskStarter<256, eeprom::data_rev_task::UpdateDataRevTask>{};
 
-void linear_motor_tasks::start_tasks(
+auto linear_motor_tasks::start_tasks(
     linear_motor_tasks::CanWriterTask& can_writer,
     motion_controller::MotionController<lms::LeadScrewConfig>&
         motion_controller,
@@ -39,7 +39,7 @@ void linear_motor_tasks::start_tasks(
     tmc2130::configs::TMC2130DriverConfig& linear_driver_configs,
     can::ids::NodeId id, motor_hardware_task::MotorHardwareTask& lmh_tsk,
     eeprom::dev_data::DevDataTailAccessor<sensor_tasks::QueueClient>&
-        tail_accessor) {
+        tail_accessor) -> interfaces::diag0_handler {
     tmc2130_queue_client.set_node_id(id);
     motion_queue_client.set_node_id(id);
 
@@ -49,8 +49,9 @@ void linear_motor_tasks::start_tasks(
     auto& motion_tasks = linear_motor_tasks::get_tasks();
 
     // Linear Motor Tasks
-    auto& motion = mc_task_builder.start(5, "motion controller",
-                                         motion_controller, queues, queues);
+    auto& motion =
+        mc_task_builder.start(5, "motion controller", motion_controller, queues,
+                              queues, tmc2130_queues, queues);
     auto& tmc2130_driver = tmc2130_driver_task_builder.start(
         5, "tmc2130 driver", linear_driver_configs, queues, spi_writer);
     auto& move_group =
@@ -80,9 +81,11 @@ void linear_motor_tasks::start_tasks(
     queues.move_status_report_queue = &move_status_reporter.get_queue();
     queues.usage_storage_queue = &usage_storage_task.get_queue();
     lmh_tsk.start_task();
+
+    return linear_motor_tasks::call_run_diag0_interrupt;
 }
 
-void linear_motor_tasks::start_tasks(
+auto linear_motor_tasks::start_tasks(
     linear_motor_tasks::CanWriterTask& can_writer,
     motion_controller::MotionController<lms::LeadScrewConfig>&
         motion_controller,
@@ -90,7 +93,7 @@ void linear_motor_tasks::start_tasks(
     tmc2160::configs::TMC2160DriverConfig& linear_driver_configs,
     can::ids::NodeId id, motor_hardware_task::MotorHardwareTask& lmh_tsk,
     eeprom::dev_data::DevDataTailAccessor<sensor_tasks::QueueClient>&
-        tail_accessor) {
+        tail_accessor) -> interfaces::diag0_handler {
     tmc2160_queue_client.set_node_id(id);
     motion_queue_client.set_node_id(id);
 
@@ -100,8 +103,9 @@ void linear_motor_tasks::start_tasks(
     auto& motion_tasks = linear_motor_tasks::get_tasks();
 
     // Linear Motor Tasks
-    auto& motion = mc_task_builder.start(5, "motion controller",
-                                         motion_controller, queues, queues);
+    auto& motion =
+        mc_task_builder.start(5, "motion controller", motion_controller, queues,
+                              queues, tmc2160_queues, queues);
     auto& tmc2160_driver = tmc2160_driver_task_builder.start(
         5, "tmc2160 driver", linear_driver_configs, queues, spi_writer);
     auto& move_group =
@@ -133,6 +137,15 @@ void linear_motor_tasks::start_tasks(
     queues.usage_storage_queue = &usage_storage_task.get_queue();
 
     lmh_tsk.start_task();
+
+    return linear_motor_tasks::call_run_diag0_interrupt;
+}
+
+void linear_motor_tasks::call_run_diag0_interrupt() {
+    if (linear_motor_tasks::get_tasks().motion_controller) {
+        return linear_motor_tasks::get_tasks()
+            .motion_controller->run_diag0_interrupt();
+    }
 }
 
 linear_motor_tasks::QueueClient::QueueClient()
diff --git a/pipettes/firmware/hardware_config.c b/pipettes/firmware/hardware_config.c
index 336feaad2..b4ada4b98 100644
--- a/pipettes/firmware/hardware_config.c
+++ b/pipettes/firmware/hardware_config.c
@@ -42,6 +42,9 @@ IRQn_Type get_interrupt_line(GPIOInterruptBlock gpio_pin_type) {
 
             // Tip sense line for the 96 channel (rear)
             // PC7 -> GPIO_EXTI6 (EXTI9_5_IRQn)
+
+            // Diag0 line for the 96 channel
+            // PB6 -> GPIO_EXTI6 (EXTI9_5_IRQn)
             return EXTI9_5_IRQn;
         case gpio_block_15_10:
             // Data ready line for 96 channel (front)
@@ -49,6 +52,9 @@ IRQn_Type get_interrupt_line(GPIOInterruptBlock gpio_pin_type) {
 
             // Tip sense line for the 96 channel (front)
             // PC12 -> GPIO_EXTI11 (EXTI15_10_IRQn)
+
+            // Diag0 line for single and eight channel
+            // PC11 -> GPIO_EXTI11 (EXTI15_10_IRQn)
             return EXTI15_10_IRQn;
         case gpio_block_3:
             // Single channel data ready line
@@ -172,8 +178,7 @@ static uint16_t get_motor_driver_pins_lt(GPIO_TypeDef* for_handle) {
      * Step Pin -> PC7
      * Enable Pin -> PA10
      *
-     * VREF (TMC2130)
-     * PA5
+     * VREF (TMC2130) -> PA5
      */
     switch((uint32_t)for_handle) {
         case (uint32_t)GPIOA: return GPIO_PIN_10;
@@ -219,3 +224,15 @@ uint16_t pipette_hardware_motor_driver_pins(const PipetteType pipette_type, GPIO
             return get_motor_driver_pins_lt(for_handle);
     }
 }
+
+uint16_t pipette_hardware_motor_driver_diag0_pin(const PipetteType pipette_type) {
+    switch (pipette_type) {
+        case NINETY_SIX_CHANNEL:
+        case THREE_EIGHTY_FOUR_CHANNEL:
+            return GPIO_PIN_6;
+        case SINGLE_CHANNEL:
+        case EIGHT_CHANNEL:
+        default:
+            return GPIO_PIN_11;
+    }
+}
diff --git a/pipettes/firmware/hardware_config.h b/pipettes/firmware/hardware_config.h
index d203ddb49..730cd48e2 100644
--- a/pipettes/firmware/hardware_config.h
+++ b/pipettes/firmware/hardware_config.h
@@ -1,4 +1,5 @@
 #pragma once
+#include <stdbool.h>
 #include "pipettes/core/pipette_type.h"
 #include "platform_specific_hal_conf.h"
 
@@ -25,5 +26,6 @@ typedef enum {
 
 uint16_t pipette_hardware_spi_pins(const PipetteType pipette_type, GPIO_TypeDef* which_handle);
 uint16_t pipette_hardware_motor_driver_pins(const PipetteType pipette_type, GPIO_TypeDef* for_handle);
+uint16_t pipette_hardware_motor_driver_diag0_pin(const PipetteType pipette_type);
 PipetteHardwarePin pipette_hardware_get_gpio(const PipetteType pipette_type, PipetteHardwareDevice device);
 IRQn_Type get_interrupt_line(GPIOInterruptBlock gpio_pin_type);
diff --git a/pipettes/firmware/interfaces.cpp b/pipettes/firmware/interfaces.cpp
index ff40c65ef..47a68a2cb 100644
--- a/pipettes/firmware/interfaces.cpp
+++ b/pipettes/firmware/interfaces.cpp
@@ -58,18 +58,24 @@ void linear_motor::encoder_interrupt(motor_hardware::MotorHardware& hw,
 auto linear_motor::get_interrupt(motor_hardware::MotorHardware& hw,
                                  LowThroughputInterruptQueues& queues,
                                  stall_check::StallCheck& stall)
-    -> MotorInterruptHandlerType<linear_motor_tasks::QueueClient> {
+    -> MotorInterruptHandlerType<
+        linear_motor_tasks::QueueClient,
+        linear_motor_tasks::tmc2130_driver::QueueClient> {
     return motor_handler::MotorInterruptHandler(
-        queues.plunger_queue, linear_motor_tasks::get_queues(), hw, stall,
+        queues.plunger_queue, linear_motor_tasks::get_queues(),
+        linear_motor_tasks::tmc2130_driver::get_queues(), hw, stall,
         queues.plunger_update_queue);
 }
 
 auto linear_motor::get_interrupt(motor_hardware::MotorHardware& hw,
                                  HighThroughputInterruptQueues& queues,
                                  stall_check::StallCheck& stall)
-    -> MotorInterruptHandlerType<linear_motor_tasks::QueueClient> {
+    -> MotorInterruptHandlerType<
+        linear_motor_tasks::QueueClient,
+        linear_motor_tasks::tmc2160_driver::QueueClient> {
     return motor_handler::MotorInterruptHandler(
-        queues.plunger_queue, linear_motor_tasks::get_queues(), hw, stall,
+        queues.plunger_queue, linear_motor_tasks::get_queues(),
+        linear_motor_tasks::tmc2160_driver::get_queues(), hw, stall,
         queues.plunger_update_queue);
 }
 
@@ -131,10 +137,12 @@ auto gear_motor::get_interrupts(gear_motor::GearHardware& hw,
     return gear_motor::GearInterruptHandlers{
         .left = motor_handler::MotorInterruptHandler(
             queues.left_motor_queue, gear_motor_tasks::get_left_gear_queues(),
-            hw.left, stall.left, queues.left_update_queue),
+            gear_motor_tasks::get_left_gear_queues(), hw.left, stall.left,
+            queues.left_update_queue),
         .right = motor_handler::MotorInterruptHandler(
             queues.right_motor_queue, gear_motor_tasks::get_right_gear_queues(),
-            hw.right, stall.right, queues.right_update_queue)};
+            gear_motor_tasks::get_right_gear_queues(), hw.right, stall.right,
+            queues.right_update_queue)};
 }
 
 auto gear_motor::get_interrupts(gear_motor::UnavailableGearHardware&,
diff --git a/pipettes/firmware/main.cpp b/pipettes/firmware/main.cpp
index 4025951bd..c76ebce94 100644
--- a/pipettes/firmware/main.cpp
+++ b/pipettes/firmware/main.cpp
@@ -46,6 +46,10 @@ constexpr auto PIPETTE_TYPE = get_pipette_type();
 
 static auto iWatchdog = iwdg::IndependentWatchDog{};
 
+static interfaces::diag0_handler call_linear_diag0_handler = nullptr;
+static interfaces::diag0_handler call_left_gear_diag0_handler = nullptr;
+static interfaces::diag0_handler call_right_gear_diag0_handler = nullptr;
+
 static auto can_bus_1 = can::hal::bus::HalCanBus(
     can_get_device_handle(), utility_configs::led_gpio(PIPETTE_TYPE));
 
@@ -152,6 +156,22 @@ extern "C" void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
                 sensor_queue_client.tip_notification_queue_rear->try_write_isr(
                     sensors::tip_presence::TipStatusChangeDetected{}));
         }
+    } else if (GPIO_Pin == linear_motor_hardware.get_pins().diag0.pin) {
+        if (call_linear_diag0_handler != nullptr) {
+            if (*call_linear_diag0_handler != nullptr) {
+                (*call_linear_diag0_handler)();
+            }
+        }
+        if (call_left_gear_diag0_handler != nullptr) {
+            if (*call_left_gear_diag0_handler != nullptr) {
+                (*call_left_gear_diag0_handler)();
+            }
+        }
+        if (call_right_gear_diag0_handler != nullptr) {
+            if (*call_right_gear_diag0_handler != nullptr) {
+                (*call_right_gear_diag0_handler)();
+            }
+        }
     }
 }
 
@@ -196,13 +216,15 @@ auto initialize_motor_tasks(
     initialize_linear_timer(plunger_callback);
     initialize_gear_timer(gear_callback_wrapper);
     initialize_enc_timer(encoder_callback);
-    linear_motor_tasks::start_tasks(
+    call_linear_diag0_handler = linear_motor_tasks::start_tasks(
         *central_tasks::get_tasks().can_writer, linear_motion_control,
         peripheral_tasks::get_spi_client(), conf.linear_motor, id, lmh_tsk,
         tail_accessor);
-    gear_motor_tasks::start_tasks(
-        *central_tasks::get_tasks().can_writer, gear_motion,
-        peripheral_tasks::get_spi_client(), conf, id, gmh_tsks, tail_accessor);
+    std::tie(call_left_gear_diag0_handler, call_right_gear_diag0_handler) =
+        gear_motor_tasks::start_tasks(*central_tasks::get_tasks().can_writer,
+                                      gear_motion,
+                                      peripheral_tasks::get_spi_client(), conf,
+                                      id, gmh_tsks, tail_accessor);
 }
 auto initialize_motor_tasks(
     can::ids::NodeId id,
@@ -231,7 +253,7 @@ auto initialize_motor_tasks(
 
     initialize_linear_timer(plunger_callback);
     initialize_enc_timer(encoder_callback);
-    linear_motor_tasks::start_tasks(
+    call_linear_diag0_handler = linear_motor_tasks::start_tasks(
         *central_tasks::get_tasks().can_writer, linear_motion_control,
         peripheral_tasks::get_spi_client(), conf.linear_motor, id, lmh_tsk,
         tail_accessor);
diff --git a/pipettes/firmware/motor_configurations.cpp b/pipettes/firmware/motor_configurations.cpp
index 425cd544b..70c96da5e 100644
--- a/pipettes/firmware/motor_configurations.cpp
+++ b/pipettes/firmware/motor_configurations.cpp
@@ -11,7 +11,7 @@ auto motor_configs::driver_config_by_axis(TMC2160PipetteAxis which)
     switch (which) {
         case TMC2160PipetteAxis::left_gear_motor:
             return tmc2160::configs::TMC2160DriverConfig{
-                .registers = {.gconfig = {.en_pwm_mode = 1},
+                .registers = {.gconfig = {.en_pwm_mode = 1, .diag0_error = 1},
                               .ihold_irun = {.hold_current = 16,
                                              .run_current = 31,
                                              .hold_current_delay = 0x7},
@@ -24,6 +24,7 @@ auto motor_configs::driver_config_by_axis(TMC2160PipetteAxis which)
                                            .tbl = 0x2,
                                            .mres = 0x4},
                               .coolconf = {.sgt = 0x6},
+                              .drvconf = {.ot_select = 0},
                               .glob_scale = {.global_scaler = 0xa7}},
                 .current_config =
                     {
@@ -37,7 +38,7 @@ auto motor_configs::driver_config_by_axis(TMC2160PipetteAxis which)
                 }};
         case TMC2160PipetteAxis::right_gear_motor:
             return tmc2160::configs::TMC2160DriverConfig{
-                .registers = {.gconfig = {.en_pwm_mode = 1},
+                .registers = {.gconfig = {.en_pwm_mode = 1, .diag0_error = 1},
                               .ihold_irun = {.hold_current = 16,
                                              .run_current = 31,
                                              .hold_current_delay = 0x7},
@@ -50,6 +51,7 @@ auto motor_configs::driver_config_by_axis(TMC2160PipetteAxis which)
                                            .tbl = 0x2,
                                            .mres = 0x4},
                               .coolconf = {.sgt = 0x6},
+                              .drvconf = {.ot_select = 0},
                               .glob_scale = {.global_scaler = 0xa7}},
                 .current_config =
                     {
@@ -64,7 +66,7 @@ auto motor_configs::driver_config_by_axis(TMC2160PipetteAxis which)
         case TMC2160PipetteAxis::linear_motor:
         default:
             return tmc2160::configs::TMC2160DriverConfig{
-                .registers = {.gconfig = {.en_pwm_mode = 0},
+                .registers = {.gconfig = {.en_pwm_mode = 0, .diag0_error = 1},
                               .ihold_irun = {.hold_current = 16,
                                              .run_current = 31,
                                              .hold_current_delay = 0x7},
@@ -79,6 +81,7 @@ auto motor_configs::driver_config_by_axis(TMC2160PipetteAxis which)
                                            .mres = 0x4,
                                            .intpol = 0x1},
                               .coolconf = {.sgt = 0x6},
+                              .drvconf = {.ot_select = 0},
                               .glob_scale = {.global_scaler = 0xff}},
                 .current_config =
                     {
@@ -99,7 +102,7 @@ auto motor_configs::driver_config_by_axis(TMC2130PipetteAxis which)
         case TMC2130PipetteAxis::linear_motor:
         default:
             return tmc2130::configs::TMC2130DriverConfig{
-                .registers = {.gconfig = {.en_pwm_mode = 1},
+                .registers = {.gconfig = {.en_pwm_mode = 1, .diag0_error = 1},
                               .ihold_irun = {.hold_current = 0x2,
                                              .run_current = 0x10,
                                              .hold_current_delay = 0x7},
@@ -163,6 +166,11 @@ auto motor_configs::hardware_config_by_axis(TMC2130PipetteAxis which)
                      .port = GPIOC,
                      .pin = GPIO_PIN_12,
                      .active_setting = GPIO_PIN_RESET},
+                .diag0 =
+                    {// NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
+                     .port = GPIOC,
+                     .pin = GPIO_PIN_11,
+                     .active_setting = GPIO_PIN_RESET},
             };
     }
 }
@@ -199,6 +207,11 @@ auto motor_configs::hardware_config_by_axis(TMC2160PipetteAxis which)
                      .port = GPIOB,
                      .pin = GPIO_PIN_9,
                      .active_setting = GPIO_PIN_RESET},
+                .diag0 =
+                    {// NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
+                     .port = GPIOB,
+                     .pin = GPIO_PIN_6,
+                     .active_setting = GPIO_PIN_RESET},
             };
         case TMC2160PipetteAxis::left_gear_motor:
             return motor_hardware::HardwareConfig{
@@ -229,6 +242,11 @@ auto motor_configs::hardware_config_by_axis(TMC2160PipetteAxis which)
                      .port = GPIOB,
                      .pin = GPIO_PIN_9,
                      .active_setting = GPIO_PIN_RESET},
+                .diag0 =
+                    {// NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
+                     .port = GPIOB,
+                     .pin = GPIO_PIN_6,
+                     .active_setting = GPIO_PIN_RESET},
             };
         case TMC2160PipetteAxis::linear_motor:
         default:
@@ -265,6 +283,11 @@ auto motor_configs::hardware_config_by_axis(TMC2160PipetteAxis which)
                      .port = GPIOB,
                      .pin = GPIO_PIN_9,
                      .active_setting = GPIO_PIN_RESET},
+                .diag0 =
+                    {// NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
+                     .port = GPIOB,
+                     .pin = GPIO_PIN_6,
+                     .active_setting = GPIO_PIN_RESET},
             };
     }
 }
diff --git a/pipettes/firmware/motor_hardware.c b/pipettes/firmware/motor_hardware.c
index 0792f25d5..867a7d4fd 100644
--- a/pipettes/firmware/motor_hardware.c
+++ b/pipettes/firmware/motor_hardware.c
@@ -94,6 +94,10 @@ void motor_driver_gpio_init() {
         // Enable Dir/Step pin
         GPIO_InitStruct.Pin = pipette_hardware_motor_driver_pins(pipette_type, GPIOA);
         HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
+        // Diag0 pin
+        GPIO_InitStruct.Pin = pipette_hardware_motor_driver_diag0_pin(pipette_type);
+        GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING_FALLING;
+        HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
     } else {
         // Enable Dir/Step pin
         GPIO_InitStruct.Pin = pipette_hardware_motor_driver_pins(pipette_type, GPIOA);
@@ -101,7 +105,10 @@ void motor_driver_gpio_init() {
         // Enable/Dir/Step pin
         GPIO_InitStruct.Pin = pipette_hardware_motor_driver_pins(pipette_type, GPIOB);
         HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
-
+        // Diag0 pin
+        GPIO_InitStruct.Pin = pipette_hardware_motor_driver_diag0_pin(pipette_type);
+        GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING_FALLING;
+        HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
     }
 
 }
diff --git a/pipettes/firmware/stm32g4xx_it.c b/pipettes/firmware/stm32g4xx_it.c
index 953d30404..de067fdd2 100644
--- a/pipettes/firmware/stm32g4xx_it.c
+++ b/pipettes/firmware/stm32g4xx_it.c
@@ -125,6 +125,8 @@ void EXTI2_IRQHandler(void) {
 void EXTI9_5_IRQHandler(void) {
     if (__HAL_GPIO_EXTI_GET_IT(GPIO_PIN_7)) {
         HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_7); 
+    } else if (__HAL_GPIO_EXTI_GET_IT(GPIO_PIN_6)) {
+        HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_6);
     }
 }
 
@@ -132,6 +134,8 @@ void EXTI9_5_IRQHandler(void) {
 void EXTI15_10_IRQHandler(void) {
     if (__HAL_GPIO_EXTI_GET_IT(GPIO_PIN_12)) {
         HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_12);
+    } else if (__HAL_GPIO_EXTI_GET_IT(GPIO_PIN_11)) {
+        HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_11);
     }
 }
 
diff --git a/pipettes/firmware/utility_gpio.c b/pipettes/firmware/utility_gpio.c
index e5c76a8d6..3d0f40bb0 100644
--- a/pipettes/firmware/utility_gpio.c
+++ b/pipettes/firmware/utility_gpio.c
@@ -59,6 +59,16 @@ static void nvic_priority_enable_init() {
         /* EXTI interrupt init block tip sense*/
         HAL_NVIC_SetPriority(block_2, 10, 0);
         HAL_NVIC_EnableIRQ(block_2);
+
+        IRQn_Type block_15_10 = get_interrupt_line(gpio_block_15_10);
+        /* EXTI interrupt init block diag0*/
+        HAL_NVIC_SetPriority(block_15_10, 5, 0);
+        HAL_NVIC_EnableIRQ(block_15_10);
+    } else {
+        IRQn_Type block_9_5 = get_interrupt_line(gpio_block_9_5);
+        /* EXTI interrupt init block diag0*/
+        HAL_NVIC_SetPriority(block_9_5, 5, 0);
+        HAL_NVIC_EnableIRQ(block_9_5);
     }
 }
 
diff --git a/pipettes/simulator/interfaces.cpp b/pipettes/simulator/interfaces.cpp
index 8d0301c85..22251dc60 100644
--- a/pipettes/simulator/interfaces.cpp
+++ b/pipettes/simulator/interfaces.cpp
@@ -47,32 +47,43 @@ auto interfaces::get_interrupt_queues<PipetteType::THREE_EIGHTY_FOUR_CHANNEL>()
 auto linear_motor::get_interrupt(
     sim_motor_hardware_iface::SimMotorHardwareIface& hw,
     LowThroughputInterruptQueues& queues, stall_check::StallCheck& stall)
-    -> MotorInterruptHandlerType<linear_motor_tasks::QueueClient> {
+    -> MotorInterruptHandlerType<
+        linear_motor_tasks::QueueClient,
+        linear_motor_tasks::tmc2130_driver::QueueClient> {
     return motor_handler::MotorInterruptHandler(
-        queues.plunger_queue, linear_motor_tasks::get_queues(), hw, stall,
+        queues.plunger_queue, linear_motor_tasks::get_queues(),
+        linear_motor_tasks::tmc2130_driver::get_queues(), hw, stall,
         queues.plunger_update_queue);
 }
 
 auto linear_motor::get_interrupt(
     sim_motor_hardware_iface::SimMotorHardwareIface& hw,
     HighThroughputInterruptQueues& queues, stall_check::StallCheck& stall)
-    -> MotorInterruptHandlerType<linear_motor_tasks::QueueClient> {
+    -> MotorInterruptHandlerType<
+        linear_motor_tasks::QueueClient,
+        linear_motor_tasks::tmc2160_driver::QueueClient> {
     return motor_handler::MotorInterruptHandler(
-        queues.plunger_queue, linear_motor_tasks::get_queues(), hw, stall,
+        queues.plunger_queue, linear_motor_tasks::get_queues(),
+        linear_motor_tasks::tmc2160_driver::get_queues(), hw, stall,
         queues.plunger_update_queue);
 }
 
 auto linear_motor::get_interrupt_driver(
     sim_motor_hardware_iface::SimMotorHardwareIface& hw,
     LowThroughputInterruptQueues& queues,
-    MotorInterruptHandlerType<linear_motor_tasks::QueueClient>& handler)
+    MotorInterruptHandlerType<linear_motor_tasks::QueueClient,
+                              linear_motor_tasks::tmc2130_driver::QueueClient>&
+        handler) ->
 #ifdef USE_SENSOR_MOVE
-    -> motor_interrupt_driver::MotorInterruptDriver<
-        linear_motor_tasks::QueueClient, motor_messages::SensorSyncMove,
+    motor_interrupt_driver::MotorInterruptDriver<
+        linear_motor_tasks::QueueClient,
+        linear_motor_tasks::tmc2130_driver::QueueClient,
+        motor_messages::SensorSyncMove,
         sim_motor_hardware_iface::SimMotorHardwareIface> {
 #else
-    -> motor_interrupt_driver::MotorInterruptDriver<
-        linear_motor_tasks::QueueClient, motor_messages::Move,
+    motor_interrupt_driver::MotorInterruptDriver<
+        linear_motor_tasks::QueueClient,
+        linear_motor_tasks::tmc2130_driver::QueueClient, motor_messages::Move,
         sim_motor_hardware_iface::SimMotorHardwareIface> {
 #endif
     return motor_interrupt_driver::MotorInterruptDriver(
@@ -82,14 +93,19 @@ auto linear_motor::get_interrupt_driver(
 auto linear_motor::get_interrupt_driver(
     sim_motor_hardware_iface::SimMotorHardwareIface& hw,
     HighThroughputInterruptQueues& queues,
-    MotorInterruptHandlerType<linear_motor_tasks::QueueClient>& handler)
+    MotorInterruptHandlerType<linear_motor_tasks::QueueClient,
+                              linear_motor_tasks::tmc2160_driver::QueueClient>&
+        handler) ->
 #ifdef USE_SENSOR_MOVE
-    -> motor_interrupt_driver::MotorInterruptDriver<
-        linear_motor_tasks::QueueClient, motor_messages::SensorSyncMove,
+    motor_interrupt_driver::MotorInterruptDriver<
+        linear_motor_tasks::QueueClient,
+        linear_motor_tasks::tmc2160_driver::QueueClient,
+        motor_messages::SensorSyncMove,
         sim_motor_hardware_iface::SimMotorHardwareIface> {
 #else
-    -> motor_interrupt_driver::MotorInterruptDriver<
-        linear_motor_tasks::QueueClient, motor_messages::Move,
+    motor_interrupt_driver::MotorInterruptDriver<
+        linear_motor_tasks::QueueClient,
+        linear_motor_tasks::tmc2160_driver::QueueClient, motor_messages::Move,
         sim_motor_hardware_iface::SimMotorHardwareIface> {
 #endif
     return motor_interrupt_driver::MotorInterruptDriver(
@@ -136,10 +152,12 @@ auto gear_motor::get_interrupts(gear_motor::GearHardware& hw,
     return gear_motor::GearInterruptHandlers{
         .left = motor_handler::MotorInterruptHandler(
             queues.left_motor_queue, gear_motor_tasks::get_left_gear_queues(),
-            hw.left, stall.left, queues.left_update_queue),
+            gear_motor_tasks::get_left_gear_queues(), hw.left, stall.left,
+            queues.left_update_queue),
         .right = motor_handler::MotorInterruptHandler(
             queues.right_motor_queue, gear_motor_tasks::get_right_gear_queues(),
-            hw.right, stall.right, queues.right_update_queue),
+            gear_motor_tasks::get_right_gear_queues(), hw.right, stall.right,
+            queues.right_update_queue),
     };
 }
 
diff --git a/sensors/tests/CMakeLists.txt b/sensors/tests/CMakeLists.txt
index 55f9d034b..982b40967 100644
--- a/sensors/tests/CMakeLists.txt
+++ b/sensors/tests/CMakeLists.txt
@@ -36,7 +36,7 @@ target_compile_options(sensors
         $<$<COMPILE_LANGUAGE:CXX>:-fno-rtti>
 )
 
-target_link_libraries(sensors PUBLIC Catch2::Catch2 common-core motor-utils common-simulation)
+target_link_libraries(sensors PUBLIC Catch2::Catch2 Boost::boost Boost::program_options Boost::date_time common-core motor-utils common-simulation)
 
 target_i2c_simlib(sensors)