diff --git a/gantry/firmware/interfaces_rev1.cpp b/gantry/firmware/interfaces_rev1.cpp
index 5a4137b12..687ca2d75 100644
--- a/gantry/firmware/interfaces_rev1.cpp
+++ b/gantry/firmware/interfaces_rev1.cpp
@@ -145,7 +145,9 @@ struct motion_controller::HardwareConfig motor_pins_y {
 };
 
 static tmc2160::configs::TMC2160DriverConfig motor_driver_config_x{
-    .registers = {.gconfig = {.en_pwm_mode = 0, .diag0_error = 1},
+    .registers = {.gconfig = {.en_pwm_mode = 0,
+                              .diag0_error = 1,
+                              .diag0_otpw = 1},
                   .ihold_irun = {.hold_current = 16,
                                  .run_current = 31,
                                  .hold_current_delay = 0x7},
@@ -171,7 +173,7 @@ static tmc2160::configs::TMC2160DriverConfig motor_driver_config_x{
                               .freewheel = 0,
                               .pwm_reg = 0x7,
                               .pwm_lim = 0xC},
-                  .drvconf = {.ot_select = 0x0},
+                  .drvconf = {.ot_select = 0},
                   .glob_scale = {.global_scaler = 0xA7}},
     .current_config =
         {
@@ -185,7 +187,9 @@ static tmc2160::configs::TMC2160DriverConfig motor_driver_config_x{
     }};
 
 static tmc2160::configs::TMC2160DriverConfig motor_driver_config_y{
-    .registers = {.gconfig = {.en_pwm_mode = 0, .diag0_error = 1},
+    .registers = {.gconfig = {.en_pwm_mode = 0,
+                              .diag0_error = 1,
+                              .diag0_otpw = 1},
                   .ihold_irun = {.hold_current = 16,
                                  .run_current = 31,
                                  .hold_current_delay = 0x7},
@@ -211,7 +215,7 @@ static tmc2160::configs::TMC2160DriverConfig motor_driver_config_y{
                               .freewheel = 0,
                               .pwm_reg = 0x7,
                               .pwm_lim = 0xC},
-                  .drvconf = {.ot_select = 0x0},
+                  .drvconf = {.ot_select = 0},
                   .glob_scale = {.global_scaler = 0xA7}},
     .current_config =
         {
diff --git a/gripper/core/tasks.cpp b/gripper/core/tasks.cpp
index cd16770c6..b58b68b44 100644
--- a/gripper/core/tasks.cpp
+++ b/gripper/core/tasks.cpp
@@ -58,7 +58,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,
@@ -68,7 +68,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;
@@ -122,7 +123,7 @@ 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,
+    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);
@@ -132,6 +133,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 7fb826448..bdeb75d3b 100644
--- a/gripper/core/tasks_z.cpp
+++ b/gripper/core/tasks_z.cpp
@@ -34,13 +34,13 @@ 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) {
+        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);
@@ -78,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()
diff --git a/gripper/firmware/interfaces_z_motor.cpp b/gripper/firmware/interfaces_z_motor.cpp
index 2d2416ec9..36d25611d 100644
--- a/gripper/firmware/interfaces_z_motor.cpp
+++ b/gripper/firmware/interfaces_z_motor.cpp
@@ -103,7 +103,12 @@ 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 +124,8 @@ static motor_hardware::MotorHardware motor_hardware_iface(motor_pins, &htim7,
  */
 static tmc2130::configs::TMC2130DriverConfig MotorDriverConfigurations{
     .registers = {.gconfig = {.en_pwm_mode = 0x0,
+                              .diag0_error = 1,
+                              .diag0_otpw = 1,
                               .stop_enable = use_stop_enable},
                   .ihold_irun = {.hold_current = 0x2,  // 0.177A
                                  .run_current = 0xA,   // 0.648A
@@ -205,12 +212,12 @@ 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 b34d83acf..5ff96f001 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{};
 
 /**
@@ -95,7 +97,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);
@@ -103,7 +105,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 62572ef6a..f07597938 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{};
 
 /**
@@ -106,7 +108,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);
@@ -114,7 +116,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 5da78f26c..6bb83221e 100644
--- a/gripper/firmware/motor_hardware_shared.c
+++ b/gripper/firmware/motor_hardware_shared.c
@@ -4,6 +4,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 =
@@ -11,11 +12,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(
@@ -213,4 +222,13 @@ void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef* htim) {
     }
 }
 
+void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
+    if (GPIO_Pin == GPIO_PIN_2) {
+        if (diag0_callback != NULL) {
+            if (*diag0_callback != NULL) {
+                (*diag0_callback)();
+            }
+        }
+    }
+}
 
diff --git a/gripper/firmware/stm32g4xx_it.c b/gripper/firmware/stm32g4xx_it.c
index 7efd9fff0..df86ca705 100644
--- a/gripper/firmware/stm32g4xx_it.c
+++ b/gripper/firmware/stm32g4xx_it.c
@@ -198,6 +198,10 @@ void TIM7_IRQHandler(void) {
     call_motor_handler();
 }
 
+void EXTI2_IRQHandler(void) {
+    HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_2);
+}
+
 extern void xPortSysTickHandler(void);
 void SysTick_Handler(void) {
     HAL_IncTick();
diff --git a/gripper/firmware/utility_gpio.c b/gripper/firmware/utility_gpio.c
index b206651a2..3eab7b052 100644
--- a/gripper/firmware/utility_gpio.c
+++ b/gripper/firmware/utility_gpio.c
@@ -105,6 +105,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;
@@ -116,6 +118,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/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 74d58a7ea..da61ddeca 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.
@@ -171,13 +173,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();
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/motor-control/core/stepper_motor/motion_controller.hpp b/include/motor-control/core/stepper_motor/motion_controller.hpp
index 9cdb71a73..3b9e73180 100644
--- a/include/motor-control/core/stepper_motor/motion_controller.hpp
+++ b/include/motor-control/core/stepper_motor/motion_controller.hpp
@@ -282,6 +282,12 @@ class PipetteMotionController {
         }
     }
 
+    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(); }