diff --git a/head/core/tasks_proto.cpp b/head/core/tasks_proto.cpp
index 51ec208e5..9567d6011 100644
--- a/head/core/tasks_proto.cpp
+++ b/head/core/tasks_proto.cpp
@@ -78,10 +78,23 @@ 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 +108,7 @@ 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);
@@ -201,6 +214,8 @@ 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
diff --git a/head/core/tasks_rev1.cpp b/head/core/tasks_rev1.cpp
index dedb0aca3..3b618f4e7 100644
--- a/head/core/tasks_rev1.cpp
+++ b/head/core/tasks_rev1.cpp
@@ -83,10 +83,23 @@ 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 +113,7 @@ 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);
@@ -222,6 +235,8 @@ 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
diff --git a/head/firmware/main_proto.cpp b/head/firmware/main_proto.cpp
index c63842b32..5d4a44cfa 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)
@@ -146,7 +150,13 @@ struct motor_hardware::HardwareConfig pin_configurations_left {
         // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
         .port = GPIOB,
         .pin = GPIO_PIN_4,
-        .active_setting = GPIO_PIN_RESET}
+        .active_setting = GPIO_PIN_RESET},
+    .diag0 =
+        {
+            // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
+            .port = GPIOC,
+            .pin = GPIO_PIN_13,
+            .active_setting = GPIO_PIN_RESET}
 };
 
 struct motor_hardware::HardwareConfig pin_configurations_right {
@@ -190,14 +200,22 @@ struct motor_hardware::HardwareConfig pin_configurations_right {
         // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
         .port = GPIOB,
         .pin = GPIO_PIN_4,
-        .active_setting = GPIO_PIN_RESET}
+        .active_setting = GPIO_PIN_RESET},
+    .diag0 =
+        {
+            // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
+            .port = GPIOC,
+            .pin = GPIO_PIN_15,
+            .active_setting = GPIO_PIN_RESET}
 };
 
 // TODO clean up the head main file by using interfaces.
 static tmc2130::configs::TMC2130DriverConfig motor_driver_configs_right{
     .registers =
         {
-            .gconfig = {.en_pwm_mode = 1},
+            .gconfig = {.en_pwm_mode = 1,
+                        .diag0_error = 1,
+                        .diag0_otpw = 1},
             .ihold_irun = {.hold_current = 0xB,
                            .run_current = 0x19,
                            .hold_current_delay = 0x7},
@@ -224,7 +242,9 @@ 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,
+                        .diag0_otpw = 1},
             .ihold_irun = {.hold_current = 0xB,
                            .run_current = 0x19,
                            .hold_current_delay = 0x7},
@@ -395,7 +415,7 @@ 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();
@@ -409,8 +429,7 @@ 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,
+    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,
diff --git a/head/firmware/main_rev1.cpp b/head/firmware/main_rev1.cpp
index 008560e77..dfa4317e6 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,9 @@ 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,
+                              .diag0_otpw = 1},
                   .ihold_irun = {.hold_current = 16,
                                  .run_current = 31,
                                  .hold_current_delay = 0x7},
@@ -232,7 +250,9 @@ 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,
+                              .diag0_otpw = 1},
                   .ihold_irun = {.hold_current = 16,
                                  .run_current = 31,
                                  .hold_current_delay = 0x7},
@@ -420,7 +440,7 @@ 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);
@@ -434,7 +454,7 @@ auto main() -> int {
 
     utility_gpio_init();
     can_bus_1.start(can_bit_timings);
-    head_tasks::start_tasks(can_bus_1, motor_left.motion_controller,
+    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,
diff --git a/head/firmware/motor_hardware.h b/head/firmware/motor_hardware.h
index 85981ae3d..b3a760ee0 100644
--- a/head/firmware/motor_hardware.h
+++ b/head/firmware/motor_hardware.h
@@ -14,10 +14,13 @@ 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 00b141da5..8c9a22ff2 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'
@@ -334,12 +355,32 @@ void HAL_TIM_Encoder_MspInit(TIM_HandleTypeDef *htim) {
     }
 }
 
+void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
+    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 57ba8aef7..6cc5709bc 100644
--- a/head/firmware/stm32g4xx_it.c
+++ b/head/firmware/stm32g4xx_it.c
@@ -168,6 +168,17 @@ void TIM2_IRQHandler(void) { HAL_TIM_IRQHandler(&htim2); }
  */
 void TIM3_IRQHandler(void) { HAL_TIM_IRQHandler(&htim3); }
 
+void EXTI15_10_IRQHandler(void) {
+    // make this get/deduce pin! Needs to work for C13/z and C15/a!
+    EXTI_HandleTypeDef hexti_13 = {.Line = EXTI_LINE_13};
+    EXTI_HandleTypeDef hexti_15 = {.Line = EXTI_LINE_15};
+    if (HAL_EXTI_GetPending(&hexti_13, 0)) {
+        HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_13);
+    } else if (HAL_EXTI_GetPending(&hexti_15, 0)) {
+        HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_15);
+    }
+}
+
 extern void xPortSysTickHandler(void);
 void SysTick_Handler(void) {
     HAL_IncTick();
diff --git a/include/head/core/tasks_proto.hpp b/include/head/core/tasks_proto.hpp
index 6f87e5286..decacd3ab 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,7 @@ 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..9f6d0f84e 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,7 @@ 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.