From e30658d432c89a39bdb57abef297fd1055ab738b Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Tue, 29 Aug 2023 17:36:05 -0400
Subject: [PATCH 01/68] Adds interrupt for motor driver. Successfully built
 gantry firmware

---
 gantry/core/tasks_proto.cpp                   | 14 ++++++--
 gantry/core/tasks_rev1.cpp                    | 19 ++++++++---
 gantry/firmware/interfaces_proto.cpp          | 24 ++++++++++----
 gantry/firmware/interfaces_rev1.cpp           | 24 ++++++++++----
 gantry/firmware/main_proto.cpp                |  6 ++--
 gantry/firmware/main_rev1.cpp                 |  6 ++--
 gantry/firmware/motor_hardware.c              | 27 +++++++++++++--
 gantry/firmware/motor_hardware.h              |  3 +-
 gantry/firmware/stm32g4xx_it.c                |  1 +
 include/can/core/ids.hpp                      |  3 ++
 include/can/core/message_handlers/motion.hpp  |  2 +-
 include/can/core/messages.hpp                 | 18 ++++++++++
 include/gantry/core/can_task.hpp              |  6 ++--
 include/gantry/core/interfaces_proto.hpp      |  4 ++-
 include/gantry/core/interfaces_rev1.hpp       |  4 ++-
 include/gantry/core/tasks_proto.hpp           |  7 ++--
 include/gantry/core/tasks_rev1.hpp            |  7 ++--
 include/motor-control/core/tasks/messages.hpp |  6 ++--
 .../core/tasks/motion_controller_task.hpp     | 33 +++++++++++++++----
 .../core/tasks/tmc2130_motor_driver_task.hpp  |  7 ++++
 .../core/tasks/tmc2160_motor_driver_task.hpp  | 19 +++++++++++
 .../core/tasks/tmc_motor_driver_common.hpp    |  3 +-
 .../firmware/stepper_motor/motor_hardware.hpp |  1 +
 include/spi/core/writer.hpp                   |  4 +--
 24 files changed, 202 insertions(+), 46 deletions(-)

diff --git a/gantry/core/tasks_proto.cpp b/gantry/core/tasks_proto.cpp
index de020ffc3..c059698aa 100644
--- a/gantry/core/tasks_proto.cpp
+++ b/gantry/core/tasks_proto.cpp
@@ -55,18 +55,18 @@ 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);
+                                         motion_controller, ::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 +116,14 @@ 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)
diff --git a/gantry/core/tasks_rev1.cpp b/gantry/core/tasks_rev1.cpp
index 95c81ba29..da7c69db8 100644
--- a/gantry/core/tasks_rev1.cpp
+++ b/gantry/core/tasks_rev1.cpp
@@ -17,6 +17,8 @@ static auto queues = gantry::queues::QueueClient{utils::get_node_id()};
 
 static auto spi_task_client =
     spi::writer::Writer<freertos_message_queue::FreeRTOSMessageQueue>();
+//check how spi_task_client is used here
+//we want an interrupt from spi, see past work
 
 static auto mc_task_builder =
     freertos_task::TaskStarter<512,
@@ -30,6 +32,7 @@ static auto move_status_task_builder = freertos_task::TaskStarter<
 
 static auto spi_task_builder =
     freertos_task::TaskStarter<512, spi::tasks::Task>{};
+//look at this class's setup, functionality
 
 template <template <typename> typename QueueImpl>
 using PollerWithTimer =
@@ -54,18 +57,18 @@ 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);
+                                         motion_controller, ::queues, ::queues, ::queues);
     auto& tmc2160_driver = motor_driver_task_builder.start(
         5, "tmc2160 driver", driver_configs, ::queues, spi_task_client);
     auto& move_group =
@@ -75,7 +78,7 @@ void gantry::tasks::start_tasks(
         ::queues);
 
     auto& spi_task = spi_task_builder.start(5, "spi task", spi_device);
-    spi_task_client.set_queue(&spi_task.get_queue());
+    spi_task_client.set_queue(&spi_task.get_queue()); // Writer class queue set here!
 
     auto& i2c2_task = i2c2_task_builder.start(5, "i2c2", i2c2);
     i2c2_task_client.set_queue(&i2c2_task.get_queue());
@@ -115,6 +118,14 @@ 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)
diff --git a/gantry/firmware/interfaces_proto.cpp b/gantry/firmware/interfaces_proto.cpp
index 584fd990e..415c44203 100644
--- a/gantry/firmware/interfaces_proto.cpp
+++ b/gantry/firmware/interfaces_proto.cpp
@@ -90,7 +90,12 @@ struct motion_controller::HardwareConfig motor_pins_x {
         // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
         .port = GPIOA,
         .pin = GPIO_PIN_10,
-        .active_setting = GPIO_PIN_RESET}
+        .active_setting = GPIO_PIN_RESET},
+    .diag0 = {
+        // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
+        .port = GPIOC,
+        .pin = GPIO_PIN_5,
+        .active_setting = GPIO_PIN_RESET} //confirm RESET not SET. See Driver RM
 };
 
 struct motion_controller::HardwareConfig motor_pins_y {
@@ -129,11 +134,17 @@ struct motion_controller::HardwareConfig motor_pins_y {
         // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
         .port = GPIOA,
         .pin = GPIO_PIN_10,
-        .active_setting = GPIO_PIN_RESET}
+        .active_setting = GPIO_PIN_RESET},
+    .diag0 = {
+        // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
+        .port = GPIOC,
+        .pin = GPIO_PIN_5,
+        .active_setting = GPIO_PIN_RESET} //confirm RESET not SET. See Driver RM
 };
 
 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 +167,8 @@ 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},
@@ -269,13 +281,13 @@ 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..692ebd482 100644
--- a/gantry/firmware/interfaces_rev1.cpp
+++ b/gantry/firmware/interfaces_rev1.cpp
@@ -90,7 +90,12 @@ struct motion_controller::HardwareConfig motor_pins_x {
         // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
         .port = GPIOA,
         .pin = GPIO_PIN_10,
-        .active_setting = GPIO_PIN_RESET}
+        .active_setting = GPIO_PIN_RESET},
+    .diag0 = {
+        // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
+        .port = GPIOC,
+        .pin = GPIO_PIN_5,
+        .active_setting = GPIO_PIN_RESET} //confirm RESET not SET. See Driver RM
 };
 
 struct motion_controller::HardwareConfig motor_pins_y {
@@ -129,11 +134,17 @@ struct motion_controller::HardwareConfig motor_pins_y {
         // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
         .port = GPIOA,
         .pin = GPIO_PIN_10,
-        .active_setting = GPIO_PIN_RESET}
+        .active_setting = GPIO_PIN_RESET},
+    .diag0 = {
+        // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
+        .port = GPIOC,
+        .pin = GPIO_PIN_5,
+        .active_setting = GPIO_PIN_RESET} //confirm RESET not SET. See Driver RM
 };
 
 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},
@@ -172,7 +183,8 @@ 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},
@@ -295,13 +307,13 @@ 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..7a57f3bab 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 = NULL;
+
 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..e2f33185a 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 = NULL;
+
 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..0f9c07427 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;
+        GPIO_InitStruct.Pull = GPIO_PULLUP; // confirm correct
+        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..1a608a8ec 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); } // source this from somewhere?
 
 
 extern void xPortSysTickHandler(void);
diff --git a/include/can/core/ids.hpp b/include/can/core/ids.hpp
index 23857604e..0773a0036 100644
--- a/include/can/core/ids.hpp
+++ b/include/can/core/ids.hpp
@@ -61,6 +61,8 @@ enum class MessageId {
     write_motor_current_request = 0x33,
     read_motor_current_request = 0x34,
     read_motor_current_response = 0x35,
+    motor_driver_error_encountered = 0x36,
+    read_motor_driver_error_status = 0x37,
     set_brushed_motor_vref_request = 0x40,
     set_brushed_motor_pwm_request = 0x41,
     gripper_grip_request = 0x42,
@@ -148,6 +150,7 @@ enum class ErrorCode {
     motor_busy = 0xb,
     stop_requested = 0xc,
     over_pressure = 0xd,
+    // motor_driver_error = 0xe,
 };
 
 /** Error Severity levels. */
diff --git a/include/can/core/message_handlers/motion.hpp b/include/can/core/message_handlers/motion.hpp
index d4d367999..e59d5fd95 100644
--- a/include/can/core/message_handlers/motion.hpp
+++ b/include/can/core/message_handlers/motion.hpp
@@ -18,7 +18,7 @@ class MotionHandler {
                      GetMotionConstraintsRequest, SetMotionConstraints,
                      ReadLimitSwitchRequest, MotorPositionRequest,
                      UpdateMotorPositionEstimationRequest,
-                     GetMotorUsageRequest>;
+                     GetMotorUsageRequest, MotorDriverErrorEncountered>;
 
     MotionHandler(MotionTaskClient &motion_client)
         : motion_client{motion_client} {}
diff --git a/include/can/core/messages.hpp b/include/can/core/messages.hpp
index f3583de20..51ee84450 100644
--- a/include/can/core/messages.hpp
+++ b/include/can/core/messages.hpp
@@ -198,6 +198,8 @@ using MotorPositionRequest = Empty<MessageId::motor_position_request>;
 using UpdateMotorPositionEstimationRequest =
     Empty<MessageId::update_motor_position_estimation_request>;
 
+using MotorDriverErrorEncountered = Empty<MessageId::motor_driver_error_encountered>;
+
 struct WriteToEEPromRequest : BaseMessage<MessageId::write_eeprom> {
     uint32_t message_index;
     eeprom::types::address address;
@@ -598,6 +600,22 @@ struct ReadMotorDriverRegister
         -> bool = default;
 };
 
+struct ReadMotorDriverErrorStatus
+    : BaseMessage<MessageId::read_motor_driver_error_status> {
+    uint32_t message_index;
+
+    template <bit_utils::ByteIterator Input, typename Limit>
+    static auto parse(Input body, Limit limit) -> ReadMotorDriverErrorStatus {
+        uint32_t msg_ind = 0;
+
+        body = bit_utils::bytes_to_int(body, limit, msg_ind);
+        return ReadMotorDriverErrorStatus{.message_index = msg_ind};
+    }
+
+    auto operator==(const ReadMotorDriverErrorStatus& other) const
+        -> bool = default;
+};
+
 struct ReadMotorDriverRegisterResponse
     : BaseMessage<MessageId::read_motor_driver_register_response> {
     uint32_t message_index;
diff --git a/include/gantry/core/can_task.hpp b/include/gantry/core/can_task.hpp
index 8f72fc7ad..4117d2ab7 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::ReadMotorDriverErrorStatus>;
 using MoveGroupDispatchTarget = can::dispatch::DispatchParseTarget<
     can::message_handlers::move_group::MoveGroupHandler<
         gantry::queues::QueueClient>,
@@ -36,7 +37,8 @@ using MotionControllerDispatchTarget = can::dispatch::DispatchParseTarget<
     can::messages::SetMotionConstraints, can::messages::ReadLimitSwitchRequest,
     can::messages::MotorPositionRequest,
     can::messages::UpdateMotorPositionEstimationRequest,
-    can::messages::GetMotorUsageRequest>;
+    can::messages::GetMotorUsageRequest,
+    can::messages::MotorDriverErrorEncountered>;
 using SystemDispatchTarget = can::dispatch::DispatchParseTarget<
     can::message_handlers::system::SystemMessageHandler<
         gantry::queues::QueueClient>,
diff --git a/include/gantry/core/interfaces_proto.hpp b/include/gantry/core/interfaces_proto.hpp
index d58ea63fd..d5da7f710 100644
--- a/include/gantry/core/interfaces_proto.hpp
+++ b/include/gantry/core/interfaces_proto.hpp
@@ -9,10 +9,12 @@
 
 namespace interfaces {
 
+extern "C" typedef void (*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..db4aba4af 100644
--- a/include/gantry/core/interfaces_rev1.hpp
+++ b/include/gantry/core/interfaces_rev1.hpp
@@ -9,10 +9,12 @@
 
 namespace interfaces {
 
+extern "C" typedef void (*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/tasks_proto.hpp b/include/gantry/core/tasks_proto.hpp
index c9fe5be64..0e439b6b3 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,16 @@ 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..567e2f181 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,16 @@ 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/motor-control/core/tasks/messages.hpp b/include/motor-control/core/tasks/messages.hpp
index 653e7a547..9152045a5 100644
--- a/include/motor-control/core/tasks/messages.hpp
+++ b/include/motor-control/core/tasks/messages.hpp
@@ -14,12 +14,14 @@ using MotionControlTaskMessage = std::variant<
     can::messages::MotorPositionRequest, can::messages::ReadLimitSwitchRequest,
     can::messages::HomeRequest,
     can::messages::UpdateMotorPositionEstimationRequest,
-    can::messages::GetMotorUsageRequest>;
+    can::messages::GetMotorUsageRequest,
+    can::messages::MotorDriverErrorEncountered>;
 
 using MotorDriverTaskMessage =
     std::variant<std::monostate, can::messages::ReadMotorDriverRegister,
                  can::messages::WriteMotorDriverRegister,
-                 can::messages::WriteMotorCurrentRequest>;
+                 can::messages::WriteMotorCurrentRequest,
+                 can::messages::ReadMotorDriverErrorStatus>;
 
 using MoveGroupTaskMessage =
     std::variant<std::monostate, can::messages::AddLinearMoveRequest,
diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp
index a157ca8e0..513c4691a 100644
--- a/include/motor-control/core/tasks/motion_controller_task.hpp
+++ b/include/motor-control/core/tasks/motion_controller_task.hpp
@@ -8,6 +8,7 @@
 #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 {
 
@@ -18,16 +19,19 @@ using TaskMessage = motor_control_task_messages::MotionControlTaskMessage;
  */
 template <lms::MotorMechanicalConfig MEConfig,
           can::message_writer_task::TaskClient CanClient,
-          usage_storage_task::TaskClient UsageClient>
+          usage_storage_task::TaskClient UsageClient,
+          tmc::tasks::TaskClient DriverClient>
 class MotionControllerMessageHandler {
   public:
     using MotorControllerType = motion_controller::MotionController<MEConfig>;
     MotionControllerMessageHandler(MotorControllerType& controller,
                                    CanClient& can_client,
-                                   UsageClient& usage_client)
+                                   UsageClient& usage_client,
+                                   DriverClient& driver_client)
         : controller{controller},
           can_client{can_client},
-          usage_client{usage_client} {}
+          usage_client{usage_client},
+          driver_client{driver_client} {}
     MotionControllerMessageHandler(const MotionControllerMessageHandler& c) =
         delete;
     MotionControllerMessageHandler(const MotionControllerMessageHandler&& c) =
@@ -140,9 +144,20 @@ class MotionControllerMessageHandler {
         controller.send_usage_data(m.message_index, usage_client);
     }
 
+    // set error flag here (C++ atomic)?! Disable subsystem (enable brakes)?!
+    // cancel_and_clear_moves(can::ids::ErrorCode::motor_driver_error)?
+    void handle(const can::messages::MotorDriverErrorEncountered& m) {
+        controller.stop(); // also request_cancel()?
+        can::messages::ReadMotorDriverErrorStatus msg{
+            .message_index = m.message_index // need?
+        };
+        driver_client.send_motor_driver_queue(msg);
+    }
+
     MotorControllerType& controller;
     CanClient& can_client;
     UsageClient& usage_client;
+    DriverClient& driver_client;
 };
 
 /**
@@ -166,12 +181,13 @@ 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>
     [[noreturn]] void operator()(
         motion_controller::MotionController<MEConfig>* controller,
-        CanClient* can_client, UsageClient* usage_client) {
+        CanClient* can_client, UsageClient* usage_client, DriverClient* driver_client) {
         auto handler = MotionControllerMessageHandler{*controller, *can_client,
-                                                      *usage_client};
+                                                      *usage_client, *driver_client};
         TaskMessage message{};
         bool first_run = true;
         for (;;) {
@@ -187,6 +203,11 @@ class MotionControllerTask {
 
     [[nodiscard]] auto get_queue() const -> QueueType& { return queue; }
 
+    void run_diag0_interrupt(void) {
+        can::messages::MotorDriverErrorEncountered msg{};
+        queue.try_write(msg);
+    }
+
   private:
     QueueType& queue;
 };
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..1df5e73f6 100644
--- a/include/motor-control/core/tasks/tmc2130_motor_driver_task.hpp
+++ b/include/motor-control/core/tasks/tmc2130_motor_driver_task.hpp
@@ -86,6 +86,13 @@ class MotorDriverMessageHandler {
         }
     }
 
+    void handle(const can::messages::ReadMotorDriverErrorStatus& m) {
+        // LOG?
+        uint32_t data = 0;
+        driver.read(tmc2130::registers::Registers::DRVSTATUS, data,
+                    m.message_index);
+    }
+
     void handle(const can::messages::WriteMotorCurrentRequest& m) {
         LOG("Received write motor current request: hold_current=%d, "
             "run_current=%d",
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..c56dfbffe 100644
--- a/include/motor-control/core/tasks/tmc2160_motor_driver_task.hpp
+++ b/include/motor-control/core/tasks/tmc2160_motor_driver_task.hpp
@@ -85,6 +85,16 @@ class MotorDriverMessageHandler {
         }
     }
 
+    // need a response handler? Eh, should get handled by TransactResponse
+    // ensure specific error is returned
+    // just use above method?!
+    void handle(const can::messages::ReadMotorDriverErrorStatus& m) {
+        // LOG?
+        uint32_t data = 0;
+        driver.read(tmc2160::registers::Registers::DRVSTATUS, data,
+                    m.message_index);
+    }
+
     void handle(const can::messages::WriteMotorCurrentRequest& m) {
         LOG("Received write motor current request: hold_current=%d, "
             "run_current=%d",
@@ -146,6 +156,15 @@ 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)};
+};
+
 }  // 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..35f1024f8 100644
--- a/include/motor-control/core/tasks/tmc_motor_driver_common.hpp
+++ b/include/motor-control/core/tasks/tmc_motor_driver_common.hpp
@@ -10,7 +10,8 @@ namespace tasks {
 using SpiResponseMessage = std::tuple<spi::messages::TransactResponse>;
 using CanMessageTuple = std::tuple<can::messages::ReadMotorDriverRegister,
                                    can::messages::WriteMotorDriverRegister,
-                                   can::messages::WriteMotorCurrentRequest>;
+                                   can::messages::WriteMotorCurrentRequest,
+                                   can::messages::ReadMotorDriverErrorStatus>;
 using GearCanMessageTuple =
     std::tuple<can::messages::GearReadMotorDriverRegister,
                can::messages::GearWriteMotorDriverRegister,
diff --git a/include/motor-control/firmware/stepper_motor/motor_hardware.hpp b/include/motor-control/firmware/stepper_motor/motor_hardware.hpp
index e750a4f02..249764a04 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;
 };
 
diff --git a/include/spi/core/writer.hpp b/include/spi/core/writer.hpp
index a3e2e1710..a0cb9df82 100644
--- a/include/spi/core/writer.hpp
+++ b/include/spi/core/writer.hpp
@@ -60,10 +60,10 @@ class Writer {
             .id = _transaction_id,
             .transaction = {.txBuffer = txBuffer, .cs_interface = cs_intf},
             .response_writer = ResponseWriter(response_queue)};
-        queue->try_write(message);
+        queue->try_write(message); // this is a FreeRTOSMessageQueue
 
         message.id.requires_response = true;
-        return queue->try_write(message);
+        return queue->try_write(message); // why twice? transmit and receive
     }
 
     /**

From 885837d1a5663c7c8e7dac500f76b36f48a9995b Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Wed, 30 Aug 2023 11:46:18 -0400
Subject: [PATCH 02/68] Updated other apps to work with MotionController
 refactor and new messages

---
 gripper/core/tasks_z.cpp                   | 2 +-
 head/core/can_task.cpp                     | 6 ++++--
 head/core/tasks_proto.cpp                  | 4 ++--
 head/core/tasks_rev1.cpp                   | 4 ++--
 include/gripper/core/can_task.hpp          | 6 ++++--
 include/pipettes/core/dispatch_builder.hpp | 9 ++++++---
 pipettes/core/linear_motor_tasks.cpp       | 4 ++--
 7 files changed, 21 insertions(+), 14 deletions(-)

diff --git a/gripper/core/tasks_z.cpp b/gripper/core/tasks_z.cpp
index 43432ea46..59a2411d0 100644
--- a/gripper/core/tasks_z.cpp
+++ b/gripper/core/tasks_z.cpp
@@ -42,7 +42,7 @@ void z_tasks::start_task(
     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);
+                                         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(
diff --git a/head/core/can_task.cpp b/head/core/can_task.cpp
index 765e749b2..358c537b5 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::ReadMotorDriverErrorStatus>;
 using MoveGroupDispatchTarget = can::dispatch::DispatchParseTarget<
     can::message_handlers::move_group::MoveGroupHandler<
         head_tasks::MotorQueueClient>,
@@ -43,7 +44,8 @@ using MotionControllerDispatchTarget = can::dispatch::DispatchParseTarget<
     can::messages::SetMotionConstraints, can::messages::ReadLimitSwitchRequest,
     can::messages::MotorPositionRequest,
     can::messages::UpdateMotorPositionEstimationRequest,
-    can::messages::GetMotorUsageRequest>;
+    can::messages::GetMotorUsageRequest,
+    can::messages::MotorDriverErrorEncountered>;
 using SystemDispatchTarget = can::dispatch::DispatchParseTarget<
     can::message_handlers::system::SystemMessageHandler<
         head_tasks::HeadQueueClient>,
diff --git a/head/core/tasks_proto.cpp b/head/core/tasks_proto.cpp
index 2690550ca..69e52a767 100644
--- a/head/core/tasks_proto.cpp
+++ b/head/core/tasks_proto.cpp
@@ -141,7 +141,7 @@ 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);
     auto& left_tmc2130_driver = left_motor_driver_task_builder.start(
         5, "left motor driver", left_driver_configs, left_queues,
         spi3_task_client);
@@ -170,7 +170,7 @@ 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);
     auto& right_tmc2130_driver = right_motor_driver_task_builder.start(
         5, "right motor driver", right_driver_configs, right_queues,
         spi2_task_client);
diff --git a/head/core/tasks_rev1.cpp b/head/core/tasks_rev1.cpp
index 2cf2d803d..e150722cb 100644
--- a/head/core/tasks_rev1.cpp
+++ b/head/core/tasks_rev1.cpp
@@ -146,7 +146,7 @@ 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);
     auto& left_tmc2160_driver = left_motor_driver_task_builder.start(
         5, "left motor driver", left_driver_configs, left_queues,
         spi3_task_client);
@@ -183,7 +183,7 @@ 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);
     auto& right_tmc2160_driver = right_motor_driver_task_builder.start(
         5, "right motor driver", right_driver_configs, right_queues,
         spi2_task_client);
diff --git a/include/gripper/core/can_task.hpp b/include/gripper/core/can_task.hpp
index f76f0c960..a5afa5800 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::ReadMotorDriverErrorStatus>;
 using MoveGroupDispatchTarget = can::dispatch::DispatchParseTarget<
     can::message_handlers::move_group::MoveGroupHandler<z_tasks::QueueClient>,
     can::messages::AddLinearMoveRequest,
@@ -32,7 +33,8 @@ using MotionControllerDispatchTarget = can::dispatch::DispatchParseTarget<
     can::messages::SetMotionConstraints, can::messages::ReadLimitSwitchRequest,
     can::messages::MotorPositionRequest,
     can::messages::UpdateMotorPositionEstimationRequest,
-    can::messages::GetMotorUsageRequest>;
+    can::messages::GetMotorUsageRequest,
+    can::messages::MotorDriverErrorEncountered>;
 using SystemDispatchTarget = can::dispatch::DispatchParseTarget<
     can::message_handlers::system::SystemMessageHandler<
         gripper_tasks::QueueClient>,
diff --git a/include/pipettes/core/dispatch_builder.hpp b/include/pipettes/core/dispatch_builder.hpp
index a3f24b51f..9d4effde2 100644
--- a/include/pipettes/core/dispatch_builder.hpp
+++ b/include/pipettes/core/dispatch_builder.hpp
@@ -23,14 +23,16 @@ 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::ReadMotorDriverErrorStatus>;
 
 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::ReadMotorDriverErrorStatus>;
 
 using GearMotorDispatchTarget = can::dispatch::DispatchParseTarget<
     can::message_handlers::motor::GearMotorHandler<
@@ -62,7 +64,8 @@ using MotionControllerDispatchTarget = can::dispatch::DispatchParseTarget<
     can::messages::SetMotionConstraints, can::messages::ReadLimitSwitchRequest,
     can::messages::MotorPositionRequest,
     can::messages::UpdateMotorPositionEstimationRequest,
-    can::messages::GetMotorUsageRequest>;
+    can::messages::GetMotorUsageRequest,
+    can::messages::MotorDriverErrorEncountered>;
 
 using GearMotionControllerDispatchTarget = can::dispatch::DispatchParseTarget<
     gear_motion_handler::GearMotorMotionHandler<gear_motor_tasks::QueueClient>,
diff --git a/pipettes/core/linear_motor_tasks.cpp b/pipettes/core/linear_motor_tasks.cpp
index 67909454b..e9939b5b6 100644
--- a/pipettes/core/linear_motor_tasks.cpp
+++ b/pipettes/core/linear_motor_tasks.cpp
@@ -50,7 +50,7 @@ void linear_motor_tasks::start_tasks(
 
     // Linear Motor Tasks
     auto& motion = mc_task_builder.start(5, "motion controller",
-                                         motion_controller, queues, queues);
+                                         motion_controller, queues, queues, tmc2130_queues);
     auto& tmc2130_driver = tmc2130_driver_task_builder.start(
         5, "tmc2130 driver", linear_driver_configs, queues, spi_writer);
     auto& move_group =
@@ -101,7 +101,7 @@ void linear_motor_tasks::start_tasks(
 
     // Linear Motor Tasks
     auto& motion = mc_task_builder.start(5, "motion controller",
-                                         motion_controller, queues, queues);
+                                         motion_controller, queues, queues, tmc2160_queues);
     auto& tmc2160_driver = tmc2160_driver_task_builder.start(
         5, "tmc2160 driver", linear_driver_configs, queues, spi_writer);
     auto& move_group =

From 9f90395c1a658f81426190818c72fb7d261bd195 Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Tue, 19 Sep 2023 17:10:06 -0400
Subject: [PATCH 03/68] messaging with robot-server working

---
 gantry/firmware/interfaces_rev1.cpp           |  2 ++
 gantry/firmware/motor_hardware.c              |  2 +-
 include/can/core/ids.hpp                      |  1 +
 include/can/core/messages.hpp                 | 21 +++++++++++++-
 .../core/stepper_motor/tmc2130_driver.hpp     |  4 +--
 .../core/stepper_motor/tmc2160.hpp            | 16 ++++++++++
 .../core/stepper_motor/tmc2160_driver.hpp     | 26 +++++++++++++++--
 .../tasks/gear_tmc2160_motor_driver_task.hpp  |  3 +-
 .../core/tasks/motion_controller_task.hpp     | 14 ++++++---
 .../core/tasks/tmc2130_motor_driver_task.hpp  |  6 ++--
 .../core/tasks/tmc2160_motor_driver_task.hpp  | 29 +++++++++++++------
 include/spi/core/messages.hpp                 |  1 +
 include/spi/core/writer.hpp                   |  5 ++--
 13 files changed, 106 insertions(+), 24 deletions(-)

diff --git a/gantry/firmware/interfaces_rev1.cpp b/gantry/firmware/interfaces_rev1.cpp
index 692ebd482..a69e6ec0c 100644
--- a/gantry/firmware/interfaces_rev1.cpp
+++ b/gantry/firmware/interfaces_rev1.cpp
@@ -170,6 +170,7 @@ static tmc2160::configs::TMC2160DriverConfig motor_driver_config_x{
                               .freewheel = 0,
                               .pwm_reg = 0x7,
                               .pwm_lim = 0xC},
+                  .drvconf = {.ot_select = 0x0},
                   .glob_scale = {.global_scaler = 0xA7}},
     .current_config =
         {
@@ -210,6 +211,7 @@ static tmc2160::configs::TMC2160DriverConfig motor_driver_config_y{
                               .freewheel = 0,
                               .pwm_reg = 0x7,
                               .pwm_lim = 0xC},
+                  .drvconf = {.ot_select = 0x0},
                   .glob_scale = {.global_scaler = 0xA7}},
     .current_config =
         {
diff --git a/gantry/firmware/motor_hardware.c b/gantry/firmware/motor_hardware.c
index 0f9c07427..1328764c9 100644
--- a/gantry/firmware/motor_hardware.c
+++ b/gantry/firmware/motor_hardware.c
@@ -59,7 +59,7 @@ void HAL_SPI_MspInit(SPI_HandleTypeDef* hspi) {
 
         // Diag0
         GPIO_InitStruct.Pin = GPIO_PIN_5;
-        GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING;
+        GPIO_InitStruct.Mode = GPIO_MODE_IT_FALLING;
         GPIO_InitStruct.Pull = GPIO_PULLUP; // confirm correct
         HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
     }
diff --git a/include/can/core/ids.hpp b/include/can/core/ids.hpp
index 0773a0036..8250d0de8 100644
--- a/include/can/core/ids.hpp
+++ b/include/can/core/ids.hpp
@@ -63,6 +63,7 @@ enum class MessageId {
     read_motor_current_response = 0x35,
     motor_driver_error_encountered = 0x36,
     read_motor_driver_error_status = 0x37,
+    read_motor_driver_error_register_response = 0x38,
     set_brushed_motor_vref_request = 0x40,
     set_brushed_motor_pwm_request = 0x41,
     gripper_grip_request = 0x42,
diff --git a/include/can/core/messages.hpp b/include/can/core/messages.hpp
index 51ee84450..49b585bd9 100644
--- a/include/can/core/messages.hpp
+++ b/include/can/core/messages.hpp
@@ -634,6 +634,24 @@ struct ReadMotorDriverRegisterResponse
         -> bool = default;
 };
 
+struct ReadMotorDriverErrorRegisterResponse
+    : BaseMessage<MessageId::read_motor_driver_error_register_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 ReadMotorDriverErrorRegisterResponse& other) const
+        -> bool = default;
+};
+
 struct WriteMotorCurrentRequest
     : BaseMessage<MessageId::write_motor_current_request> {
     uint32_t message_index;
@@ -1519,6 +1537,7 @@ using ResponseMessageType = std::variant<
     BindSensorOutputResponse, GripperInfoResponse, TipActionResponse,
     PeripheralStatusResponse, BrushedMotorConfResponse,
     UpdateMotorPositionEstimationResponse, BaselineSensorResponse,
-    PushTipPresenceNotification, GetMotorUsageResponse>;
+    PushTipPresenceNotification, GetMotorUsageResponse,
+    ReadMotorDriverErrorRegisterResponse>;
 
 }  // namespace can::messages
diff --git a/include/motor-control/core/stepper_motor/tmc2130_driver.hpp b/include/motor-control/core/stepper_motor/tmc2130_driver.hpp
index fefe1f049..bc7983be6 100644
--- a/include/motor-control/core/stepper_motor/tmc2130_driver.hpp
+++ b/include/motor-control/core/stepper_motor/tmc2130_driver.hpp
@@ -52,11 +52,11 @@ class TMC2130 {
     auto operator=(const TMC2130&& c) = delete;
     ~TMC2130() = default;
 
-    auto read(Registers addr, uint32_t command_data, uint32_t message_index)
+    auto read(Registers addr, uint32_t command_data, uint32_t message_index, bool error_response)
         -> void {
         auto converted_addr = static_cast<uint8_t>(addr);
         _spi_manager.read(converted_addr, command_data, _task_queue, _cs_intf,
-                          message_index);
+                          message_index, error_response);
     }
 
     auto write(Registers addr, uint32_t command_data) -> bool {
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 31f6d2a0c..d31bf86cf 100644
--- a/include/motor-control/core/stepper_motor/tmc2160_driver.hpp
+++ b/include/motor-control/core/stepper_motor/tmc2160_driver.hpp
@@ -53,11 +53,11 @@ class TMC2160 {
     auto operator=(const TMC2160&& c) = delete;
     ~TMC2160() = default;
 
-    auto read(Registers addr, uint32_t command_data, uint32_t message_index)
+    auto read(Registers addr, uint32_t command_data, uint32_t message_index, bool error_response)
         -> void {
         auto converted_addr = static_cast<uint8_t>(addr);
         _spi_manager.read(converted_addr, command_data, _task_queue, _cs_intf,
-                          message_index);
+                          message_index, error_response);
     }
 
     auto write(Registers addr, uint32_t command_data) -> bool {
@@ -105,6 +105,9 @@ class TMC2160 {
         if (!set_glob_scaler(_registers.glob_scale)) {
             return false;
         }
+        if (!set_drv_conf(_registers.drvconf)) {
+            return false;
+        }
         _initialized = true;
         return true;
     }
@@ -159,6 +162,9 @@ class TMC2160 {
             case Registers::DRVSTATUS:
                 update_driver_status(0);
                 break;
+            // case Registers::DRV_CONF:
+            //     update_drv_conf(0);
+            //     break;
             default:
                 break;
         }
@@ -272,6 +278,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..c97e67d52 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
@@ -81,9 +81,10 @@ class MotorDriverMessageHandler {
     void handle(const can::messages::GearReadMotorDriverRegister& m) {
         LOG("Received read motor driver request: addr=%d", m.reg_address);
         uint32_t data = 0;
+        bool error_response = false;
         if (tmc2160::registers::is_valid_address(m.reg_address)) {
             driver.read(tmc2160::registers::Registers(m.reg_address), data,
-                        m.message_index);
+                        m.message_index, error_response);
         }
     }
 
diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp
index 513c4691a..feac3833c 100644
--- a/include/motor-control/core/tasks/motion_controller_task.hpp
+++ b/include/motor-control/core/tasks/motion_controller_task.hpp
@@ -147,10 +147,10 @@ class MotionControllerMessageHandler {
     // set error flag here (C++ atomic)?! Disable subsystem (enable brakes)?!
     // cancel_and_clear_moves(can::ids::ErrorCode::motor_driver_error)?
     void handle(const can::messages::MotorDriverErrorEncountered& m) {
+        // check if gpio is low before making controller calls
         controller.stop(); // also request_cancel()?
         can::messages::ReadMotorDriverErrorStatus msg{
-            .message_index = m.message_index // need?
-        };
+            .message_index = m.message_index};
         driver_client.send_motor_driver_queue(msg);
     }
 
@@ -203,9 +203,15 @@ class MotionControllerTask {
 
     [[nodiscard]] auto get_queue() const -> QueueType& { return queue; }
 
+    // also create top level query msg
     void run_diag0_interrupt(void) {
-        can::messages::MotorDriverErrorEncountered msg{};
-        queue.try_write(msg);
+        can::messages::MotorDriverErrorEncountered msg{
+            .message_index = 0}; //fixed msg index, see elsewhere
+        bool sent = queue.try_write_isr(msg); //error begins here
+        if (!sent) { // improve
+            sent = queue.try_write_isr(msg);
+        }
+        // handler.handle_message(msg); ?
     }
 
   private:
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 1df5e73f6..d249a5c91 100644
--- a/include/motor-control/core/tasks/tmc2130_motor_driver_task.hpp
+++ b/include/motor-control/core/tasks/tmc2130_motor_driver_task.hpp
@@ -80,17 +80,19 @@ class MotorDriverMessageHandler {
     void handle(const can::messages::ReadMotorDriverRegister& m) {
         LOG("Received read motor driver request: addr=%d", m.reg_address);
         uint32_t data = 0;
+        bool error_response = false;
         if (tmc2130::registers::is_valid_address(m.reg_address)) {
             driver.read(tmc2130::registers::Registers(m.reg_address), data,
-                        m.message_index);
+                        m.message_index, error_response);
         }
     }
 
     void handle(const can::messages::ReadMotorDriverErrorStatus& m) {
         // LOG?
         uint32_t data = 0;
+        bool error_response = true;
         driver.read(tmc2130::registers::Registers::DRVSTATUS, data,
-                    m.message_index);
+                    m.message_index, error_response);
     }
 
     void handle(const can::messages::WriteMotorCurrentRequest& m) {
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 c56dfbffe..3a7b7dc96 100644
--- a/include/motor-control/core/tasks/tmc2160_motor_driver_task.hpp
+++ b/include/motor-control/core/tasks/tmc2160_motor_driver_task.hpp
@@ -57,12 +57,21 @@ 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 (m.id.error_response) {
+                can::messages::ReadMotorDriverErrorRegisterResponse 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);
+            }
         }
     }
 
@@ -79,20 +88,22 @@ class MotorDriverMessageHandler {
     void handle(const can::messages::ReadMotorDriverRegister& m) {
         LOG("Received read motor driver request: addr=%d", m.reg_address);
         uint32_t data = 0;
+        bool error_response = false;
         if (tmc2160::registers::is_valid_address(m.reg_address)) {
             driver.read(tmc2160::registers::Registers(m.reg_address), data,
-                        m.message_index);
+                        m.message_index, error_response);
         }
     }
 
-    // need a response handler? Eh, should get handled by TransactResponse
+    // need a response handler? Yes, send response once when interrupt and twice when response received
     // ensure specific error is returned
     // just use above method?!
     void handle(const can::messages::ReadMotorDriverErrorStatus& m) {
         // LOG?
         uint32_t data = 0;
+        bool error_response = true;
         driver.read(tmc2160::registers::Registers::DRVSTATUS, data,
-                    m.message_index);
+                    m.message_index, error_response);
     }
 
     void handle(const can::messages::WriteMotorCurrentRequest& m) {
diff --git a/include/spi/core/messages.hpp b/include/spi/core/messages.hpp
index 344b06b9e..d96c74d5a 100644
--- a/include/spi/core/messages.hpp
+++ b/include/spi/core/messages.hpp
@@ -20,6 +20,7 @@ struct TransactionIdentifier {
     uint8_t command_type;
     bool requires_response;
     uint32_t message_index;
+    bool error_response;
 
     auto operator==(const TransactionIdentifier& other) const -> bool = default;
 };
diff --git a/include/spi/core/writer.hpp b/include/spi/core/writer.hpp
index a0cb9df82..00693eb81 100644
--- a/include/spi/core/writer.hpp
+++ b/include/spi/core/writer.hpp
@@ -48,14 +48,15 @@ class Writer {
     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 {
+              uint32_t message_index, bool error_response) -> bool {
         auto txBuffer = build_message(register_addr, spi::hardware::Mode::READ,
                                       command_data);
         TransactionIdentifier _transaction_id{
             .token = register_addr,
             .command_type = static_cast<uint8_t>(spi::hardware::Mode::READ),
             .requires_response = false,
-            .message_index = message_index};
+            .message_index = message_index,
+            .error_response = error_response};
         Transact message{
             .id = _transaction_id,
             .transaction = {.txBuffer = txBuffer, .cs_interface = cs_intf},

From 2f1d488d19cef5159e8c318dc43e9964d598bf8d Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Thu, 12 Oct 2023 16:33:53 -0400
Subject: [PATCH 04/68] working prototype

---
 gantry/firmware/interfaces_proto.cpp          |  2 +-
 gantry/firmware/interfaces_rev1.cpp           |  2 +-
 gripper/firmware/interfaces_z_motor.cpp       |  2 +-
 head/firmware/main_proto.cpp                  |  4 +--
 head/firmware/main_rev1.cpp                   |  4 +--
 include/bootloader/core/ids.h                 |  1 +
 include/can/core/ids.hpp                      |  1 +
 include/can/core/messages.hpp                 | 32 +++++++++----------
 .../brushed_motion_controller.hpp             |  4 +--
 .../brushed_motor_interrupt_handler.hpp       |  4 +--
 .../core/motor_hardware_interface.hpp         |  4 +--
 .../core/stepper_motor/motion_controller.hpp  | 20 +++++++-----
 .../stepper_motor/motor_interrupt_handler.hpp | 23 ++++++++++---
 .../core/stepper_motor/tmc2130_driver.hpp     |  6 ++--
 .../core/stepper_motor/tmc2160_driver.hpp     |  6 ++--
 .../tasks/gear_tmc2160_motor_driver_task.hpp  | 16 ++++++++--
 .../core/tasks/motion_controller_task.hpp     | 31 ++++++++++++------
 .../core/tasks/tmc2130_motor_driver_task.hpp  | 13 ++++----
 .../core/tasks/tmc2160_motor_driver_task.hpp  | 16 ++++++----
 .../core/tasks/tmc_motor_driver_common.hpp    |  1 +
 .../brushed_motor/brushed_motor_hardware.hpp  |  8 ++---
 .../firmware/stepper_motor/motor_hardware.hpp |  8 ++---
 include/pipettes/core/dispatch_builder.hpp    |  1 +
 include/pipettes/core/gear_motor_tasks.hpp    |  2 +-
 include/pipettes/core/linear_motor_tasks.hpp  |  4 +--
 include/pipettes/firmware/interfaces.hpp      | 10 +++---
 include/pipettes/firmware/interfaces_g4.hpp   | 10 +++---
 include/spi/core/messages.hpp                 |  1 -
 include/spi/core/utils.hpp                    | 30 +++++++++++++++++
 include/spi/core/writer.hpp                   | 13 ++++----
 pipettes/core/gear_motor_tasks.cpp            |  2 +-
 pipettes/firmware/interfaces.cpp              | 12 +++----
 32 files changed, 181 insertions(+), 112 deletions(-)

diff --git a/gantry/firmware/interfaces_proto.cpp b/gantry/firmware/interfaces_proto.cpp
index 415c44203..8fac0992a 100644
--- a/gantry/firmware/interfaces_proto.cpp
+++ b/gantry/firmware/interfaces_proto.cpp
@@ -244,7 +244,7 @@ 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,
+    motor_queue, gantry::queues::get_queues(), gantry::queues::get_queues(), motor_hardware_iface, stallcheck,
     update_position_queue);
 
 static auto encoder_background_timer =
diff --git a/gantry/firmware/interfaces_rev1.cpp b/gantry/firmware/interfaces_rev1.cpp
index a69e6ec0c..338ae5935 100644
--- a/gantry/firmware/interfaces_rev1.cpp
+++ b/gantry/firmware/interfaces_rev1.cpp
@@ -271,7 +271,7 @@ 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,
+    motor_queue, gantry::queues::get_queues(), gantry::queues::get_queues(), motor_hardware_iface, stallcheck,
     update_position_queue);
 
 static auto encoder_background_timer =
diff --git a/gripper/firmware/interfaces_z_motor.cpp b/gripper/firmware/interfaces_z_motor.cpp
index c9c43c9a9..d691d4ad5 100644
--- a/gripper/firmware/interfaces_z_motor.cpp
+++ b/gripper/firmware/interfaces_z_motor.cpp
@@ -190,7 +190,7 @@ 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,
+    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 =
diff --git a/head/firmware/main_proto.cpp b/head/firmware/main_proto.cpp
index d3ba07641..7b0948814 100644
--- a/head/firmware/main_proto.cpp
+++ b/head/firmware/main_proto.cpp
@@ -268,7 +268,7 @@ 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,
+    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 =
@@ -289,7 +289,7 @@ 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,
+    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 =
diff --git a/head/firmware/main_rev1.cpp b/head/firmware/main_rev1.cpp
index ce5849129..4d8e27054 100644
--- a/head/firmware/main_rev1.cpp
+++ b/head/firmware/main_rev1.cpp
@@ -286,7 +286,7 @@ 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,
+    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 =
@@ -311,7 +311,7 @@ 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,
+    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 =
diff --git a/include/bootloader/core/ids.h b/include/bootloader/core/ids.h
index 36150808d..32b400abb 100644
--- a/include/bootloader/core/ids.h
+++ b/include/bootloader/core/ids.h
@@ -93,6 +93,7 @@ typedef enum {
     can_messageid_gear_set_current_request = 0x505,
     can_messageid_gear_write_motor_driver_request = 0x506,
     can_messageid_gear_read_motor_driver_request = 0x507,
+    can_messageid_gear_read_motor_driver_error_status = 0x508,
     can_messageid_read_sensor_request = 0x82,
     can_messageid_write_sensor_request = 0x83,
     can_messageid_baseline_sensor_request = 0x84,
diff --git a/include/can/core/ids.hpp b/include/can/core/ids.hpp
index 8250d0de8..ba6926300 100644
--- a/include/can/core/ids.hpp
+++ b/include/can/core/ids.hpp
@@ -98,6 +98,7 @@ enum class MessageId {
     gear_set_current_request = 0x505,
     gear_write_motor_driver_request = 0x506,
     gear_read_motor_driver_request = 0x507,
+    gear_read_motor_driver_error_status = 0x508,
     read_sensor_request = 0x82,
     write_sensor_request = 0x83,
     baseline_sensor_request = 0x84,
diff --git a/include/can/core/messages.hpp b/include/can/core/messages.hpp
index 49b585bd9..52dcd7345 100644
--- a/include/can/core/messages.hpp
+++ b/include/can/core/messages.hpp
@@ -600,22 +600,6 @@ struct ReadMotorDriverRegister
         -> bool = default;
 };
 
-struct ReadMotorDriverErrorStatus
-    : BaseMessage<MessageId::read_motor_driver_error_status> {
-    uint32_t message_index;
-
-    template <bit_utils::ByteIterator Input, typename Limit>
-    static auto parse(Input body, Limit limit) -> ReadMotorDriverErrorStatus {
-        uint32_t msg_ind = 0;
-
-        body = bit_utils::bytes_to_int(body, limit, msg_ind);
-        return ReadMotorDriverErrorStatus{.message_index = msg_ind};
-    }
-
-    auto operator==(const ReadMotorDriverErrorStatus& other) const
-        -> bool = default;
-};
-
 struct ReadMotorDriverRegisterResponse
     : BaseMessage<MessageId::read_motor_driver_register_response> {
     uint32_t message_index;
@@ -634,6 +618,22 @@ struct ReadMotorDriverRegisterResponse
         -> bool = default;
 };
 
+struct ReadMotorDriverErrorStatus
+    : BaseMessage<MessageId::read_motor_driver_error_status> {
+    uint32_t message_index;
+
+    template <bit_utils::ByteIterator Input, typename Limit>
+    static auto parse(Input body, Limit limit) -> ReadMotorDriverErrorStatus {
+        uint32_t msg_ind = 0;
+
+        body = bit_utils::bytes_to_int(body, limit, msg_ind);
+        return ReadMotorDriverErrorStatus{.message_index = msg_ind};
+    }
+
+    auto operator==(const ReadMotorDriverErrorStatus& other) const
+        -> bool = default;
+};
+
 struct ReadMotorDriverErrorRegisterResponse
     : BaseMessage<MessageId::read_motor_driver_error_register_response> {
     uint32_t message_index;
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 18fbc315d..eefa230b7 100644
--- a/include/motor-control/core/brushed_motor/brushed_motion_controller.hpp
+++ b/include/motor-control/core/brushed_motor/brushed_motion_controller.hpp
@@ -100,14 +100,14 @@ class MotionController {
         enabled = false;
     }
 
-    void stop() {
+    void stop(can::ids::ErrorSeverity error_severity = can::ids::ErrorSeverity::warning) {
         queue.reset();
         // if we're gripping something we need to flag this so we don't drop it
         if (!hardware.get_stay_enabled()) {
             disable_motor();
         }
         if (hardware.is_timer_interrupt_running()) {
-            hardware.request_cancel();
+            hardware.request_cancel(static_cast<uint8_t>(error_severity));
         }
     }
 
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 a4c8b4ff8..9a3d084e2 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
@@ -196,8 +196,8 @@ class BrushedMotorInterruptHandler {
             cancel_and_clear_moves(can::ids::ErrorCode::estop_detected);
             motor_state = ControlState::ESTOP;
         } else if (hardware.has_cancel_request()) {
-            cancel_and_clear_moves(can::ids::ErrorCode::stop_requested,
-                                   can::ids::ErrorSeverity::warning);
+            cancel_and_clear_moves();
+            // cancel_and_clear_moves(can::ids::ErrorCode::stop_requested, can::ids::ErrorSeverity::warning);
             motor_state = ControlState::IDLE;
         } else if (tick < HOLDOFF_TICKS) {
             tick++;
diff --git a/include/motor-control/core/motor_hardware_interface.hpp b/include/motor-control/core/motor_hardware_interface.hpp
index 5d2173415..0e81b3556 100644
--- a/include/motor-control/core/motor_hardware_interface.hpp
+++ b/include/motor-control/core/motor_hardware_interface.hpp
@@ -98,8 +98,8 @@ 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 has_cancel_request() -> uint8_t = 0;
+    virtual void request_cancel(uint8_t error) = 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 af76fb65f..ff96c64a4 100644
--- a/include/motor-control/core/stepper_motor/motion_controller.hpp
+++ b/include/motor-control/core/stepper_motor/motion_controller.hpp
@@ -107,14 +107,18 @@ class MotionController {
         return update_queue.try_write(can_msg);
     }
 
-    void stop() {
-        queue.reset();
+    void stop(can::ids::ErrorSeverity error_severity = can::ids::ErrorSeverity::warning) {
+        queue.reset(); // empties queue
         disable_motor();
         if (hardware.is_timer_interrupt_running()) {
-            hardware.request_cancel();
+            hardware.request_cancel(static_cast<uint8_t>(error_severity)); // use ErrorSeverity, converted to uint_8! // see if/how checked. Calls cancel_and_clear_moves!
         }
     }
 
+    bool is_timer_interrupt_running() {
+        return hardware.is_timer_interrupt_running();
+    }
+
     auto read_limit_switch() -> bool { return hardware.check_limit_switch(); }
 
     [[nodiscard]] auto read_motor_position() const {
@@ -136,10 +140,10 @@ class MotionController {
     }
 
     void disable_motor() {
-        hardware.deactivate_motor();
+        hardware.deactivate_motor(); // sets e-stop pin and resets enable pin
         hardware.position_flags.clear_flag(
             can::ids::MotorPositionFlags::stepper_position_ok);
-        enabled = false;
+        enabled = false; // see if/how checked
     }
 
     void set_motion_constraints(
@@ -255,14 +259,14 @@ class PipetteMotionController {
         return false;
     }
 
-    void stop() {
+    void stop(can::ids::ErrorSeverity error_severity = can::ids::ErrorSeverity::warning) {
         queue.reset();
         disable_motor();
         // 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();
+        if (hardware.is_timer_interrupt_running()) { // should always be true, right?
+            hardware.request_cancel(static_cast<uint8_t>(error_severity));
         }
     }
 
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 39122274c..1b62a81f7 100644
--- a/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp
+++ b/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp
@@ -9,6 +9,7 @@
 #include "motor-control/core/motor_messages.hpp"
 #include "motor-control/core/stall_check.hpp"
 #include "motor-control/core/tasks/move_status_reporter_task.hpp"
+#include "motor-control/core/tasks/tmc_motor_driver_common.hpp"
 
 namespace motor_handler {
 
@@ -41,7 +42,7 @@ using namespace motor_messages;
  * Note: The position tracker should never be allowed to go below zero.
  */
 
-template <template <class> class QueueImpl, class StatusClient,
+template <template <class> class QueueImpl, class StatusClient, class DriverClient,
           typename MotorMoveMessage, typename MotorHardware>
 requires MessageQueue<QueueImpl<MotorMoveMessage>, MotorMoveMessage> &&
     std::is_base_of_v<motor_hardware::MotorHardwareIface, MotorHardware>
@@ -54,11 +55,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) {
@@ -178,6 +181,7 @@ class MotorInterruptHandler {
 
     void run_interrupt() {
         // handle various error states
+        uint8_t has_cancel_request = hardware.has_cancel_request();
         if (clear_queue_until_empty) {
             // If we were executing a move when estop asserted, and
             // what's in the queue is the remaining enqueued moves from
@@ -196,9 +200,12 @@ 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 (has_cancel_request) { // consider stall detected error. Don't see issue
+            if (has_cancel_request == static_cast<uint8_t>(can::ids::ErrorSeverity::unrecoverable)) {
+                cancel_and_clear_moves();
+            } else {
+                cancel_and_clear_moves(can::ids::ErrorCode::stop_requested, can::ids::ErrorSeverity::warning);
+            }
         } else {
             // Normal Move logic
             run_normal_interrupt();
@@ -399,7 +406,7 @@ class MotorInterruptHandler {
     [[nodiscard]] auto set_direction_pin() const -> bool {
         return (buffered_move.velocity > 0);
     }
-    void cancel_and_clear_moves(
+    void cancel_and_clear_moves( // use this! Change input args
         can::ids::ErrorCode err_code = can::ids::ErrorCode::hardware,
         can::ids::ErrorSeverity severity =
             can::ids::ErrorSeverity::unrecoverable) {
@@ -414,6 +421,11 @@ class MotorInterruptHandler {
             can::messages::ErrorMessage{.message_index = message_index,
                                         .severity = severity,
                                         .error_code = err_code});
+        // only send when motor driver error
+        if (err_code == can::ids::ErrorCode::hardware) {
+            driver_client.send_motor_driver_queue(
+                can::messages::ReadMotorDriverErrorStatus{.message_index = message_index});
+        }
         if (err_code == can::ids::ErrorCode::collision_detected) {
             build_and_send_ack(AckMessageId::position_error);
         }
@@ -586,6 +598,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 bc7983be6..12e920752 100644
--- a/include/motor-control/core/stepper_motor/tmc2130_driver.hpp
+++ b/include/motor-control/core/stepper_motor/tmc2130_driver.hpp
@@ -52,11 +52,9 @@ class TMC2130 {
     auto operator=(const TMC2130&& c) = delete;
     ~TMC2130() = default;
 
-    auto read(Registers addr, uint32_t command_data, uint32_t message_index, bool error_response)
+    auto read(uint32_t token, uint32_t command_data, uint32_t message_index)
         -> void {
-        auto converted_addr = static_cast<uint8_t>(addr);
-        _spi_manager.read(converted_addr, command_data, _task_queue, _cs_intf,
-                          message_index, error_response);
+        _spi_manager.read(token, command_data, _task_queue, _cs_intf, message_index);
     }
 
     auto write(Registers addr, uint32_t command_data) -> bool {
diff --git a/include/motor-control/core/stepper_motor/tmc2160_driver.hpp b/include/motor-control/core/stepper_motor/tmc2160_driver.hpp
index d31bf86cf..b15f850e9 100644
--- a/include/motor-control/core/stepper_motor/tmc2160_driver.hpp
+++ b/include/motor-control/core/stepper_motor/tmc2160_driver.hpp
@@ -53,11 +53,9 @@ class TMC2160 {
     auto operator=(const TMC2160&& c) = delete;
     ~TMC2160() = default;
 
-    auto read(Registers addr, uint32_t command_data, uint32_t message_index, bool error_response)
+    auto read(uint32_t token, uint32_t command_data, uint32_t message_index)
         -> void {
-        auto converted_addr = static_cast<uint8_t>(addr);
-        _spi_manager.read(converted_addr, command_data, _task_queue, _cs_intf,
-                          message_index, error_response);
+        _spi_manager.read(token, command_data, _task_queue, _cs_intf, message_index);
     }
 
     auto write(Registers addr, uint32_t command_data) -> bool {
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 c97e67d52..406914e06 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
@@ -81,13 +81,23 @@ class MotorDriverMessageHandler {
     void handle(const can::messages::GearReadMotorDriverRegister& m) {
         LOG("Received read motor driver request: addr=%d", m.reg_address);
         uint32_t data = 0;
-        bool error_response = false;
         if (tmc2160::registers::is_valid_address(m.reg_address)) {
-            driver.read(tmc2160::registers::Registers(m.reg_address), data,
-                        m.message_index, error_response);
+            auto converted_addr = static_cast<uint8_t>(m.reg_address);
+            uint32_t token = spi::utils::build_token(converted_addr);
+            driver.read(token, data, m.message_index);
         }
     }
 
+    // see notes re:this in tmc2160_motor_driver_task
+    void handle(const can::messages::ReadMotorDriverErrorStatus& m) {
+        // LOG?
+        uint32_t data = 0;
+        auto converted_addr = static_cast<uint8_t>(tmc2160::registers::Registers::DRVSTATUS);
+        std::array tags{spi::utils::ResponseTag::IS_ERROR_RESPONSE};
+        uint32_t token = spi::utils::build_token(converted_addr, spi::utils::byte_from_tags(tags));
+        driver.read(token, data, m.message_index);
+    }
+
     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/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp
index feac3833c..ee98c6b5f 100644
--- a/include/motor-control/core/tasks/motion_controller_task.hpp
+++ b/include/motor-control/core/tasks/motion_controller_task.hpp
@@ -148,16 +148,26 @@ class MotionControllerMessageHandler {
     // cancel_and_clear_moves(can::ids::ErrorCode::motor_driver_error)?
     void handle(const can::messages::MotorDriverErrorEncountered& m) {
         // check if gpio is low before making controller calls
-        controller.stop(); // also request_cancel()?
-        can::messages::ReadMotorDriverErrorStatus msg{
-            .message_index = m.message_index};
-        driver_client.send_motor_driver_queue(msg);
+        //if (!driver_error_handled()) {
+            controller.stop(can::ids::ErrorSeverity::unrecoverable);
+            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::hardware});
+                driver_client.send_motor_driver_queue(
+                    can::messages::ReadMotorDriverErrorStatus{.message_index = m.message_index});
+            }
+        //}
     }
 
+    bool driver_error_handled() { return driver_error_handled_flag.exchange(true); }
+
     MotorControllerType& controller;
     CanClient& can_client;
     UsageClient& usage_client;
     DriverClient& driver_client;
+    std::atomic<bool> driver_error_handled_flag = false;
 };
 
 /**
@@ -205,17 +215,18 @@ class MotionControllerTask {
 
     // also create top level query msg
     void run_diag0_interrupt(void) {
-        can::messages::MotorDriverErrorEncountered msg{
-            .message_index = 0}; //fixed msg index, see elsewhere
-        bool sent = queue.try_write_isr(msg); //error begins here
-        if (!sent) { // improve
-            sent = queue.try_write_isr(msg);
+        if (!driver_error_handled()) {
+            sent = true;
+            // static_cast<void>(queue.try_write_isr(can::messages::MotorDriverErrorEncountered{.message_index = 0}));
         }
-        // handler.handle_message(msg); ?
     }
 
+    bool driver_error_handled() { return driver_error_handled_flag.exchange(true); }
+
   private:
     QueueType& queue;
+    std::atomic<bool> driver_error_handled_flag = false;
+    bool sent = false;
 };
 
 /**
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 d249a5c91..3d3ffd3b8 100644
--- a/include/motor-control/core/tasks/tmc2130_motor_driver_task.hpp
+++ b/include/motor-control/core/tasks/tmc2130_motor_driver_task.hpp
@@ -80,19 +80,20 @@ class MotorDriverMessageHandler {
     void handle(const can::messages::ReadMotorDriverRegister& m) {
         LOG("Received read motor driver request: addr=%d", m.reg_address);
         uint32_t data = 0;
-        bool error_response = false;
         if (tmc2130::registers::is_valid_address(m.reg_address)) {
-            driver.read(tmc2130::registers::Registers(m.reg_address), data,
-                        m.message_index, error_response);
+            auto converted_addr = static_cast<uint8_t>(m.reg_address);
+            uint32_t token = spi::utils::build_token(converted_addr);
+            driver.read(token, data, m.message_index);
         }
     }
 
     void handle(const can::messages::ReadMotorDriverErrorStatus& m) {
         // LOG?
         uint32_t data = 0;
-        bool error_response = true;
-        driver.read(tmc2130::registers::Registers::DRVSTATUS, data,
-                    m.message_index, error_response);
+        auto converted_addr = static_cast<uint8_t>(tmc2130::registers::Registers::DRVSTATUS);
+        std::array tags{spi::utils::ResponseTag::IS_ERROR_RESPONSE};
+        uint32_t token = spi::utils::build_token(converted_addr, spi::utils::byte_from_tags(tags));
+        driver.read(token, data, m.message_index);
     }
 
     void handle(const can::messages::WriteMotorCurrentRequest& m) {
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 3a7b7dc96..3122cf950 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,7 +58,7 @@ class MotorDriverMessageHandler {
             auto data = driver.handle_spi_read(
                 tmc2160::registers::Registers(static_cast<uint8_t>(m.id.token)),
                 m.rxBuffer);
-            if (m.id.error_response) {
+            if (spi::utils::tag_in_token(m.id.token, spi::utils::ResponseTag::IS_ERROR_RESPONSE)) {
                 can::messages::ReadMotorDriverErrorRegisterResponse response_msg{
                     .message_index = m.id.message_index,
                     .reg_address = static_cast<uint8_t>(m.id.token),
@@ -88,10 +89,10 @@ class MotorDriverMessageHandler {
     void handle(const can::messages::ReadMotorDriverRegister& m) {
         LOG("Received read motor driver request: addr=%d", m.reg_address);
         uint32_t data = 0;
-        bool error_response = false;
         if (tmc2160::registers::is_valid_address(m.reg_address)) {
-            driver.read(tmc2160::registers::Registers(m.reg_address), data,
-                        m.message_index, error_response);
+            auto converted_addr = static_cast<uint8_t>(m.reg_address);
+            uint32_t token = spi::utils::build_token(converted_addr);
+            driver.read(token, data, m.message_index);
         }
     }
 
@@ -101,9 +102,10 @@ class MotorDriverMessageHandler {
     void handle(const can::messages::ReadMotorDriverErrorStatus& m) {
         // LOG?
         uint32_t data = 0;
-        bool error_response = true;
-        driver.read(tmc2160::registers::Registers::DRVSTATUS, data,
-                    m.message_index, error_response);
+        auto converted_addr = static_cast<uint8_t>(tmc2160::registers::Registers::DRVSTATUS);
+        std::array tags{spi::utils::ResponseTag::IS_ERROR_RESPONSE};
+        uint32_t token = spi::utils::build_token(converted_addr, spi::utils::byte_from_tags(tags));
+        driver.read(token, data, m.message_index);
     }
 
     void handle(const can::messages::WriteMotorCurrentRequest& m) {
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 35f1024f8..0a132dc9a 100644
--- a/include/motor-control/core/tasks/tmc_motor_driver_common.hpp
+++ b/include/motor-control/core/tasks/tmc_motor_driver_common.hpp
@@ -14,6 +14,7 @@ using CanMessageTuple = std::tuple<can::messages::ReadMotorDriverRegister,
                                    can::messages::ReadMotorDriverErrorStatus>;
 using GearCanMessageTuple =
     std::tuple<can::messages::GearReadMotorDriverRegister,
+               can::messages::ReadMotorDriverErrorStatus,
                can::messages::GearWriteMotorDriverRegister,
                can::messages::GearWriteMotorCurrentRequest>;
 using CanMessage =
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 a3c3987b0..bf086d8fe 100644
--- a/include/motor-control/firmware/brushed_motor/brushed_motor_hardware.hpp
+++ b/include/motor-control/firmware/brushed_motor/brushed_motor_hardware.hpp
@@ -78,10 +78,10 @@ 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 has_cancel_request() -> uint8_t final {
+        return cancel_request.exchange(0);
     }
-    void request_cancel() final { cancel_request.store(true); }
+    void request_cancel(uint8_t error_severity) final { cancel_request.store(error_severity); }
     auto get_usage_eeprom_config() -> const UsageEEpromConfig& final {
         return eeprom_config;
     }
@@ -98,7 +98,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<uint8_t> cancel_request = 0;
     const UsageEEpromConfig& eeprom_config;
     void* stopwatch_handle;
 };
diff --git a/include/motor-control/firmware/stepper_motor/motor_hardware.hpp b/include/motor-control/firmware/stepper_motor/motor_hardware.hpp
index 249764a04..bdd228095 100644
--- a/include/motor-control/firmware/stepper_motor/motor_hardware.hpp
+++ b/include/motor-control/firmware/stepper_motor/motor_hardware.hpp
@@ -54,13 +54,13 @@ class MotorHardware : public StepperMotorHardwareIface {
     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);
+    auto has_cancel_request() -> uint8_t final {
+        return cancel_request.exchange(0);
     }
     void disable_encoder() final;
     void enable_encoder() final;
 
-    void request_cancel() final { cancel_request.store(true); }
+    void request_cancel(uint8_t error_severity) final { cancel_request.store(error_severity); }
 
     auto get_usage_eeprom_config() -> const UsageEEpromConfig& final {
         return eeprom_config;
@@ -78,7 +78,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<uint8_t> cancel_request = 0;
     static constexpr uint32_t ENCODER_OVERFLOW_PULSES_BIT = 0x1 << 31;
 };
 
diff --git a/include/pipettes/core/dispatch_builder.hpp b/include/pipettes/core/dispatch_builder.hpp
index 9d4effde2..0acd404bf 100644
--- a/include/pipettes/core/dispatch_builder.hpp
+++ b/include/pipettes/core/dispatch_builder.hpp
@@ -38,6 +38,7 @@ using GearMotorDispatchTarget = can::dispatch::DispatchParseTarget<
     can::message_handlers::motor::GearMotorHandler<
         gear_motor_tasks::QueueClient>,
     can::messages::GearReadMotorDriverRegister,
+    can::messages::ReadMotorDriverErrorStatus,
     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..24fc368ad 100644
--- a/include/pipettes/core/gear_motor_tasks.hpp
+++ b/include/pipettes/core/gear_motor_tasks.hpp
@@ -71,7 +71,7 @@ struct QueueClient : can::message_writer::MessageWriter {
     void send_motion_controller_queue(
         const pipettes::tasks::motion_controller_task::TaskMessage& m);
 
-    void send_motor_driver_queue(const tmc2160::tasks::gear::TaskMessage& m);
+    void send_motor_driver_queue(const tmc2160::tasks::gear::TaskMessage& m); // const tmc::tasks::TaskMessage& m
 
     void send_move_group_queue(
         const pipettes::tasks::move_group_task::TaskMessage& m);
diff --git a/include/pipettes/core/linear_motor_tasks.hpp b/include/pipettes/core/linear_motor_tasks.hpp
index ae20e52d8..d6ac52de9 100644
--- a/include/pipettes/core/linear_motor_tasks.hpp
+++ b/include/pipettes/core/linear_motor_tasks.hpp
@@ -126,7 +126,7 @@ struct Tasks {
 /**
  * Queues related specifically to the the tmc2130 driver.
  */
-struct QueueClient : can::message_writer::MessageWriter {
+struct QueueClient : can::message_writer::MessageWriter { // USE THIS! See where/how it's used
     QueueClient()
         : can::message_writer::MessageWriter{can::ids::NodeId::pipette_left} {}
 
@@ -165,7 +165,7 @@ struct Tasks {
 /**
  * Queues related specifically to the the tmc2160 driver.
  */
-struct QueueClient : can::message_writer::MessageWriter {
+struct QueueClient : can::message_writer::MessageWriter { // AND THIS!
     QueueClient()
         : can::message_writer::MessageWriter{can::ids::NodeId::pipette_left} {}
 
diff --git a/include/pipettes/firmware/interfaces.hpp b/include/pipettes/firmware/interfaces.hpp
index 5f7c3c5db..b51aa717a 100644
--- a/include/pipettes/firmware/interfaces.hpp
+++ b/include/pipettes/firmware/interfaces.hpp
@@ -18,13 +18,13 @@
 
 namespace interfaces {
 
-template <typename Client>
+template <typename Client, typename DriverClient>
 using MotorInterruptHandlerType = motor_handler::MotorInterruptHandler<
-    freertos_message_queue::FreeRTOSMessageQueue, Client, motor_messages::Move,
+    freertos_message_queue::FreeRTOSMessageQueue, Client, DriverClient, motor_messages::Move,
     motor_hardware::MotorHardware>;
 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>
@@ -67,11 +67,11 @@ 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>; // how to accomodate 2130?
 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 fbeafd042..5f97c0d0d 100644
--- a/include/pipettes/firmware/interfaces_g4.hpp
+++ b/include/pipettes/firmware/interfaces_g4.hpp
@@ -27,13 +27,13 @@
 
 namespace interfaces {
 
-template <typename Client>
+template <typename Client, typename DriverClient>
 using MotorInterruptHandlerType = motor_handler::MotorInterruptHandler<
-    freertos_message_queue::FreeRTOSMessageQueue, Client, motor_messages::Move,
+    freertos_message_queue::FreeRTOSMessageQueue, Client, DriverClient, motor_messages::Move,
     motor_hardware::MotorHardware>;
 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>
@@ -76,11 +76,11 @@ 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>; // how to accomodate 2130 too?
 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/spi/core/messages.hpp b/include/spi/core/messages.hpp
index d96c74d5a..344b06b9e 100644
--- a/include/spi/core/messages.hpp
+++ b/include/spi/core/messages.hpp
@@ -20,7 +20,6 @@ struct TransactionIdentifier {
     uint8_t command_type;
     bool requires_response;
     uint32_t message_index;
-    bool error_response;
 
     auto operator==(const TransactionIdentifier& other) const -> bool = default;
 };
diff --git a/include/spi/core/utils.hpp b/include/spi/core/utils.hpp
index e41b4468f..47f8e6784 100644
--- a/include/spi/core/utils.hpp
+++ b/include/spi/core/utils.hpp
@@ -16,6 +16,36 @@ 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 { // utilize
+    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 { // utilize? If not, discard
+    return static_cast<RegType>(static_cast<uint8_t>(id & 0xff));
+}
 }  // namespace utils
 
 }  // namespace spi
\ No newline at end of file
diff --git a/include/spi/core/writer.hpp b/include/spi/core/writer.hpp
index 00693eb81..07b108291 100644
--- a/include/spi/core/writer.hpp
+++ b/include/spi/core/writer.hpp
@@ -46,17 +46,16 @@ class Writer {
      * @return A success boolean
      */
     template <OriginatingResponseQueue RQType>
-    auto read(uint8_t register_addr, uint32_t command_data,
+    auto read(uint32_t token, uint32_t command_data,
               RQType& response_queue, utils::ChipSelectInterface cs_intf,
-              uint32_t message_index, bool error_response) -> bool {
-        auto txBuffer = build_message(register_addr, spi::hardware::Mode::READ,
+              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,
-            .error_response = error_response};
+            .message_index = message_index};
         Transact message{
             .id = _transaction_id,
             .transaction = {.txBuffer = txBuffer, .cs_interface = cs_intf},
@@ -84,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/pipettes/core/gear_motor_tasks.cpp b/pipettes/core/gear_motor_tasks.cpp
index c5caabc5d..693d3a20f 100644
--- a/pipettes/core/gear_motor_tasks.cpp
+++ b/pipettes/core/gear_motor_tasks.cpp
@@ -129,7 +129,7 @@ void gear_motor_tasks::QueueClient::send_motion_controller_queue(
 }
 
 void gear_motor_tasks::QueueClient::send_motor_driver_queue(
-    const tmc2160::tasks::gear::TaskMessage& m) {
+    const tmc2160::tasks::gear::TaskMessage& m) { // const tmc::tasks::TaskMessage& m
     driver_queue->try_write(m);
 }
 
diff --git a/pipettes/firmware/interfaces.cpp b/pipettes/firmware/interfaces.cpp
index ff40c65ef..d963e270e 100644
--- a/pipettes/firmware/interfaces.cpp
+++ b/pipettes/firmware/interfaces.cpp
@@ -58,18 +58,18 @@ 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> { // how to accomodate 2130?
     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);
 }
 
@@ -130,10 +130,10 @@ auto gear_motor::get_interrupts(gear_motor::GearHardware& hw,
     -> gear_motor::GearInterruptHandlers {
     return gear_motor::GearInterruptHandlers{
         .left = motor_handler::MotorInterruptHandler(
-            queues.left_motor_queue, gear_motor_tasks::get_left_gear_queues(),
+            queues.left_motor_queue, gear_motor_tasks::get_left_gear_queues(), 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(),
+            queues.right_motor_queue, gear_motor_tasks::get_right_gear_queues(), gear_motor_tasks::get_left_gear_queues(),
             hw.right, stall.right, queues.right_update_queue)};
 }
 

From b8afae2055ae5ca1d006d8037eefcd2d072289a4 Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Thu, 12 Oct 2023 16:58:02 -0400
Subject: [PATCH 05/68] debug update

---
 include/motor-control/core/tasks/motion_controller_task.hpp | 4 +---
 1 file changed, 1 insertion(+), 3 deletions(-)

diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp
index ee98c6b5f..df738b2fc 100644
--- a/include/motor-control/core/tasks/motion_controller_task.hpp
+++ b/include/motor-control/core/tasks/motion_controller_task.hpp
@@ -216,8 +216,7 @@ class MotionControllerTask {
     // also create top level query msg
     void run_diag0_interrupt(void) {
         if (!driver_error_handled()) {
-            sent = true;
-            // static_cast<void>(queue.try_write_isr(can::messages::MotorDriverErrorEncountered{.message_index = 0}));
+            static_cast<void>(queue.try_write_isr(can::messages::MotorDriverErrorEncountered{.message_index = 0}));
         }
     }
 
@@ -226,7 +225,6 @@ class MotionControllerTask {
   private:
     QueueType& queue;
     std::atomic<bool> driver_error_handled_flag = false;
-    bool sent = false;
 };
 
 /**

From 16baac04434c310d94e63e1f325435646fb055d9 Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Thu, 12 Oct 2023 17:16:21 -0400
Subject: [PATCH 06/68] clean up

---
 .../core/tasks/motion_controller_task.hpp     | 23 ++++++++-----------
 1 file changed, 9 insertions(+), 14 deletions(-)

diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp
index df738b2fc..c21dc95a9 100644
--- a/include/motor-control/core/tasks/motion_controller_task.hpp
+++ b/include/motor-control/core/tasks/motion_controller_task.hpp
@@ -148,26 +148,21 @@ class MotionControllerMessageHandler {
     // cancel_and_clear_moves(can::ids::ErrorCode::motor_driver_error)?
     void handle(const can::messages::MotorDriverErrorEncountered& m) {
         // check if gpio is low before making controller calls
-        //if (!driver_error_handled()) {
-            controller.stop(can::ids::ErrorSeverity::unrecoverable);
-            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::hardware});
-                driver_client.send_motor_driver_queue(
-                    can::messages::ReadMotorDriverErrorStatus{.message_index = m.message_index});
-            }
-        //}
+        controller.stop(can::ids::ErrorSeverity::unrecoverable);
+        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::hardware});
+            driver_client.send_motor_driver_queue(
+                can::messages::ReadMotorDriverErrorStatus{.message_index = m.message_index});
+        }
     }
 
-    bool driver_error_handled() { return driver_error_handled_flag.exchange(true); }
-
     MotorControllerType& controller;
     CanClient& can_client;
     UsageClient& usage_client;
     DriverClient& driver_client;
-    std::atomic<bool> driver_error_handled_flag = false;
 };
 
 /**

From d2307beaa1550d91432a496b510313bc29edd1e2 Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Thu, 19 Oct 2023 16:47:16 -0400
Subject: [PATCH 07/68] cleanup and fixed tests

---
 gantry/core/tasks_rev1.cpp                     |  5 +----
 gantry/firmware/interfaces_proto.cpp           |  4 ++--
 gantry/firmware/interfaces_rev1.cpp            |  4 ++--
 gantry/firmware/motor_hardware.c               |  2 +-
 .../brushed_motor_interrupt_handler.hpp        |  3 +--
 .../core/motor_hardware_interface.hpp          |  2 +-
 .../core/stepper_motor/motion_controller.hpp   | 10 +++++-----
 .../stepper_motor/motor_interrupt_handler.hpp  |  5 ++---
 .../tasks/gear_tmc2160_motor_driver_task.hpp   |  3 +--
 .../core/tasks/motion_controller_task.hpp      |  4 +---
 .../core/tasks/tmc2130_motor_driver_task.hpp   |  2 +-
 .../core/tasks/tmc2160_motor_driver_task.hpp   |  5 +----
 .../tests/mock_brushed_motor_components.hpp    | 10 +++++-----
 .../tests/mock_motor_driver_client.hpp         | 18 ++++++++++++++++++
 .../tests/mock_motor_hardware.hpp              | 10 +++++-----
 include/pipettes/core/gear_motor_tasks.hpp     |  2 +-
 include/pipettes/core/linear_motor_tasks.hpp   |  4 ++--
 include/pipettes/firmware/interfaces.hpp       |  2 +-
 include/pipettes/firmware/interfaces_g4.hpp    |  2 +-
 include/spi/core/writer.hpp                    |  4 ++--
 motor-control/tests/test_limit_switch.cpp      |  7 +++++--
 .../tests/test_limit_switch_backoff.cpp        |  7 +++++--
 .../tests/test_motor_interrupt_handler.cpp     |  7 +++++--
 .../tests/test_motor_interrupt_queue.cpp       |  4 +++-
 motor-control/tests/test_motor_pulse.cpp       |  7 +++++--
 .../tests/test_motor_stall_handling.cpp        |  7 +++++--
 motor-control/tests/test_sync_handling.cpp     |  7 +++++--
 pipettes/core/gear_motor_tasks.cpp             |  2 +-
 pipettes/firmware/interfaces.cpp               |  2 +-
 29 files changed, 89 insertions(+), 62 deletions(-)
 create mode 100644 include/motor-control/tests/mock_motor_driver_client.hpp

diff --git a/gantry/core/tasks_rev1.cpp b/gantry/core/tasks_rev1.cpp
index da7c69db8..4c31ddb72 100644
--- a/gantry/core/tasks_rev1.cpp
+++ b/gantry/core/tasks_rev1.cpp
@@ -17,8 +17,6 @@ static auto queues = gantry::queues::QueueClient{utils::get_node_id()};
 
 static auto spi_task_client =
     spi::writer::Writer<freertos_message_queue::FreeRTOSMessageQueue>();
-//check how spi_task_client is used here
-//we want an interrupt from spi, see past work
 
 static auto mc_task_builder =
     freertos_task::TaskStarter<512,
@@ -32,7 +30,6 @@ static auto move_status_task_builder = freertos_task::TaskStarter<
 
 static auto spi_task_builder =
     freertos_task::TaskStarter<512, spi::tasks::Task>{};
-//look at this class's setup, functionality
 
 template <template <typename> typename QueueImpl>
 using PollerWithTimer =
@@ -78,7 +75,7 @@ auto gantry::tasks::start_tasks(
         ::queues);
 
     auto& spi_task = spi_task_builder.start(5, "spi task", spi_device);
-    spi_task_client.set_queue(&spi_task.get_queue()); // Writer class queue set here!
+    spi_task_client.set_queue(&spi_task.get_queue());
 
     auto& i2c2_task = i2c2_task_builder.start(5, "i2c2", i2c2);
     i2c2_task_client.set_queue(&i2c2_task.get_queue());
diff --git a/gantry/firmware/interfaces_proto.cpp b/gantry/firmware/interfaces_proto.cpp
index 8fac0992a..123f3e835 100644
--- a/gantry/firmware/interfaces_proto.cpp
+++ b/gantry/firmware/interfaces_proto.cpp
@@ -95,7 +95,7 @@ struct motion_controller::HardwareConfig motor_pins_x {
         // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
         .port = GPIOC,
         .pin = GPIO_PIN_5,
-        .active_setting = GPIO_PIN_RESET} //confirm RESET not SET. See Driver RM
+        .active_setting = GPIO_PIN_RESET}
 };
 
 struct motion_controller::HardwareConfig motor_pins_y {
@@ -139,7 +139,7 @@ struct motion_controller::HardwareConfig motor_pins_y {
         // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
         .port = GPIOC,
         .pin = GPIO_PIN_5,
-        .active_setting = GPIO_PIN_RESET} //confirm RESET not SET. See Driver RM
+        .active_setting = GPIO_PIN_RESET}
 };
 
 static tmc2130::configs::TMC2130DriverConfig gantry_x_driver_configs{
diff --git a/gantry/firmware/interfaces_rev1.cpp b/gantry/firmware/interfaces_rev1.cpp
index 338ae5935..cb0f96a22 100644
--- a/gantry/firmware/interfaces_rev1.cpp
+++ b/gantry/firmware/interfaces_rev1.cpp
@@ -95,7 +95,7 @@ struct motion_controller::HardwareConfig motor_pins_x {
         // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
         .port = GPIOC,
         .pin = GPIO_PIN_5,
-        .active_setting = GPIO_PIN_RESET} //confirm RESET not SET. See Driver RM
+        .active_setting = GPIO_PIN_RESET}
 };
 
 struct motion_controller::HardwareConfig motor_pins_y {
@@ -139,7 +139,7 @@ struct motion_controller::HardwareConfig motor_pins_y {
         // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
         .port = GPIOC,
         .pin = GPIO_PIN_5,
-        .active_setting = GPIO_PIN_RESET} //confirm RESET not SET. See Driver RM
+        .active_setting = GPIO_PIN_RESET}
 };
 
 static tmc2160::configs::TMC2160DriverConfig motor_driver_config_x{
diff --git a/gantry/firmware/motor_hardware.c b/gantry/firmware/motor_hardware.c
index 1328764c9..f2f83eb9f 100644
--- a/gantry/firmware/motor_hardware.c
+++ b/gantry/firmware/motor_hardware.c
@@ -60,7 +60,7 @@ void HAL_SPI_MspInit(SPI_HandleTypeDef* hspi) {
         // Diag0
         GPIO_InitStruct.Pin = GPIO_PIN_5;
         GPIO_InitStruct.Mode = GPIO_MODE_IT_FALLING;
-        GPIO_InitStruct.Pull = GPIO_PULLUP; // confirm correct
+        GPIO_InitStruct.Pull = GPIO_PULLUP;
         HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
     }
 }
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 9a3d084e2..dbd3824df 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
@@ -196,8 +196,7 @@ class BrushedMotorInterruptHandler {
             cancel_and_clear_moves(can::ids::ErrorCode::estop_detected);
             motor_state = ControlState::ESTOP;
         } else if (hardware.has_cancel_request()) {
-            cancel_and_clear_moves();
-            // cancel_and_clear_moves(can::ids::ErrorCode::stop_requested, can::ids::ErrorSeverity::warning);
+            cancel_and_clear_moves(can::ids::ErrorCode::stop_requested, can::ids::ErrorSeverity::warning);
             motor_state = ControlState::IDLE;
         } else if (tick < HOLDOFF_TICKS) {
             tick++;
diff --git a/include/motor-control/core/motor_hardware_interface.hpp b/include/motor-control/core/motor_hardware_interface.hpp
index 0e81b3556..60eace601 100644
--- a/include/motor-control/core/motor_hardware_interface.hpp
+++ b/include/motor-control/core/motor_hardware_interface.hpp
@@ -99,7 +99,7 @@ class MotorHardwareIface {
     virtual void disable_encoder() = 0;
 
     virtual auto has_cancel_request() -> uint8_t = 0;
-    virtual void request_cancel(uint8_t error) = 0;
+    virtual void request_cancel(uint8_t error_severity) = 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 ff96c64a4..901c476a9 100644
--- a/include/motor-control/core/stepper_motor/motion_controller.hpp
+++ b/include/motor-control/core/stepper_motor/motion_controller.hpp
@@ -108,10 +108,10 @@ class MotionController {
     }
 
     void stop(can::ids::ErrorSeverity error_severity = can::ids::ErrorSeverity::warning) {
-        queue.reset(); // empties queue
+        queue.reset();
         disable_motor();
         if (hardware.is_timer_interrupt_running()) {
-            hardware.request_cancel(static_cast<uint8_t>(error_severity)); // use ErrorSeverity, converted to uint_8! // see if/how checked. Calls cancel_and_clear_moves!
+            hardware.request_cancel(static_cast<uint8_t>(error_severity));
         }
     }
 
@@ -140,10 +140,10 @@ class MotionController {
     }
 
     void disable_motor() {
-        hardware.deactivate_motor(); // sets e-stop pin and resets enable pin
+        hardware.deactivate_motor();
         hardware.position_flags.clear_flag(
             can::ids::MotorPositionFlags::stepper_position_ok);
-        enabled = false; // see if/how checked
+        enabled = false;
     }
 
     void set_motion_constraints(
@@ -265,7 +265,7 @@ class PipetteMotionController {
         // 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()) { // should always be true, right?
+        if (hardware.is_timer_interrupt_running()) {
             hardware.request_cancel(static_cast<uint8_t>(error_severity));
         }
     }
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 1b62a81f7..68f9f15af 100644
--- a/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp
+++ b/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp
@@ -200,7 +200,7 @@ class MotorInterruptHandler {
         } else if (estop_triggered()) {
             cancel_and_clear_moves(can::ids::ErrorCode::estop_detected);
             in_estop = true;
-        } else if (has_cancel_request) { // consider stall detected error. Don't see issue
+        } else if (has_cancel_request) {
             if (has_cancel_request == static_cast<uint8_t>(can::ids::ErrorSeverity::unrecoverable)) {
                 cancel_and_clear_moves();
             } else {
@@ -406,7 +406,7 @@ class MotorInterruptHandler {
     [[nodiscard]] auto set_direction_pin() const -> bool {
         return (buffered_move.velocity > 0);
     }
-    void cancel_and_clear_moves( // use this! Change input args
+    void cancel_and_clear_moves(
         can::ids::ErrorCode err_code = can::ids::ErrorCode::hardware,
         can::ids::ErrorSeverity severity =
             can::ids::ErrorSeverity::unrecoverable) {
@@ -421,7 +421,6 @@ class MotorInterruptHandler {
             can::messages::ErrorMessage{.message_index = message_index,
                                         .severity = severity,
                                         .error_code = err_code});
-        // only send when motor driver error
         if (err_code == can::ids::ErrorCode::hardware) {
             driver_client.send_motor_driver_queue(
                 can::messages::ReadMotorDriverErrorStatus{.message_index = message_index});
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 406914e06..ed6db0362 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
@@ -88,9 +88,8 @@ class MotorDriverMessageHandler {
         }
     }
 
-    // see notes re:this in tmc2160_motor_driver_task
     void handle(const can::messages::ReadMotorDriverErrorStatus& m) {
-        // LOG?
+        LOG("Received read motor driver error register request");
         uint32_t data = 0;
         auto converted_addr = static_cast<uint8_t>(tmc2160::registers::Registers::DRVSTATUS);
         std::array tags{spi::utils::ResponseTag::IS_ERROR_RESPONSE};
diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp
index c21dc95a9..c1acd2aaa 100644
--- a/include/motor-control/core/tasks/motion_controller_task.hpp
+++ b/include/motor-control/core/tasks/motion_controller_task.hpp
@@ -144,8 +144,6 @@ class MotionControllerMessageHandler {
         controller.send_usage_data(m.message_index, usage_client);
     }
 
-    // set error flag here (C++ atomic)?! Disable subsystem (enable brakes)?!
-    // cancel_and_clear_moves(can::ids::ErrorCode::motor_driver_error)?
     void handle(const can::messages::MotorDriverErrorEncountered& m) {
         // check if gpio is low before making controller calls
         controller.stop(can::ids::ErrorSeverity::unrecoverable);
@@ -153,7 +151,7 @@ class MotionControllerMessageHandler {
             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::hardware});
+                                            .error_code = can::ids::ErrorCode::hardware}); // make this motor_driver_error instead of hardware?
             driver_client.send_motor_driver_queue(
                 can::messages::ReadMotorDriverErrorStatus{.message_index = m.message_index});
         }
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 3d3ffd3b8..c8450b1b5 100644
--- a/include/motor-control/core/tasks/tmc2130_motor_driver_task.hpp
+++ b/include/motor-control/core/tasks/tmc2130_motor_driver_task.hpp
@@ -88,7 +88,7 @@ class MotorDriverMessageHandler {
     }
 
     void handle(const can::messages::ReadMotorDriverErrorStatus& m) {
-        // LOG?
+        LOG("Received read motor driver error register request");
         uint32_t data = 0;
         auto converted_addr = static_cast<uint8_t>(tmc2130::registers::Registers::DRVSTATUS);
         std::array tags{spi::utils::ResponseTag::IS_ERROR_RESPONSE};
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 3122cf950..933fca708 100644
--- a/include/motor-control/core/tasks/tmc2160_motor_driver_task.hpp
+++ b/include/motor-control/core/tasks/tmc2160_motor_driver_task.hpp
@@ -96,11 +96,8 @@ class MotorDriverMessageHandler {
         }
     }
 
-    // need a response handler? Yes, send response once when interrupt and twice when response received
-    // ensure specific error is returned
-    // just use above method?!
     void handle(const can::messages::ReadMotorDriverErrorStatus& m) {
-        // LOG?
+        LOG("Received read motor driver error register request");
         uint32_t data = 0;
         auto converted_addr = static_cast<uint8_t>(tmc2160::registers::Registers::DRVSTATUS);
         std::array tags{spi::utils::ResponseTag::IS_ERROR_RESPONSE};
diff --git a/include/motor-control/tests/mock_brushed_motor_components.hpp b/include/motor-control/tests/mock_brushed_motor_components.hpp
index dcc47eb8b..6ddc8f97a 100644
--- a/include/motor-control/tests/mock_brushed_motor_components.hpp
+++ b/include/motor-control/tests/mock_brushed_motor_components.hpp
@@ -71,12 +71,12 @@ 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 has_cancel_request() -> uint8_t final {
+        uint8_t old_request = cancel_request;
+        cancel_request = 0;
         return old_request;
     }
-    void request_cancel() final { cancel_request = true; }
+    void request_cancel(uint8_t error_severity) final { cancel_request = error_severity; }
     void set_timer_interrupt_running(bool is_running) {
         timer_interrupt_running = is_running;
     }
@@ -104,7 +104,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;
+    uint8_t cancel_request = 0;
     bool timer_interrupt_running = true;
     motor_hardware::UsageEEpromConfig eeprom_config =
         motor_hardware::UsageEEpromConfig{
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..787d87d7e
--- /dev/null
+++ b/include/motor-control/tests/mock_motor_driver_client.hpp
@@ -0,0 +1,18 @@
+#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);
+    }
+
+    std::vector<tmc::tasks::TaskMessage> messages{};
+};
+
+}  // namespace test_mocks
\ No newline at end of file
diff --git a/include/motor-control/tests/mock_motor_hardware.hpp b/include/motor-control/tests/mock_motor_hardware.hpp
index ff6e0bd69..f4d855487 100644
--- a/include/motor-control/tests/mock_motor_hardware.hpp
+++ b/include/motor-control/tests/mock_motor_hardware.hpp
@@ -39,12 +39,12 @@ 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 has_cancel_request() -> uint8_t final {
+        uint8_t old_request = cancel_request;
+        cancel_request = 0;
         return old_request;
     }
-    void request_cancel() final { cancel_request = true; }
+    void request_cancel(uint8_t error_severity) final { cancel_request = error_severity; }
     void sim_set_timer_interrupt_running(bool is_running) {
         mock_timer_interrupt_running = is_running;
     }
@@ -65,7 +65,7 @@ class MockMotorHardware : public motor_hardware::StepperMotorHardwareIface {
     bool mock_dir_value = false;
     uint8_t finished_move_id = 0x0;
     int32_t test_pulses = 0x0;
-    bool cancel_request = false;
+    uint8_t cancel_request = 0;
     bool mock_timer_interrupt_running = true;
     motor_hardware::UsageEEpromConfig eeprom_config =
         motor_hardware::UsageEEpromConfig{
diff --git a/include/pipettes/core/gear_motor_tasks.hpp b/include/pipettes/core/gear_motor_tasks.hpp
index 24fc368ad..292f73b4d 100644
--- a/include/pipettes/core/gear_motor_tasks.hpp
+++ b/include/pipettes/core/gear_motor_tasks.hpp
@@ -71,7 +71,7 @@ struct QueueClient : can::message_writer::MessageWriter {
     void send_motion_controller_queue(
         const pipettes::tasks::motion_controller_task::TaskMessage& m);
 
-    void send_motor_driver_queue(const tmc2160::tasks::gear::TaskMessage& m); // const tmc::tasks::TaskMessage& m
+    void send_motor_driver_queue(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/linear_motor_tasks.hpp b/include/pipettes/core/linear_motor_tasks.hpp
index d6ac52de9..ae20e52d8 100644
--- a/include/pipettes/core/linear_motor_tasks.hpp
+++ b/include/pipettes/core/linear_motor_tasks.hpp
@@ -126,7 +126,7 @@ struct Tasks {
 /**
  * Queues related specifically to the the tmc2130 driver.
  */
-struct QueueClient : can::message_writer::MessageWriter { // USE THIS! See where/how it's used
+struct QueueClient : can::message_writer::MessageWriter {
     QueueClient()
         : can::message_writer::MessageWriter{can::ids::NodeId::pipette_left} {}
 
@@ -165,7 +165,7 @@ struct Tasks {
 /**
  * Queues related specifically to the the tmc2160 driver.
  */
-struct QueueClient : can::message_writer::MessageWriter { // AND THIS!
+struct QueueClient : can::message_writer::MessageWriter {
     QueueClient()
         : can::message_writer::MessageWriter{can::ids::NodeId::pipette_left} {}
 
diff --git a/include/pipettes/firmware/interfaces.hpp b/include/pipettes/firmware/interfaces.hpp
index b51aa717a..e9586c78b 100644
--- a/include/pipettes/firmware/interfaces.hpp
+++ b/include/pipettes/firmware/interfaces.hpp
@@ -67,7 +67,7 @@ namespace linear_motor {
 auto get_interrupt(motor_hardware::MotorHardware& hw,
                    LowThroughputInterruptQueues& queues,
                    stall_check::StallCheck& stall)
-    -> MotorInterruptHandlerType<linear_motor_tasks::QueueClient, linear_motor_tasks::tmc2130_driver::QueueClient>; // how to accomodate 2130?
+    -> MotorInterruptHandlerType<linear_motor_tasks::QueueClient, linear_motor_tasks::tmc2130_driver::QueueClient>;
 auto get_interrupt(motor_hardware::MotorHardware& hw,
                    HighThroughputInterruptQueues& queues,
                    stall_check::StallCheck& stall)
diff --git a/include/pipettes/firmware/interfaces_g4.hpp b/include/pipettes/firmware/interfaces_g4.hpp
index 5f97c0d0d..cc31615e6 100644
--- a/include/pipettes/firmware/interfaces_g4.hpp
+++ b/include/pipettes/firmware/interfaces_g4.hpp
@@ -76,7 +76,7 @@ namespace linear_motor {
 auto get_interrupt(motor_hardware::MotorHardware& hw,
                    LowThroughputInterruptQueues& queues,
                    stall_check::StallCheck& stall)
-    -> MotorInterruptHandlerType<linear_motor_tasks::QueueClient, linear_motor_tasks::tmc2130_driver::QueueClient>; // how to accomodate 2130 too?
+    -> MotorInterruptHandlerType<linear_motor_tasks::QueueClient, linear_motor_tasks::tmc2130_driver::QueueClient>;
 auto get_interrupt(motor_hardware::MotorHardware& hw,
                    HighThroughputInterruptQueues& queues,
                    stall_check::StallCheck& stall)
diff --git a/include/spi/core/writer.hpp b/include/spi/core/writer.hpp
index 07b108291..0fd6661c1 100644
--- a/include/spi/core/writer.hpp
+++ b/include/spi/core/writer.hpp
@@ -60,10 +60,10 @@ class Writer {
             .id = _transaction_id,
             .transaction = {.txBuffer = txBuffer, .cs_interface = cs_intf},
             .response_writer = ResponseWriter(response_queue)};
-        queue->try_write(message); // this is a FreeRTOSMessageQueue
+        queue->try_write(message);
 
         message.id.requires_response = true;
-        return queue->try_write(message); // why twice? transmit and receive
+        return queue->try_write(message);
     }
 
     /**
diff --git a/motor-control/tests/test_limit_switch.cpp b/motor-control/tests/test_limit_switch.cpp
index 2413fa499..717b2229e 100644
--- a/motor-control/tests/test_limit_switch.cpp
+++ b/motor-control/tests/test_limit_switch.cpp
@@ -4,6 +4,7 @@
 #include "motor-control/core/stepper_motor/motor_interrupt_handler.hpp"
 #include "motor-control/tests/mock_motor_hardware.hpp"
 #include "motor-control/tests/mock_move_status_reporter_client.hpp"
+#include "motor-control/tests/mock_motor_driver_client.hpp"
 
 using namespace motor_handler;
 
@@ -16,11 +17,13 @@ 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::MockMoveStatusReporterClient, 
+                          test_mocks::MockMotorDriverClient, Move,
                           test_mocks::MockMotorHardware>
-        handler{queue, reporter, hw, stall, update_position_queue};
+        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..16c6d3166 100644
--- a/motor-control/tests/test_limit_switch_backoff.cpp
+++ b/motor-control/tests/test_limit_switch_backoff.cpp
@@ -4,6 +4,7 @@
 #include "motor-control/core/stepper_motor/motor_interrupt_handler.hpp"
 #include "motor-control/tests/mock_motor_hardware.hpp"
 #include "motor-control/tests/mock_move_status_reporter_client.hpp"
+#include "motor-control/tests/mock_motor_driver_client.hpp"
 
 using namespace motor_handler;
 
@@ -16,11 +17,13 @@ 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::MockMoveStatusReporterClient, 
+                          test_mocks::MockMotorDriverClient, Move,
                           test_mocks::MockMotorHardware>
-        handler{queue, reporter, hw, stall, update_position_queue};
+        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..f0eb679d2 100644
--- a/motor-control/tests/test_motor_interrupt_handler.cpp
+++ b/motor-control/tests/test_motor_interrupt_handler.cpp
@@ -5,6 +5,7 @@
 #include "motor-control/core/stepper_motor/motor_interrupt_handler.hpp"
 #include "motor-control/tests/mock_motor_hardware.hpp"
 #include "motor-control/tests/mock_move_status_reporter_client.hpp"
+#include "motor-control/tests/mock_motor_driver_client.hpp"
 
 using namespace motor_handler;
 
@@ -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,7 @@ 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.request_cancel(1);
             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..c16254c46 100644
--- a/motor-control/tests/test_motor_interrupt_queue.cpp
+++ b/motor-control/tests/test_motor_interrupt_queue.cpp
@@ -4,6 +4,7 @@
 #include "motor-control/core/stepper_motor/motor_interrupt_handler.hpp"
 #include "motor-control/tests/mock_motor_hardware.hpp"
 #include "motor-control/tests/mock_move_status_reporter_client.hpp"
+#include "motor-control/tests/mock_motor_driver_client.hpp"
 
 using namespace motor_handler;
 
@@ -15,9 +16,10 @@ 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,
+        auto handler = MotorInterruptHandler(queue, reporter, driver, hardware, stall,
                                              update_position_queue);
 
         WHEN("add multiple moves to the queue") {
diff --git a/motor-control/tests/test_motor_pulse.cpp b/motor-control/tests/test_motor_pulse.cpp
index 856edaad1..b625a9a24 100644
--- a/motor-control/tests/test_motor_pulse.cpp
+++ b/motor-control/tests/test_motor_pulse.cpp
@@ -3,6 +3,7 @@
 #include "motor-control/core/stepper_motor/motor_interrupt_handler.hpp"
 #include "motor-control/tests/mock_motor_hardware.hpp"
 #include "motor-control/tests/mock_move_status_reporter_client.hpp"
+#include "motor-control/tests/mock_motor_driver_client.hpp"
 
 using namespace motor_handler;
 
@@ -21,11 +22,13 @@ 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::MockMoveStatusReporterClient, 
+                          test_mocks::MockMotorDriverClient, Move,
                           test_mocks::MockMotorHardware>
-        handler{queue, reporter, hw, stall, update_position_queue};
+        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..c2893e763 100644
--- a/motor-control/tests/test_motor_stall_handling.cpp
+++ b/motor-control/tests/test_motor_stall_handling.cpp
@@ -4,6 +4,7 @@
 #include "motor-control/core/usage_messages.hpp"
 #include "motor-control/tests/mock_motor_hardware.hpp"
 #include "motor-control/tests/mock_move_status_reporter_client.hpp"
+#include "motor-control/tests/mock_motor_driver_client.hpp"
 
 using namespace motor_handler;
 
@@ -21,11 +22,13 @@ 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::MockMoveStatusReporterClient, 
+                          test_mocks::MockMotorDriverClient, Move,
                           test_mocks::MockMotorHardware>
-        handler{queue, reporter, hw, stall, update_position_queue};
+        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..37d4a65dc 100644
--- a/motor-control/tests/test_sync_handling.cpp
+++ b/motor-control/tests/test_sync_handling.cpp
@@ -4,6 +4,7 @@
 #include "motor-control/core/stepper_motor/motor_interrupt_handler.hpp"
 #include "motor-control/tests/mock_motor_hardware.hpp"
 #include "motor-control/tests/mock_move_status_reporter_client.hpp"
+#include "motor-control/tests/mock_motor_driver_client.hpp"
 
 using namespace motor_handler;
 
@@ -16,11 +17,13 @@ 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::MockMoveStatusReporterClient, 
+                          test_mocks::MockMotorDriverClient, Move,
                           test_mocks::MockMotorHardware>
-        handler{queue, reporter, hw, stall, update_position_queue};
+        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 693d3a20f..c5caabc5d 100644
--- a/pipettes/core/gear_motor_tasks.cpp
+++ b/pipettes/core/gear_motor_tasks.cpp
@@ -129,7 +129,7 @@ void gear_motor_tasks::QueueClient::send_motion_controller_queue(
 }
 
 void gear_motor_tasks::QueueClient::send_motor_driver_queue(
-    const tmc2160::tasks::gear::TaskMessage& m) { // const tmc::tasks::TaskMessage& m
+    const tmc2160::tasks::gear::TaskMessage& m) {
     driver_queue->try_write(m);
 }
 
diff --git a/pipettes/firmware/interfaces.cpp b/pipettes/firmware/interfaces.cpp
index d963e270e..5a8a22a43 100644
--- a/pipettes/firmware/interfaces.cpp
+++ b/pipettes/firmware/interfaces.cpp
@@ -58,7 +58,7 @@ 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, linear_motor_tasks::tmc2130_driver::QueueClient> { // how to accomodate 2130?
+    -> MotorInterruptHandlerType<linear_motor_tasks::QueueClient, linear_motor_tasks::tmc2130_driver::QueueClient> {
     return motor_handler::MotorInterruptHandler(
         queues.plunger_queue, linear_motor_tasks::get_queues(), linear_motor_tasks::tmc2130_driver::get_queues(), hw, stall,
         queues.plunger_update_queue);

From f3f224bb487f9fcf4fc98907482ef3fa8096cded Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Wed, 25 Oct 2023 18:19:04 -0400
Subject: [PATCH 08/68] test and simulator build changes

---
 gantry/simulator/interfaces.cpp               |  6 ++-
 gantry/simulator/main.cpp                     |  6 ++-
 gripper/simulator/interfaces.cpp              |  2 +-
 head/simulator/main.cpp                       |  4 +-
 .../simulation/motor_interrupt_driver.hpp     |  4 +-
 .../simulation/sim_motor_hardware_iface.hpp   | 24 +++++-----
 include/pipettes/simulator/interfaces.hpp     | 37 ++++++++++-----
 pipettes/simulator/interfaces.cpp             | 47 ++++++++++++++-----
 pipettes/simulator/main.cpp                   |  6 +--
 sensors/tests/CMakeLists.txt                  |  2 +-
 10 files changed, 87 insertions(+), 51 deletions(-)

diff --git a/gantry/simulator/interfaces.cpp b/gantry/simulator/interfaces.cpp
index f1a59f0bd..e7a8a8f56 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,
+    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/simulator/interfaces.cpp b/gripper/simulator/interfaces.cpp
index 3f7374a71..13469ba52 100644
--- a/gripper/simulator/interfaces.cpp
+++ b/gripper/simulator/interfaces.cpp
@@ -94,7 +94,7 @@ 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,
+    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,
diff --git a/head/simulator/main.cpp b/head/simulator/main.cpp
index 79db8f736..bb2c03b63 100644
--- a/head/simulator/main.cpp
+++ b/head/simulator/main.cpp
@@ -94,7 +94,7 @@ 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,
+    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(
@@ -117,7 +117,7 @@ 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,
+    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{
diff --git a/include/motor-control/simulation/motor_interrupt_driver.hpp b/include/motor-control/simulation/motor_interrupt_driver.hpp
index f7dbb193d..a649252bf 100644
--- a/include/motor-control/simulation/motor_interrupt_driver.hpp
+++ b/include/motor-control/simulation/motor_interrupt_driver.hpp
@@ -9,7 +9,7 @@
 
 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>;
@@ -17,7 +17,7 @@ class MotorInterruptDriver {
         freertos_message_queue::FreeRTOSMessageQueue<
             can::messages::UpdateMotorPositionEstimationRequest>;
     using InterruptHandler = motor_handler::MotorInterruptHandler<
-        freertos_message_queue::FreeRTOSMessageQueue, StatusClient,
+        freertos_message_queue::FreeRTOSMessageQueue, StatusClient, DriverClient,
         MotorMoveMessage, MotorHardware>;
 
   public:
diff --git a/include/motor-control/simulation/sim_motor_hardware_iface.hpp b/include/motor-control/simulation/sim_motor_hardware_iface.hpp
index 2a666a95f..3174648fd 100644
--- a/include/motor-control/simulation/sim_motor_hardware_iface.hpp
+++ b/include/motor-control/simulation/sim_motor_hardware_iface.hpp
@@ -87,10 +87,10 @@ 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 has_cancel_request() -> uint8_t final {
+        return cancel_request.exchange(0);
     }
-    void request_cancel() final { cancel_request.store(true); }
+    void request_cancel(uint8_t error_severity) final { cancel_request.store(error_severity); }
     auto get_usage_eeprom_config()
         -> motor_hardware::UsageEEpromConfig & final {
         return eeprom_config;
@@ -106,7 +106,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<uint8_t> cancel_request = 0;
     motor_hardware::UsageEEpromConfig eeprom_config = {
         std::array<UsageRequestSet, 1>{UsageRequestSet{
             .eeprom_key = 0,
@@ -177,10 +177,10 @@ class SimBrushedMotorHardwareIface
         return ret;
     }
 
-    auto has_cancel_request() -> bool final {
-        return cancel_request.exchange(false);
+    auto has_cancel_request() -> uint8_t final {
+        return cancel_request.exchange(0);
     }
-    void request_cancel() final { cancel_request.store(true); }
+    void request_cancel(uint8_t error_severity) final { cancel_request.store(error_severity); }
 
     void disable_encoder() final {}
     void enable_encoder() final {}
@@ -198,7 +198,7 @@ class SimBrushedMotorHardwareIface
     StateManagerHandle _state_manager = nullptr;
     MoveMessageHardware _id;
     bool estop_detected = false;
-    std::atomic<bool> cancel_request = false;
+    std::atomic<uint8_t> cancel_request = 0;
     motor_hardware::UsageEEpromConfig eeprom_config{
         std::array<UsageRequestSet, 1>{UsageRequestSet{
             .eeprom_key = 0,
@@ -263,10 +263,10 @@ 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 has_cancel_request() -> uint8_t final {
+        return cancel_request.exchange(0);
     }
-    void request_cancel() final { cancel_request.store(true); }
+    void request_cancel(uint8_t error_severity) final { cancel_request.store(error_severity); }
 
     auto get_usage_eeprom_config()
         -> motor_hardware::UsageEEpromConfig & final {
@@ -283,7 +283,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<uint8_t> cancel_request = 0;
     motor_hardware::UsageEEpromConfig eeprom_config = {
         std::array<UsageRequestSet, 1>{UsageRequestSet{
             .eeprom_key = 0,
diff --git a/include/pipettes/simulator/interfaces.hpp b/include/pipettes/simulator/interfaces.hpp
index b646bf55e..5826db532 100644
--- a/include/pipettes/simulator/interfaces.hpp
+++ b/include/pipettes/simulator/interfaces.hpp
@@ -12,14 +12,14 @@
 
 namespace interfaces {
 
-template <typename Client>
+template <typename Client, typename DriverClient>
 using MotorInterruptHandlerType = motor_handler::MotorInterruptHandler<
-    freertos_message_queue::FreeRTOSMessageQueue, Client, motor_messages::Move,
+    freertos_message_queue::FreeRTOSMessageQueue, Client, DriverClient, motor_messages::Move,
     sim_motor_hardware_iface::SimMotorHardwareIface>;
 
 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>;
 
@@ -61,16 +61,29 @@ auto get_interrupt_queues<PipetteType::THREE_EIGHTY_FOUR_CHANNEL>()
 namespace linear_motor {
 
 auto get_interrupt(sim_motor_hardware_iface::SimMotorHardwareIface& hw,
-                   MoveQueue& queue, stall_check::StallCheck& stall,
-                   UpdatePositionQueue& update_queue)
-    -> MotorInterruptHandlerType<linear_motor_tasks::QueueClient>;
+                                 LowThroughputInterruptQueues& queues,
+                                 stall_check::StallCheck& stall)
+    -> 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, linear_motor_tasks::tmc2160_driver::QueueClient>;
+
+auto get_interrupt_driver(
+    sim_motor_hardware_iface::SimMotorHardwareIface& hw,
+    LowThroughputInterruptQueues& queues,
+    MotorInterruptHandlerType<linear_motor_tasks::QueueClient, linear_motor_tasks::tmc2130_driver::QueueClient>& handler)
+    -> motor_interrupt_driver::MotorInterruptDriver<
+        linear_motor_tasks::QueueClient, linear_motor_tasks::tmc2130_driver::QueueClient, motor_messages::Move,
+        sim_motor_hardware_iface::SimMotorHardwareIface>;
 
 auto get_interrupt_driver(
-    sim_motor_hardware_iface::SimMotorHardwareIface& hw, MoveQueue& queue,
-    MotorInterruptHandlerType<linear_motor_tasks::QueueClient>& handler,
-    UpdatePositionQueue& update_queue)
+    sim_motor_hardware_iface::SimMotorHardwareIface& hw,
+    HighThroughputInterruptQueues& queues,
+    MotorInterruptHandlerType<linear_motor_tasks::QueueClient, linear_motor_tasks::tmc2160_driver::QueueClient>& handler)
     -> motor_interrupt_driver::MotorInterruptDriver<
-        linear_motor_tasks::QueueClient, motor_messages::Move,
+        linear_motor_tasks::QueueClient, linear_motor_tasks::tmc2160_driver::QueueClient, motor_messages::Move,
         sim_motor_hardware_iface::SimMotorHardwareIface>;
 
 auto get_motor_hardware() -> sim_motor_hardware_iface::SimMotorHardwareIface;
@@ -93,11 +106,11 @@ 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/pipettes/simulator/interfaces.cpp b/pipettes/simulator/interfaces.cpp
index e90b7356b..960dd9c69 100644
--- a/pipettes/simulator/interfaces.cpp
+++ b/pipettes/simulator/interfaces.cpp
@@ -44,23 +44,44 @@ auto interfaces::get_interrupt_queues<PipetteType::THREE_EIGHTY_FOUR_CHANNEL>()
         .left_update_queue = UpdatePositionQueue{"Left PUpdate Queue"}};
 }
 
-auto linear_motor::get_interrupt(
-    sim_motor_hardware_iface::SimMotorHardwareIface& hw, MoveQueue& queue,
-    stall_check::StallCheck& stall, UpdatePositionQueue& update_queue)
-    -> MotorInterruptHandlerType<linear_motor_tasks::QueueClient> {
+auto linear_motor::get_interrupt(sim_motor_hardware_iface::SimMotorHardwareIface& hw,
+                                 LowThroughputInterruptQueues& queues,
+                                 stall_check::StallCheck& stall)
+    -> MotorInterruptHandlerType<linear_motor_tasks::QueueClient, linear_motor_tasks::tmc2130_driver::QueueClient> {
     return motor_handler::MotorInterruptHandler(
-        queue, linear_motor_tasks::get_queues(), hw, stall, update_queue);
+        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, linear_motor_tasks::tmc2160_driver::QueueClient> {
+    return motor_handler::MotorInterruptHandler(
+        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, linear_motor_tasks::tmc2130_driver::QueueClient>& handler)
+    -> motor_interrupt_driver::MotorInterruptDriver<
+        linear_motor_tasks::QueueClient, linear_motor_tasks::tmc2130_driver::QueueClient, motor_messages::Move,
+        sim_motor_hardware_iface::SimMotorHardwareIface> {
+    return motor_interrupt_driver::MotorInterruptDriver(queues.plunger_queue, handler, hw,
+                                                        queues.plunger_update_queue);
 }
 
 auto linear_motor::get_interrupt_driver(
-    sim_motor_hardware_iface::SimMotorHardwareIface& hw, MoveQueue& queue,
-    MotorInterruptHandlerType<linear_motor_tasks::QueueClient>& handler,
-    UpdatePositionQueue& update_queue)
+    sim_motor_hardware_iface::SimMotorHardwareIface& hw,
+    HighThroughputInterruptQueues& queues,
+    MotorInterruptHandlerType<linear_motor_tasks::QueueClient, linear_motor_tasks::tmc2160_driver::QueueClient>& handler)
     -> motor_interrupt_driver::MotorInterruptDriver<
-        linear_motor_tasks::QueueClient, motor_messages::Move,
+        linear_motor_tasks::QueueClient, linear_motor_tasks::tmc2160_driver::QueueClient, motor_messages::Move,
         sim_motor_hardware_iface::SimMotorHardwareIface> {
-    return motor_interrupt_driver::MotorInterruptDriver(queue, handler, hw,
-                                                        update_queue);
+    return motor_interrupt_driver::MotorInterruptDriver(queues.plunger_queue, handler, hw,
+                                                        queues.plunger_update_queue);
 }
 
 auto linear_motor::get_motor_hardware()
@@ -103,10 +124,10 @@ 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/pipettes/simulator/main.cpp b/pipettes/simulator/main.cpp
index 07786f48a..6dd209d03 100644
--- a/pipettes/simulator/main.cpp
+++ b/pipettes/simulator/main.cpp
@@ -59,12 +59,10 @@ static auto gear_stall_check = interfaces::gear_motor::GearStallCheck{
 static auto linear_motor_hardware =
     interfaces::linear_motor::get_motor_hardware();
 static auto plunger_interrupt = interfaces::linear_motor::get_interrupt(
-    linear_motor_hardware, interrupt_queues.plunger_queue, linear_stall_check,
-    interrupt_queues.plunger_update_queue);
+    linear_motor_hardware, interrupt_queues, linear_stall_check);
 static auto plunger_interrupt_driver =
     interfaces::linear_motor::get_interrupt_driver(
-        linear_motor_hardware, interrupt_queues.plunger_queue,
-        plunger_interrupt, interrupt_queues.plunger_update_queue);
+        linear_motor_hardware, interrupt_queues, plunger_interrupt);
 static auto linear_motion_control =
     interfaces::linear_motor::get_motion_control(linear_motor_hardware,
                                                  interrupt_queues);
diff --git a/sensors/tests/CMakeLists.txt b/sensors/tests/CMakeLists.txt
index b00f035c7..b96edd688 100644
--- a/sensors/tests/CMakeLists.txt
+++ b/sensors/tests/CMakeLists.txt
@@ -35,7 +35,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)
 

From 759eac89bb17c963e98bea3718b4288ea30a06bf Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Wed, 25 Oct 2023 23:20:12 -0400
Subject: [PATCH 09/68] format

---
 gantry/core/tasks_proto.cpp                   | 11 ++--
 gantry/core/tasks_rev1.cpp                    | 11 ++--
 gantry/firmware/interfaces_proto.cpp          | 35 ++++++------
 gantry/firmware/interfaces_rev1.cpp           | 35 ++++++------
 gantry/simulator/interfaces.cpp               |  4 +-
 gripper/firmware/interfaces_z_motor.cpp       |  5 +-
 gripper/simulator/interfaces.cpp              |  5 +-
 head/core/tasks_proto.cpp                     | 10 ++--
 head/core/tasks_rev1.cpp                      | 10 ++--
 head/firmware/main_proto.cpp                  | 10 ++--
 head/firmware/main_rev1.cpp                   | 10 ++--
 head/simulator/main.cpp                       | 10 ++--
 include/can/core/message_handlers/motion.hpp  |  4 +-
 include/can/core/messages.hpp                 |  3 +-
 include/gantry/core/interfaces_proto.hpp      |  2 +-
 include/gantry/core/interfaces_rev1.hpp       |  2 +-
 include/gantry/core/tasks_proto.hpp           |  3 +-
 include/gantry/core/tasks_rev1.hpp            |  3 +-
 .../brushed_motion_controller.hpp             |  3 +-
 .../core/stepper_motor/motion_controller.hpp  |  6 ++-
 .../stepper_motor/motor_interrupt_handler.hpp | 13 +++--
 .../core/stepper_motor/tmc2130_driver.hpp     |  3 +-
 .../core/stepper_motor/tmc2160_driver.hpp     |  3 +-
 .../tasks/gear_tmc2160_motor_driver_task.hpp  |  6 ++-
 .../core/tasks/motion_controller_task.hpp     | 32 +++++++----
 .../core/tasks/tmc2130_motor_driver_task.hpp  |  6 ++-
 .../core/tasks/tmc2160_motor_driver_task.hpp  | 26 +++++----
 .../brushed_motor/brushed_motor_hardware.hpp  |  4 +-
 .../firmware/stepper_motor/motor_hardware.hpp |  4 +-
 .../simulation/motor_interrupt_driver.hpp     |  7 +--
 .../simulation/sim_motor_hardware_iface.hpp   | 12 +++--
 .../tests/mock_brushed_motor_components.hpp   |  4 +-
 .../tests/mock_motor_driver_client.hpp        |  3 +-
 .../tests/mock_motor_hardware.hpp             |  4 +-
 include/pipettes/firmware/interfaces.hpp      | 12 +++--
 include/pipettes/firmware/interfaces_g4.hpp   | 12 +++--
 include/pipettes/simulator/interfaces.hpp     | 40 +++++++++-----
 include/spi/core/utils.hpp                    | 13 +++--
 include/spi/core/writer.hpp                   | 10 ++--
 motor-control/tests/test_limit_switch.cpp     |  9 ++--
 .../tests/test_limit_switch_backoff.cpp       |  9 ++--
 .../tests/test_motor_interrupt_handler.cpp    |  2 +-
 .../tests/test_motor_interrupt_queue.cpp      |  6 +--
 motor-control/tests/test_motor_pulse.cpp      |  9 ++--
 .../tests/test_motor_stall_handling.cpp       |  9 ++--
 motor-control/tests/test_sync_handling.cpp    |  9 ++--
 pipettes/core/linear_motor_tasks.cpp          | 10 ++--
 pipettes/firmware/interfaces.cpp              | 24 ++++++---
 pipettes/simulator/interfaces.cpp             | 54 ++++++++++++-------
 49 files changed, 323 insertions(+), 214 deletions(-)

diff --git a/gantry/core/tasks_proto.cpp b/gantry/core/tasks_proto.cpp
index c059698aa..143c18c12 100644
--- a/gantry/core/tasks_proto.cpp
+++ b/gantry/core/tasks_proto.cpp
@@ -62,11 +62,13 @@ auto gantry::tasks::start_tasks(
     tmc2130::configs::TMC2130DriverConfig& driver_configs,
     motor_hardware_task::MotorHardwareTask& mh_tsk,
     i2c::hardware::I2CBase& i2c2,
-    eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface) -> interfaces::diag0_handler {
+    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, ::queues);
+    auto& motion =
+        mc_task_builder.start(5, "motion controller", motion_controller,
+                              ::queues, ::queues, ::queues);
     auto& tmc2130_driver = motor_driver_task_builder.start(
         5, "tmc2130 driver", driver_configs, ::queues, spi_task_client);
     auto& move_group =
@@ -122,7 +124,8 @@ auto gantry::tasks::start_tasks(
 
 void gantry::tasks::call_run_diag0_interrupt() {
     if (gantry::tasks::get_tasks().motion_controller) {
-        return gantry::tasks::get_tasks().motion_controller->run_diag0_interrupt();
+        return gantry::tasks::get_tasks()
+            .motion_controller->run_diag0_interrupt();
     }
 }
 
diff --git a/gantry/core/tasks_rev1.cpp b/gantry/core/tasks_rev1.cpp
index 4c31ddb72..d7a294a1a 100644
--- a/gantry/core/tasks_rev1.cpp
+++ b/gantry/core/tasks_rev1.cpp
@@ -61,11 +61,13 @@ auto gantry::tasks::start_tasks(
     tmc2160::configs::TMC2160DriverConfig& driver_configs,
     motor_hardware_task::MotorHardwareTask& mh_tsk,
     i2c::hardware::I2CBase& i2c2,
-    eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface) -> interfaces::diag0_handler {
+    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, ::queues);
+    auto& motion =
+        mc_task_builder.start(5, "motion controller", motion_controller,
+                              ::queues, ::queues, ::queues);
     auto& tmc2160_driver = motor_driver_task_builder.start(
         5, "tmc2160 driver", driver_configs, ::queues, spi_task_client);
     auto& move_group =
@@ -121,7 +123,8 @@ auto gantry::tasks::start_tasks(
 
 void gantry::tasks::call_run_diag0_interrupt() {
     if (gantry::tasks::get_tasks().motion_controller) {
-        return gantry::tasks::get_tasks().motion_controller->run_diag0_interrupt();
+        return gantry::tasks::get_tasks()
+            .motion_controller->run_diag0_interrupt();
     }
 }
 
diff --git a/gantry/firmware/interfaces_proto.cpp b/gantry/firmware/interfaces_proto.cpp
index 123f3e835..0c142d197 100644
--- a/gantry/firmware/interfaces_proto.cpp
+++ b/gantry/firmware/interfaces_proto.cpp
@@ -86,11 +86,12 @@ struct motion_controller::HardwareConfig motor_pins_x {
             .port = GPIOB,
             .pin = GPIO_PIN_7,
             .active_setting = GPIO_PIN_RESET},
-    .estop_in = {
-        // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
-        .port = GPIOA,
-        .pin = GPIO_PIN_10,
-        .active_setting = GPIO_PIN_RESET},
+    .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 = GPIOC,
@@ -130,11 +131,12 @@ struct motion_controller::HardwareConfig motor_pins_y {
             .port = GPIOB,
             .pin = GPIO_PIN_5,
             .active_setting = GPIO_PIN_RESET},
-    .estop_in = {
-        // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
-        .port = GPIOA,
-        .pin = GPIO_PIN_10,
-        .active_setting = GPIO_PIN_RESET},
+    .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 = GPIOC,
@@ -143,8 +145,7 @@ struct motion_controller::HardwareConfig motor_pins_y {
 };
 
 static tmc2130::configs::TMC2130DriverConfig gantry_x_driver_configs{
-    .registers = {.gconfig = {.en_pwm_mode = 1,
-                              .diag0_error = 1},
+    .registers = {.gconfig = {.en_pwm_mode = 1, .diag0_error = 1},
                   .ihold_irun = {.hold_current = 0x2,
                                  .run_current = 0x18,
                                  .hold_current_delay = 0x7},
@@ -167,8 +168,7 @@ static tmc2130::configs::TMC2130DriverConfig gantry_x_driver_configs{
     }};
 
 static tmc2130::configs::TMC2130DriverConfig gantry_y_driver_configs{
-    .registers = {.gconfig = {.en_pwm_mode = 1,
-                              .diag0_error = 1},
+    .registers = {.gconfig = {.en_pwm_mode = 1, .diag0_error = 1},
                   .ihold_irun = {.hold_current = 0x2,
                                  .run_current = 0x18,
                                  .hold_current_delay = 0x7},
@@ -244,8 +244,8 @@ static stall_check::StallCheck stallcheck(
  * Handler of motor interrupts.
  */
 static motor_handler::MotorInterruptHandler motor_interrupt(
-    motor_queue, gantry::queues::get_queues(), 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);
@@ -287,7 +287,8 @@ void interfaces::initialize(diag0_handler* call_diag0_handler) {
         Error_Handler();
     }
 
-    initialize_timer(call_motor_handler, call_diag0_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 cb0f96a22..5a4137b12 100644
--- a/gantry/firmware/interfaces_rev1.cpp
+++ b/gantry/firmware/interfaces_rev1.cpp
@@ -86,11 +86,12 @@ struct motion_controller::HardwareConfig motor_pins_x {
             .port = GPIOB,
             .pin = GPIO_PIN_7,
             .active_setting = GPIO_PIN_RESET},
-    .estop_in = {
-        // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
-        .port = GPIOA,
-        .pin = GPIO_PIN_10,
-        .active_setting = GPIO_PIN_RESET},
+    .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 = GPIOC,
@@ -130,11 +131,12 @@ struct motion_controller::HardwareConfig motor_pins_y {
             .port = GPIOB,
             .pin = GPIO_PIN_7,
             .active_setting = GPIO_PIN_RESET},
-    .estop_in = {
-        // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
-        .port = GPIOA,
-        .pin = GPIO_PIN_10,
-        .active_setting = GPIO_PIN_RESET},
+    .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 = GPIOC,
@@ -143,8 +145,7 @@ 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},
                   .ihold_irun = {.hold_current = 16,
                                  .run_current = 31,
                                  .hold_current_delay = 0x7},
@@ -184,8 +185,7 @@ 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},
                   .ihold_irun = {.hold_current = 16,
                                  .run_current = 31,
                                  .hold_current_delay = 0x7},
@@ -271,8 +271,8 @@ static stall_check::StallCheck stallcheck(
  * Handler of motor interrupts.
  */
 static motor_handler::MotorInterruptHandler motor_interrupt(
-    motor_queue, gantry::queues::get_queues(), 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);
@@ -315,7 +315,8 @@ void interfaces::initialize(diag0_handler* call_diag0_handler) {
         Error_Handler();
     }
 
-    initialize_timer(call_motor_handler, call_diag0_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/simulator/interfaces.cpp b/gantry/simulator/interfaces.cpp
index e7a8a8f56..a22db5105 100644
--- a/gantry/simulator/interfaces.cpp
+++ b/gantry/simulator/interfaces.cpp
@@ -89,8 +89,8 @@ static stall_check::StallCheck stallcheck(
  * Handler of motor interrupts.
  */
 static motor_handler::MotorInterruptHandler motor_interrupt(
-    motor_queue, gantry::queues::get_queues(), 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,
diff --git a/gripper/firmware/interfaces_z_motor.cpp b/gripper/firmware/interfaces_z_motor.cpp
index d691d4ad5..2d2416ec9 100644
--- a/gripper/firmware/interfaces_z_motor.cpp
+++ b/gripper/firmware/interfaces_z_motor.cpp
@@ -190,8 +190,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(), 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);
diff --git a/gripper/simulator/interfaces.cpp b/gripper/simulator/interfaces.cpp
index 13469ba52..dd419450e 100644
--- a/gripper/simulator/interfaces.cpp
+++ b/gripper/simulator/interfaces.cpp
@@ -94,8 +94,9 @@ static stall_check::StallCheck stallcheck(
  * Handler of motor interrupts.
  */
 static motor_handler::MotorInterruptHandler motor_interrupt(
-    motor_queue, gripper_tasks::z_tasks::get_queues(), 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,
diff --git a/head/core/tasks_proto.cpp b/head/core/tasks_proto.cpp
index 69e52a767..6c3d4efd6 100644
--- a/head/core/tasks_proto.cpp
+++ b/head/core/tasks_proto.cpp
@@ -140,8 +140,9 @@ void head_tasks::start_tasks(
     head_queues.eeprom_queue = &eeprom_task.get_queue();
 
     // Start the left motor tasks
-    auto& left_motion = left_mc_task_builder.start(
-        5, "left mc", left_motion_controller, left_queues, left_queues, left_queues);
+    auto& left_motion =
+        left_mc_task_builder.start(5, "left mc", left_motion_controller,
+                                   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);
@@ -169,8 +170,9 @@ void head_tasks::start_tasks(
     left_queues.usage_storage_queue = &left_usage_storage_task.get_queue();
 
     // Start the right motor tasks
-    auto& right_motion = right_mc_task_builder.start(
-        5, "right mc", right_motion_controller, right_queues, right_queues, right_queues);
+    auto& right_motion =
+        right_mc_task_builder.start(5, "right mc", right_motion_controller,
+                                    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);
diff --git a/head/core/tasks_rev1.cpp b/head/core/tasks_rev1.cpp
index e150722cb..c875df463 100644
--- a/head/core/tasks_rev1.cpp
+++ b/head/core/tasks_rev1.cpp
@@ -145,8 +145,9 @@ void head_tasks::start_tasks(
     head_queues.eeprom_queue = &eeprom_task.get_queue();
 #endif
     // Start the left motor tasks
-    auto& left_motion = left_mc_task_builder.start(
-        5, "left mc", left_motion_controller, left_queues, left_queues, left_queues);
+    auto& left_motion =
+        left_mc_task_builder.start(5, "left mc", left_motion_controller,
+                                   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);
@@ -182,8 +183,9 @@ void head_tasks::start_tasks(
 #endif
 
     // Start the right motor tasks
-    auto& right_motion = right_mc_task_builder.start(
-        5, "right mc", right_motion_controller, right_queues, right_queues, right_queues);
+    auto& right_motion =
+        right_mc_task_builder.start(5, "right mc", right_motion_controller,
+                                    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);
diff --git a/head/firmware/main_proto.cpp b/head/firmware/main_proto.cpp
index 7b0948814..c63842b32 100644
--- a/head/firmware/main_proto.cpp
+++ b/head/firmware/main_proto.cpp
@@ -268,8 +268,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(), 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 +290,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(), 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);
diff --git a/head/firmware/main_rev1.cpp b/head/firmware/main_rev1.cpp
index 4d8e27054..008560e77 100644
--- a/head/firmware/main_rev1.cpp
+++ b/head/firmware/main_rev1.cpp
@@ -286,8 +286,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(), 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 +312,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(), 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);
diff --git a/head/simulator/main.cpp b/head/simulator/main.cpp
index bb2c03b63..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(), 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(), 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/message_handlers/motion.hpp b/include/can/core/message_handlers/motion.hpp
index 7434a5fab..b01ad0e9d 100644
--- a/include/can/core/message_handlers/motion.hpp
+++ b/include/can/core/message_handlers/motion.hpp
@@ -17,8 +17,8 @@ class MotionHandler {
         std::variant<std::monostate, DisableMotorRequest, EnableMotorRequest,
                      GetMotionConstraintsRequest, SetMotionConstraints,
                      ReadLimitSwitchRequest, MotorPositionRequest,
-                     UpdateMotorPositionEstimationRequest,
-                     GetMotorUsageRequest, MotorDriverErrorEncountered>;
+                     UpdateMotorPositionEstimationRequest, GetMotorUsageRequest,
+                     MotorDriverErrorEncountered>;
 
     MotionHandler(MotionTaskClient &motion_client)
         : motion_client{motion_client} {}
diff --git a/include/can/core/messages.hpp b/include/can/core/messages.hpp
index 30668c1d7..60c2decb5 100644
--- a/include/can/core/messages.hpp
+++ b/include/can/core/messages.hpp
@@ -198,7 +198,8 @@ using MotorPositionRequest = Empty<MessageId::motor_position_request>;
 using UpdateMotorPositionEstimationRequest =
     Empty<MessageId::update_motor_position_estimation_request>;
 
-using MotorDriverErrorEncountered = Empty<MessageId::motor_driver_error_encountered>;
+using MotorDriverErrorEncountered =
+    Empty<MessageId::motor_driver_error_encountered>;
 
 struct WriteToEEPromRequest : BaseMessage<MessageId::write_eeprom> {
     uint32_t message_index;
diff --git a/include/gantry/core/interfaces_proto.hpp b/include/gantry/core/interfaces_proto.hpp
index d5da7f710..264c3fd6e 100644
--- a/include/gantry/core/interfaces_proto.hpp
+++ b/include/gantry/core/interfaces_proto.hpp
@@ -14,7 +14,7 @@ extern "C" typedef void (*diag0_handler)(void);
 /**
  * Initialize the hardware portability layer.
  */
-void initialize(diag0_handler* call_diag0_handler);
+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 db4aba4af..fd5af0640 100644
--- a/include/gantry/core/interfaces_rev1.hpp
+++ b/include/gantry/core/interfaces_rev1.hpp
@@ -14,7 +14,7 @@ extern "C" typedef void (*diag0_handler)(void);
 /**
  * Initialize the hardware portability layer.
  */
-void initialize(diag0_handler* call_diag0_handler);
+void initialize(diag0_handler *call_diag0_handler);
 
 /**
  * Get the CAN bus interface.
diff --git a/include/gantry/core/tasks_proto.hpp b/include/gantry/core/tasks_proto.hpp
index 0e439b6b3..8fe6846d4 100644
--- a/include/gantry/core/tasks_proto.hpp
+++ b/include/gantry/core/tasks_proto.hpp
@@ -36,7 +36,8 @@ auto start_tasks(
     tmc2130::configs::TMC2130DriverConfig& driver_configs,
     motor_hardware_task::MotorHardwareTask& mh_tsk,
     i2c::hardware::I2CBase& i2c2,
-    eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface) -> interfaces::diag0_handler;
+    eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface)
+    -> interfaces::diag0_handler;
 
 void call_run_diag0_interrupt();
 
diff --git a/include/gantry/core/tasks_rev1.hpp b/include/gantry/core/tasks_rev1.hpp
index 567e2f181..ea086c412 100644
--- a/include/gantry/core/tasks_rev1.hpp
+++ b/include/gantry/core/tasks_rev1.hpp
@@ -36,7 +36,8 @@ auto start_tasks(
     tmc2160::configs::TMC2160DriverConfig& driver_configs,
     motor_hardware_task::MotorHardwareTask& mh_tsk,
     i2c::hardware::I2CBase& i2c2,
-    eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface) -> interfaces::diag0_handler;
+    eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface)
+    -> interfaces::diag0_handler;
 
 void call_run_diag0_interrupt();
 
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 449965b92..0c57df98f 100644
--- a/include/motor-control/core/brushed_motor/brushed_motion_controller.hpp
+++ b/include/motor-control/core/brushed_motor/brushed_motion_controller.hpp
@@ -101,7 +101,8 @@ class MotionController {
         enabled = false;
     }
 
-    void stop(can::ids::ErrorSeverity error_severity = can::ids::ErrorSeverity::warning) {
+    void stop(can::ids::ErrorSeverity error_severity =
+                  can::ids::ErrorSeverity::warning) {
         queue.reset();
         // if we're gripping something we need to flag this so we don't drop it
         if (!hardware.get_stay_enabled()) {
diff --git a/include/motor-control/core/stepper_motor/motion_controller.hpp b/include/motor-control/core/stepper_motor/motion_controller.hpp
index c49ff0353..75badedf8 100644
--- a/include/motor-control/core/stepper_motor/motion_controller.hpp
+++ b/include/motor-control/core/stepper_motor/motion_controller.hpp
@@ -107,7 +107,8 @@ class MotionController {
         return update_queue.try_write(can_msg);
     }
 
-    void stop(can::ids::ErrorSeverity error_severity = can::ids::ErrorSeverity::warning) {
+    void stop(can::ids::ErrorSeverity error_severity =
+                  can::ids::ErrorSeverity::warning) {
         queue.reset();
         if (hardware.is_timer_interrupt_running()) {
             hardware.request_cancel(static_cast<uint8_t>(error_severity));
@@ -262,7 +263,8 @@ class PipetteMotionController {
         return false;
     }
 
-    void stop(can::ids::ErrorSeverity error_severity = can::ids::ErrorSeverity::warning) {
+    void stop(can::ids::ErrorSeverity error_severity =
+                  can::ids::ErrorSeverity::warning) {
         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
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 0906849f2..cf870597a 100644
--- a/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp
+++ b/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp
@@ -42,8 +42,8 @@ using namespace motor_messages;
  * Note: The position tracker should never be allowed to go below zero.
  */
 
-template <template <class> class QueueImpl, class StatusClient, class DriverClient,
-          typename MotorMoveMessage, typename MotorHardware>
+template <template <class> class QueueImpl, class StatusClient,
+          class DriverClient, typename MotorMoveMessage, typename MotorHardware>
 requires MessageQueue<QueueImpl<MotorMoveMessage>, MotorMoveMessage> &&
     std::is_base_of_v<motor_hardware::MotorHardwareIface, MotorHardware>
 class MotorInterruptHandler {
@@ -207,10 +207,12 @@ class MotorInterruptHandler {
             cancel_and_clear_moves(can::ids::ErrorCode::estop_detected);
             in_estop = true;
         } else if (has_cancel_request) {
-            if (has_cancel_request == static_cast<uint8_t>(can::ids::ErrorSeverity::unrecoverable)) {
+            if (has_cancel_request ==
+                static_cast<uint8_t>(can::ids::ErrorSeverity::unrecoverable)) {
                 cancel_and_clear_moves();
             } else {
-                cancel_and_clear_moves(can::ids::ErrorCode::stop_requested, can::ids::ErrorSeverity::warning);
+                cancel_and_clear_moves(can::ids::ErrorCode::stop_requested,
+                                       can::ids::ErrorSeverity::warning);
             }
         } else {
             // Normal Move logic
@@ -431,7 +433,8 @@ class MotorInterruptHandler {
                                         .error_code = err_code});
         if (err_code == can::ids::ErrorCode::hardware) {
             driver_client.send_motor_driver_queue(
-                can::messages::ReadMotorDriverErrorStatus{.message_index = message_index});
+                can::messages::ReadMotorDriverErrorStatus{.message_index =
+                                                              message_index});
         }
         if (err_code == can::ids::ErrorCode::collision_detected) {
             build_and_send_ack(AckMessageId::position_error);
diff --git a/include/motor-control/core/stepper_motor/tmc2130_driver.hpp b/include/motor-control/core/stepper_motor/tmc2130_driver.hpp
index 12e920752..5e28596fe 100644
--- a/include/motor-control/core/stepper_motor/tmc2130_driver.hpp
+++ b/include/motor-control/core/stepper_motor/tmc2130_driver.hpp
@@ -54,7 +54,8 @@ class TMC2130 {
 
     auto read(uint32_t token, uint32_t command_data, uint32_t message_index)
         -> void {
-        _spi_manager.read(token, command_data, _task_queue, _cs_intf, message_index);
+        _spi_manager.read(token, command_data, _task_queue, _cs_intf,
+                          message_index);
     }
 
     auto write(Registers addr, uint32_t command_data) -> bool {
diff --git a/include/motor-control/core/stepper_motor/tmc2160_driver.hpp b/include/motor-control/core/stepper_motor/tmc2160_driver.hpp
index b15f850e9..bc78efbbb 100644
--- a/include/motor-control/core/stepper_motor/tmc2160_driver.hpp
+++ b/include/motor-control/core/stepper_motor/tmc2160_driver.hpp
@@ -55,7 +55,8 @@ class TMC2160 {
 
     auto read(uint32_t token, uint32_t command_data, uint32_t message_index)
         -> void {
-        _spi_manager.read(token, command_data, _task_queue, _cs_intf, message_index);
+        _spi_manager.read(token, command_data, _task_queue, _cs_intf,
+                          message_index);
     }
 
     auto write(Registers addr, uint32_t command_data) -> bool {
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 ed6db0362..56230e1cd 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
@@ -91,9 +91,11 @@ class MotorDriverMessageHandler {
     void handle(const can::messages::ReadMotorDriverErrorStatus& m) {
         LOG("Received read motor driver error register request");
         uint32_t data = 0;
-        auto converted_addr = static_cast<uint8_t>(tmc2160::registers::Registers::DRVSTATUS);
+        auto converted_addr =
+            static_cast<uint8_t>(tmc2160::registers::Registers::DRVSTATUS);
         std::array tags{spi::utils::ResponseTag::IS_ERROR_RESPONSE};
-        uint32_t token = spi::utils::build_token(converted_addr, spi::utils::byte_from_tags(tags));
+        uint32_t token = spi::utils::build_token(
+            converted_addr, spi::utils::byte_from_tags(tags));
         driver.read(token, data, m.message_index);
     }
 
diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp
index c1acd2aaa..073e21b6c 100644
--- a/include/motor-control/core/tasks/motion_controller_task.hpp
+++ b/include/motor-control/core/tasks/motion_controller_task.hpp
@@ -147,13 +147,18 @@ class MotionControllerMessageHandler {
     void handle(const can::messages::MotorDriverErrorEncountered& m) {
         // check if gpio is low before making controller calls
         controller.stop(can::ids::ErrorSeverity::unrecoverable);
-        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::hardware}); // make this motor_driver_error instead of hardware?
+        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::
+                        hardware});  // make this motor_driver_error instead of
+                                     // hardware?
             driver_client.send_motor_driver_queue(
-                can::messages::ReadMotorDriverErrorStatus{.message_index = m.message_index});
+                can::messages::ReadMotorDriverErrorStatus{.message_index =
+                                                              m.message_index});
         }
     }
 
@@ -188,9 +193,10 @@ class MotionControllerTask {
               tmc::tasks::TaskClient DriverClient>
     [[noreturn]] void operator()(
         motion_controller::MotionController<MEConfig>* controller,
-        CanClient* can_client, UsageClient* usage_client, DriverClient* driver_client) {
-        auto handler = MotionControllerMessageHandler{*controller, *can_client,
-                                                      *usage_client, *driver_client};
+        CanClient* can_client, UsageClient* usage_client,
+        DriverClient* driver_client) {
+        auto handler = MotionControllerMessageHandler{
+            *controller, *can_client, *usage_client, *driver_client};
         TaskMessage message{};
         bool first_run = true;
         for (;;) {
@@ -209,11 +215,15 @@ class MotionControllerTask {
     // also create top level query msg
     void run_diag0_interrupt(void) {
         if (!driver_error_handled()) {
-            static_cast<void>(queue.try_write_isr(can::messages::MotorDriverErrorEncountered{.message_index = 0}));
+            static_cast<void>(
+                queue.try_write_isr(can::messages::MotorDriverErrorEncountered{
+                    .message_index = 0}));
         }
     }
 
-    bool driver_error_handled() { return driver_error_handled_flag.exchange(true); }
+    bool driver_error_handled() {
+        return driver_error_handled_flag.exchange(true);
+    }
 
   private:
     QueueType& queue;
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 c8450b1b5..19d87c05b 100644
--- a/include/motor-control/core/tasks/tmc2130_motor_driver_task.hpp
+++ b/include/motor-control/core/tasks/tmc2130_motor_driver_task.hpp
@@ -90,9 +90,11 @@ class MotorDriverMessageHandler {
     void handle(const can::messages::ReadMotorDriverErrorStatus& m) {
         LOG("Received read motor driver error register request");
         uint32_t data = 0;
-        auto converted_addr = static_cast<uint8_t>(tmc2130::registers::Registers::DRVSTATUS);
+        auto converted_addr =
+            static_cast<uint8_t>(tmc2130::registers::Registers::DRVSTATUS);
         std::array tags{spi::utils::ResponseTag::IS_ERROR_RESPONSE};
-        uint32_t token = spi::utils::build_token(converted_addr, spi::utils::byte_from_tags(tags));
+        uint32_t token = spi::utils::build_token(
+            converted_addr, spi::utils::byte_from_tags(tags));
         driver.read(token, data, m.message_index);
     }
 
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 933fca708..4c4edb2bd 100644
--- a/include/motor-control/core/tasks/tmc2160_motor_driver_task.hpp
+++ b/include/motor-control/core/tasks/tmc2160_motor_driver_task.hpp
@@ -58,20 +58,24 @@ class MotorDriverMessageHandler {
             auto data = driver.handle_spi_read(
                 tmc2160::registers::Registers(static_cast<uint8_t>(m.id.token)),
                 m.rxBuffer);
-            if (spi::utils::tag_in_token(m.id.token, spi::utils::ResponseTag::IS_ERROR_RESPONSE)) {
-                can::messages::ReadMotorDriverErrorRegisterResponse 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::ReadMotorDriverErrorRegisterResponse
+                    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);
+                can_client.send_can_message(can::ids::NodeId::host,
+                                            response_msg);
             }
         }
     }
@@ -99,9 +103,11 @@ class MotorDriverMessageHandler {
     void handle(const can::messages::ReadMotorDriverErrorStatus& m) {
         LOG("Received read motor driver error register request");
         uint32_t data = 0;
-        auto converted_addr = static_cast<uint8_t>(tmc2160::registers::Registers::DRVSTATUS);
+        auto converted_addr =
+            static_cast<uint8_t>(tmc2160::registers::Registers::DRVSTATUS);
         std::array tags{spi::utils::ResponseTag::IS_ERROR_RESPONSE};
-        uint32_t token = spi::utils::build_token(converted_addr, spi::utils::byte_from_tags(tags));
+        uint32_t token = spi::utils::build_token(
+            converted_addr, spi::utils::byte_from_tags(tags));
         driver.read(token, data, m.message_index);
     }
 
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 93b8cee08..dde450562 100644
--- a/include/motor-control/firmware/brushed_motor/brushed_motor_hardware.hpp
+++ b/include/motor-control/firmware/brushed_motor/brushed_motor_hardware.hpp
@@ -81,7 +81,9 @@ class BrushedMotorHardware : public BrushedMotorHardwareIface {
     auto has_cancel_request() -> uint8_t final {
         return cancel_request.exchange(0);
     }
-    void request_cancel(uint8_t error_severity) final { cancel_request.store(error_severity); }
+    void request_cancel(uint8_t error_severity) final {
+        cancel_request.store(error_severity);
+    }
     auto get_usage_eeprom_config() -> const UsageEEpromConfig& final {
         return eeprom_config;
     }
diff --git a/include/motor-control/firmware/stepper_motor/motor_hardware.hpp b/include/motor-control/firmware/stepper_motor/motor_hardware.hpp
index bdd228095..245588afa 100644
--- a/include/motor-control/firmware/stepper_motor/motor_hardware.hpp
+++ b/include/motor-control/firmware/stepper_motor/motor_hardware.hpp
@@ -60,7 +60,9 @@ class MotorHardware : public StepperMotorHardwareIface {
     void disable_encoder() final;
     void enable_encoder() final;
 
-    void request_cancel(uint8_t error_severity) final { cancel_request.store(error_severity); }
+    void request_cancel(uint8_t error_severity) final {
+        cancel_request.store(error_severity);
+    }
 
     auto get_usage_eeprom_config() -> const UsageEEpromConfig& final {
         return eeprom_config;
diff --git a/include/motor-control/simulation/motor_interrupt_driver.hpp b/include/motor-control/simulation/motor_interrupt_driver.hpp
index a649252bf..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, class DriverClient, typename MotorMoveMessage, class MotorHardware>
+template <class StatusClient, class DriverClient, typename MotorMoveMessage,
+          class MotorHardware>
 class MotorInterruptDriver {
     using InterruptQueue =
         freertos_message_queue::FreeRTOSMessageQueue<MotorMoveMessage>;
@@ -17,8 +18,8 @@ class MotorInterruptDriver {
         freertos_message_queue::FreeRTOSMessageQueue<
             can::messages::UpdateMotorPositionEstimationRequest>;
     using InterruptHandler = motor_handler::MotorInterruptHandler<
-        freertos_message_queue::FreeRTOSMessageQueue, StatusClient, DriverClient,
-        MotorMoveMessage, MotorHardware>;
+        freertos_message_queue::FreeRTOSMessageQueue, StatusClient,
+        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 43fa1812a..c858d6fa3 100644
--- a/include/motor-control/simulation/sim_motor_hardware_iface.hpp
+++ b/include/motor-control/simulation/sim_motor_hardware_iface.hpp
@@ -90,7 +90,9 @@ class SimMotorHardwareIface : public motor_hardware::StepperMotorHardwareIface {
     auto has_cancel_request() -> uint8_t final {
         return cancel_request.exchange(0);
     }
-    void request_cancel(uint8_t error_severity) final { cancel_request.store(error_severity); }
+    void request_cancel(uint8_t error_severity) final {
+        cancel_request.store(error_severity);
+    }
     auto get_usage_eeprom_config()
         -> motor_hardware::UsageEEpromConfig & final {
         return eeprom_config;
@@ -180,7 +182,9 @@ class SimBrushedMotorHardwareIface
     auto has_cancel_request() -> uint8_t final {
         return cancel_request.exchange(0);
     }
-    void request_cancel(uint8_t error_severity) final { cancel_request.store(error_severity); }
+    void request_cancel(uint8_t error_severity) final {
+        cancel_request.store(error_severity);
+    }
 
     void disable_encoder() final {}
     void enable_encoder() final {}
@@ -270,7 +274,9 @@ class SimGearMotorHardwareIface
     auto has_cancel_request() -> uint8_t final {
         return cancel_request.exchange(0);
     }
-    void request_cancel(uint8_t error_severity) final { cancel_request.store(error_severity); }
+    void request_cancel(uint8_t error_severity) final {
+        cancel_request.store(error_severity);
+    }
 
     auto get_usage_eeprom_config()
         -> motor_hardware::UsageEEpromConfig & final {
diff --git a/include/motor-control/tests/mock_brushed_motor_components.hpp b/include/motor-control/tests/mock_brushed_motor_components.hpp
index a5d6f7143..a62522d7e 100644
--- a/include/motor-control/tests/mock_brushed_motor_components.hpp
+++ b/include/motor-control/tests/mock_brushed_motor_components.hpp
@@ -76,7 +76,9 @@ class MockBrushedMotorHardware : public BrushedMotorHardwareIface {
         cancel_request = 0;
         return old_request;
     }
-    void request_cancel(uint8_t error_severity) final { cancel_request = error_severity; }
+    void request_cancel(uint8_t error_severity) final {
+        cancel_request = error_severity;
+    }
     void set_timer_interrupt_running(bool is_running) {
         timer_interrupt_running = is_running;
     }
diff --git a/include/motor-control/tests/mock_motor_driver_client.hpp b/include/motor-control/tests/mock_motor_driver_client.hpp
index 787d87d7e..ac760bc14 100644
--- a/include/motor-control/tests/mock_motor_driver_client.hpp
+++ b/include/motor-control/tests/mock_motor_driver_client.hpp
@@ -7,8 +7,7 @@
 namespace test_mocks {
 
 struct MockMotorDriverClient {
-    void send_motor_driver_queue(
-        const tmc::tasks::TaskMessage& m) {
+    void send_motor_driver_queue(const tmc::tasks::TaskMessage& m) {
         messages.push_back(m);
     }
 
diff --git a/include/motor-control/tests/mock_motor_hardware.hpp b/include/motor-control/tests/mock_motor_hardware.hpp
index f4d855487..7eeb361e1 100644
--- a/include/motor-control/tests/mock_motor_hardware.hpp
+++ b/include/motor-control/tests/mock_motor_hardware.hpp
@@ -44,7 +44,9 @@ class MockMotorHardware : public motor_hardware::StepperMotorHardwareIface {
         cancel_request = 0;
         return old_request;
     }
-    void request_cancel(uint8_t error_severity) final { cancel_request = error_severity; }
+    void request_cancel(uint8_t error_severity) final {
+        cancel_request = error_severity;
+    }
     void sim_set_timer_interrupt_running(bool is_running) {
         mock_timer_interrupt_running = is_running;
     }
diff --git a/include/pipettes/firmware/interfaces.hpp b/include/pipettes/firmware/interfaces.hpp
index e9586c78b..d58f56f5d 100644
--- a/include/pipettes/firmware/interfaces.hpp
+++ b/include/pipettes/firmware/interfaces.hpp
@@ -20,8 +20,8 @@ namespace interfaces {
 
 template <typename Client, typename DriverClient>
 using MotorInterruptHandlerType = motor_handler::MotorInterruptHandler<
-    freertos_message_queue::FreeRTOSMessageQueue, Client, DriverClient, motor_messages::Move,
-    motor_hardware::MotorHardware>;
+    freertos_message_queue::FreeRTOSMessageQueue, Client, DriverClient,
+    motor_messages::Move, motor_hardware::MotorHardware>;
 template <typename Client>
 using GearMotorInterruptHandlerType = motor_handler::MotorInterruptHandler<
     freertos_message_queue::FreeRTOSMessageQueue, Client, Client,
@@ -67,11 +67,15 @@ namespace linear_motor {
 auto get_interrupt(motor_hardware::MotorHardware& hw,
                    LowThroughputInterruptQueues& queues,
                    stall_check::StallCheck& stall)
-    -> MotorInterruptHandlerType<linear_motor_tasks::QueueClient, linear_motor_tasks::tmc2130_driver::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, linear_motor_tasks::tmc2160_driver::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 cc31615e6..63d63b183 100644
--- a/include/pipettes/firmware/interfaces_g4.hpp
+++ b/include/pipettes/firmware/interfaces_g4.hpp
@@ -29,8 +29,8 @@ namespace interfaces {
 
 template <typename Client, typename DriverClient>
 using MotorInterruptHandlerType = motor_handler::MotorInterruptHandler<
-    freertos_message_queue::FreeRTOSMessageQueue, Client, DriverClient, motor_messages::Move,
-    motor_hardware::MotorHardware>;
+    freertos_message_queue::FreeRTOSMessageQueue, Client, DriverClient,
+    motor_messages::Move, motor_hardware::MotorHardware>;
 template <typename Client>
 using GearMotorInterruptHandlerType = motor_handler::MotorInterruptHandler<
     freertos_message_queue::FreeRTOSMessageQueue, Client, Client,
@@ -76,11 +76,15 @@ namespace linear_motor {
 auto get_interrupt(motor_hardware::MotorHardware& hw,
                    LowThroughputInterruptQueues& queues,
                    stall_check::StallCheck& stall)
-    -> MotorInterruptHandlerType<linear_motor_tasks::QueueClient, linear_motor_tasks::tmc2130_driver::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, linear_motor_tasks::tmc2160_driver::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 5826db532..369c65f0e 100644
--- a/include/pipettes/simulator/interfaces.hpp
+++ b/include/pipettes/simulator/interfaces.hpp
@@ -14,8 +14,8 @@ namespace interfaces {
 
 template <typename Client, typename DriverClient>
 using MotorInterruptHandlerType = motor_handler::MotorInterruptHandler<
-    freertos_message_queue::FreeRTOSMessageQueue, Client, DriverClient, motor_messages::Move,
-    sim_motor_hardware_iface::SimMotorHardwareIface>;
+    freertos_message_queue::FreeRTOSMessageQueue, Client, DriverClient,
+    motor_messages::Move, sim_motor_hardware_iface::SimMotorHardwareIface>;
 
 template <typename Client>
 using GearMotorInterruptHandlerType = motor_handler::MotorInterruptHandler<
@@ -61,29 +61,39 @@ auto get_interrupt_queues<PipetteType::THREE_EIGHTY_FOUR_CHANNEL>()
 namespace linear_motor {
 
 auto get_interrupt(sim_motor_hardware_iface::SimMotorHardwareIface& hw,
-                                 LowThroughputInterruptQueues& queues,
-                                 stall_check::StallCheck& stall)
-    -> MotorInterruptHandlerType<linear_motor_tasks::QueueClient, linear_motor_tasks::tmc2130_driver::QueueClient>;
+                   LowThroughputInterruptQueues& queues,
+                   stall_check::StallCheck& stall)
+    -> 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, linear_motor_tasks::tmc2160_driver::QueueClient>;
+                   HighThroughputInterruptQueues& queues,
+                   stall_check::StallCheck& stall)
+    -> 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, linear_motor_tasks::tmc2130_driver::QueueClient>& handler)
+    MotorInterruptHandlerType<linear_motor_tasks::QueueClient,
+                              linear_motor_tasks::tmc2130_driver::QueueClient>&
+        handler)
     -> motor_interrupt_driver::MotorInterruptDriver<
-        linear_motor_tasks::QueueClient, linear_motor_tasks::tmc2130_driver::QueueClient, motor_messages::Move,
+        linear_motor_tasks::QueueClient,
+        linear_motor_tasks::tmc2130_driver::QueueClient, motor_messages::Move,
         sim_motor_hardware_iface::SimMotorHardwareIface>;
 
 auto get_interrupt_driver(
     sim_motor_hardware_iface::SimMotorHardwareIface& hw,
     HighThroughputInterruptQueues& queues,
-    MotorInterruptHandlerType<linear_motor_tasks::QueueClient, linear_motor_tasks::tmc2160_driver::QueueClient>& handler)
+    MotorInterruptHandlerType<linear_motor_tasks::QueueClient,
+                              linear_motor_tasks::tmc2160_driver::QueueClient>&
+        handler)
     -> motor_interrupt_driver::MotorInterruptDriver<
-        linear_motor_tasks::QueueClient, linear_motor_tasks::tmc2160_driver::QueueClient, motor_messages::Move,
+        linear_motor_tasks::QueueClient,
+        linear_motor_tasks::tmc2160_driver::QueueClient, motor_messages::Move,
         sim_motor_hardware_iface::SimMotorHardwareIface>;
 
 auto get_motor_hardware() -> sim_motor_hardware_iface::SimMotorHardwareIface;
@@ -106,11 +116,13 @@ struct GearInterruptHandlers {
 
 struct GearInterruptDrivers {
     motor_interrupt_driver::MotorInterruptDriver<
-        gear_motor_tasks::QueueClient, 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, 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 47f8e6784..eed939ef9 100644
--- a/include/spi/core/utils.hpp
+++ b/include/spi/core/utils.hpp
@@ -34,16 +34,21 @@ template <std::ranges::range Tags>
     return result;
 }
 
-[[nodiscard]] inline constexpr auto tag_in_token(uint32_t token, ResponseTag tag) -> bool { // utilize
-    return bool(token & (1 << (static_cast<size_t>(tag) + static_cast<size_t>(8))));
+[[nodiscard]] inline constexpr auto tag_in_token(uint32_t token,
+                                                 ResponseTag tag)
+    -> bool {  // utilize
+    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 {
+[[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 { // utilize? If not, discard
+[[nodiscard]] inline constexpr auto reg_from_token(uint32_t id)
+    -> RegType {  // utilize? If not, discard
     return static_cast<RegType>(static_cast<uint8_t>(id & 0xff));
 }
 }  // namespace utils
diff --git a/include/spi/core/writer.hpp b/include/spi/core/writer.hpp
index 0fd6661c1..cf887f4b6 100644
--- a/include/spi/core/writer.hpp
+++ b/include/spi/core/writer.hpp
@@ -46,11 +46,11 @@ class Writer {
      * @return A success boolean
      */
     template <OriginatingResponseQueue RQType>
-    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);
+    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 = token,
             .command_type = static_cast<uint8_t>(spi::hardware::Mode::READ),
diff --git a/motor-control/tests/test_limit_switch.cpp b/motor-control/tests/test_limit_switch.cpp
index 4800a25d1..962874bee 100644
--- a/motor-control/tests/test_limit_switch.cpp
+++ b/motor-control/tests/test_limit_switch.cpp
@@ -2,9 +2,9 @@
 #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"
-#include "motor-control/tests/mock_motor_driver_client.hpp"
 
 using namespace motor_handler;
 
@@ -19,10 +19,9 @@ struct HandlerContainer {
     test_mocks::MockMoveStatusReporterClient reporter{};
     test_mocks::MockMotorDriverClient driver{};
     stall_check::StallCheck stall{10, 10, 10};
-    MotorInterruptHandler<test_mocks::MockMessageQueue,
-                          test_mocks::MockMoveStatusReporterClient, 
-                          test_mocks::MockMotorDriverClient, Move,
-                          test_mocks::MockMotorHardware>
+    MotorInterruptHandler<
+        test_mocks::MockMessageQueue, test_mocks::MockMoveStatusReporterClient,
+        test_mocks::MockMotorDriverClient, Move, test_mocks::MockMotorHardware>
         handler{queue, reporter, driver, hw, stall, update_position_queue};
 };
 
diff --git a/motor-control/tests/test_limit_switch_backoff.cpp b/motor-control/tests/test_limit_switch_backoff.cpp
index 16c6d3166..df1e462a1 100644
--- a/motor-control/tests/test_limit_switch_backoff.cpp
+++ b/motor-control/tests/test_limit_switch_backoff.cpp
@@ -2,9 +2,9 @@
 #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"
-#include "motor-control/tests/mock_motor_driver_client.hpp"
 
 using namespace motor_handler;
 
@@ -19,10 +19,9 @@ struct HandlerContainer {
     test_mocks::MockMoveStatusReporterClient reporter{};
     test_mocks::MockMotorDriverClient driver{};
     stall_check::StallCheck stall{10, 10, 10};
-    MotorInterruptHandler<test_mocks::MockMessageQueue,
-                          test_mocks::MockMoveStatusReporterClient, 
-                          test_mocks::MockMotorDriverClient, Move,
-                          test_mocks::MockMotorHardware>
+    MotorInterruptHandler<
+        test_mocks::MockMessageQueue, test_mocks::MockMoveStatusReporterClient,
+        test_mocks::MockMotorDriverClient, Move, test_mocks::MockMotorHardware>
         handler{queue, reporter, driver, hw, stall, update_position_queue};
 };
 
diff --git a/motor-control/tests/test_motor_interrupt_handler.cpp b/motor-control/tests/test_motor_interrupt_handler.cpp
index f0eb679d2..b0f2bd75c 100644
--- a/motor-control/tests/test_motor_interrupt_handler.cpp
+++ b/motor-control/tests/test_motor_interrupt_handler.cpp
@@ -3,9 +3,9 @@
 #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"
-#include "motor-control/tests/mock_motor_driver_client.hpp"
 
 using namespace motor_handler;
 
diff --git a/motor-control/tests/test_motor_interrupt_queue.cpp b/motor-control/tests/test_motor_interrupt_queue.cpp
index c16254c46..906da0ebd 100644
--- a/motor-control/tests/test_motor_interrupt_queue.cpp
+++ b/motor-control/tests/test_motor_interrupt_queue.cpp
@@ -2,9 +2,9 @@
 #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"
-#include "motor-control/tests/mock_motor_driver_client.hpp"
 
 using namespace motor_handler;
 
@@ -19,8 +19,8 @@ TEST_CASE("motor interrupt handler queue functionality") {
         test_mocks::MockMotorDriverClient driver{};
         test_mocks::MockMotorHardware hardware;
         stall_check::StallCheck stall(10, 10, 10);
-        auto handler = MotorInterruptHandler(queue, reporter, driver, 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 b625a9a24..312aa86d2 100644
--- a/motor-control/tests/test_motor_pulse.cpp
+++ b/motor-control/tests/test_motor_pulse.cpp
@@ -1,9 +1,9 @@
 #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"
-#include "motor-control/tests/mock_motor_driver_client.hpp"
 
 using namespace motor_handler;
 
@@ -24,10 +24,9 @@ struct HandlerContainer {
     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, 
-                          test_mocks::MockMotorDriverClient, Move,
-                          test_mocks::MockMotorHardware>
+    MotorInterruptHandler<
+        test_mocks::MockMessageQueue, test_mocks::MockMoveStatusReporterClient,
+        test_mocks::MockMotorDriverClient, Move, test_mocks::MockMotorHardware>
         handler{queue, reporter, driver, hw, stall, update_position_queue};
 };
 
diff --git a/motor-control/tests/test_motor_stall_handling.cpp b/motor-control/tests/test_motor_stall_handling.cpp
index c2893e763..f75b2db23 100644
--- a/motor-control/tests/test_motor_stall_handling.cpp
+++ b/motor-control/tests/test_motor_stall_handling.cpp
@@ -2,9 +2,9 @@
 #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"
-#include "motor-control/tests/mock_motor_driver_client.hpp"
 
 using namespace motor_handler;
 
@@ -24,10 +24,9 @@ struct HandlerContainer {
     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, 
-                          test_mocks::MockMotorDriverClient, Move,
-                          test_mocks::MockMotorHardware>
+    MotorInterruptHandler<
+        test_mocks::MockMessageQueue, test_mocks::MockMoveStatusReporterClient,
+        test_mocks::MockMotorDriverClient, Move, test_mocks::MockMotorHardware>
         handler{queue, reporter, driver, hw, stall, update_position_queue};
 };
 
diff --git a/motor-control/tests/test_sync_handling.cpp b/motor-control/tests/test_sync_handling.cpp
index 37d4a65dc..f35e21d0c 100644
--- a/motor-control/tests/test_sync_handling.cpp
+++ b/motor-control/tests/test_sync_handling.cpp
@@ -2,9 +2,9 @@
 #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"
-#include "motor-control/tests/mock_motor_driver_client.hpp"
 
 using namespace motor_handler;
 
@@ -19,10 +19,9 @@ struct HandlerContainer {
     test_mocks::MockMoveStatusReporterClient reporter{};
     test_mocks::MockMotorDriverClient driver{};
     stall_check::StallCheck stall{10, 10, 10};
-    MotorInterruptHandler<test_mocks::MockMessageQueue,
-                          test_mocks::MockMoveStatusReporterClient, 
-                          test_mocks::MockMotorDriverClient, Move,
-                          test_mocks::MockMotorHardware>
+    MotorInterruptHandler<
+        test_mocks::MockMessageQueue, test_mocks::MockMoveStatusReporterClient,
+        test_mocks::MockMotorDriverClient, Move, test_mocks::MockMotorHardware>
         handler{queue, reporter, driver, hw, stall, update_position_queue};
 };
 
diff --git a/pipettes/core/linear_motor_tasks.cpp b/pipettes/core/linear_motor_tasks.cpp
index e9939b5b6..5674238ec 100644
--- a/pipettes/core/linear_motor_tasks.cpp
+++ b/pipettes/core/linear_motor_tasks.cpp
@@ -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, tmc2130_queues);
+    auto& motion =
+        mc_task_builder.start(5, "motion controller", motion_controller, queues,
+                              queues, tmc2130_queues);
     auto& tmc2130_driver = tmc2130_driver_task_builder.start(
         5, "tmc2130 driver", linear_driver_configs, queues, spi_writer);
     auto& move_group =
@@ -100,8 +101,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, tmc2160_queues);
+    auto& motion =
+        mc_task_builder.start(5, "motion controller", motion_controller, queues,
+                              queues, tmc2160_queues);
     auto& tmc2160_driver = tmc2160_driver_task_builder.start(
         5, "tmc2160 driver", linear_driver_configs, queues, spi_writer);
     auto& move_group =
diff --git a/pipettes/firmware/interfaces.cpp b/pipettes/firmware/interfaces.cpp
index 5a8a22a43..909fa809d 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, linear_motor_tasks::tmc2130_driver::QueueClient> {
+    -> MotorInterruptHandlerType<
+        linear_motor_tasks::QueueClient,
+        linear_motor_tasks::tmc2130_driver::QueueClient> {
     return motor_handler::MotorInterruptHandler(
-        queues.plunger_queue, linear_motor_tasks::get_queues(), linear_motor_tasks::tmc2130_driver::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, linear_motor_tasks::tmc2160_driver::QueueClient> {
+    -> MotorInterruptHandlerType<
+        linear_motor_tasks::QueueClient,
+        linear_motor_tasks::tmc2160_driver::QueueClient> {
     return motor_handler::MotorInterruptHandler(
-        queues.plunger_queue, linear_motor_tasks::get_queues(), linear_motor_tasks::tmc2160_driver::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);
 }
 
@@ -130,11 +136,13 @@ auto gear_motor::get_interrupts(gear_motor::GearHardware& hw,
     -> gear_motor::GearInterruptHandlers {
     return gear_motor::GearInterruptHandlers{
         .left = motor_handler::MotorInterruptHandler(
-            queues.left_motor_queue, gear_motor_tasks::get_left_gear_queues(), gear_motor_tasks::get_left_gear_queues(),
-            hw.left, stall.left, queues.left_update_queue),
+            queues.left_motor_queue, gear_motor_tasks::get_left_gear_queues(),
+            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(), gear_motor_tasks::get_left_gear_queues(),
-            hw.right, stall.right, queues.right_update_queue)};
+            queues.right_motor_queue, gear_motor_tasks::get_right_gear_queues(),
+            gear_motor_tasks::get_left_gear_queues(), hw.right, stall.right,
+            queues.right_update_queue)};
 }
 
 auto gear_motor::get_interrupts(gear_motor::UnavailableGearHardware&,
diff --git a/pipettes/simulator/interfaces.cpp b/pipettes/simulator/interfaces.cpp
index 960dd9c69..a0029b4bc 100644
--- a/pipettes/simulator/interfaces.cpp
+++ b/pipettes/simulator/interfaces.cpp
@@ -44,44 +44,56 @@ auto interfaces::get_interrupt_queues<PipetteType::THREE_EIGHTY_FOUR_CHANNEL>()
         .left_update_queue = UpdatePositionQueue{"Left PUpdate Queue"}};
 }
 
-auto linear_motor::get_interrupt(sim_motor_hardware_iface::SimMotorHardwareIface& hw,
-                                 LowThroughputInterruptQueues& queues,
-                                 stall_check::StallCheck& stall)
-    -> MotorInterruptHandlerType<linear_motor_tasks::QueueClient, linear_motor_tasks::tmc2130_driver::QueueClient> {
+auto linear_motor::get_interrupt(
+    sim_motor_hardware_iface::SimMotorHardwareIface& hw,
+    LowThroughputInterruptQueues& queues, stall_check::StallCheck& stall)
+    -> MotorInterruptHandlerType<
+        linear_motor_tasks::QueueClient,
+        linear_motor_tasks::tmc2130_driver::QueueClient> {
     return motor_handler::MotorInterruptHandler(
-        queues.plunger_queue, linear_motor_tasks::get_queues(), linear_motor_tasks::tmc2130_driver::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, linear_motor_tasks::tmc2160_driver::QueueClient> {
+auto linear_motor::get_interrupt(
+    sim_motor_hardware_iface::SimMotorHardwareIface& hw,
+    HighThroughputInterruptQueues& queues, stall_check::StallCheck& stall)
+    -> MotorInterruptHandlerType<
+        linear_motor_tasks::QueueClient,
+        linear_motor_tasks::tmc2160_driver::QueueClient> {
     return motor_handler::MotorInterruptHandler(
-        queues.plunger_queue, linear_motor_tasks::get_queues(), linear_motor_tasks::tmc2160_driver::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, linear_motor_tasks::tmc2130_driver::QueueClient>& handler)
+    MotorInterruptHandlerType<linear_motor_tasks::QueueClient,
+                              linear_motor_tasks::tmc2130_driver::QueueClient>&
+        handler)
     -> motor_interrupt_driver::MotorInterruptDriver<
-        linear_motor_tasks::QueueClient, linear_motor_tasks::tmc2130_driver::QueueClient, motor_messages::Move,
+        linear_motor_tasks::QueueClient,
+        linear_motor_tasks::tmc2130_driver::QueueClient, motor_messages::Move,
         sim_motor_hardware_iface::SimMotorHardwareIface> {
-    return motor_interrupt_driver::MotorInterruptDriver(queues.plunger_queue, handler, hw,
-                                                        queues.plunger_update_queue);
+    return motor_interrupt_driver::MotorInterruptDriver(
+        queues.plunger_queue, handler, hw, queues.plunger_update_queue);
 }
 
 auto linear_motor::get_interrupt_driver(
     sim_motor_hardware_iface::SimMotorHardwareIface& hw,
     HighThroughputInterruptQueues& queues,
-    MotorInterruptHandlerType<linear_motor_tasks::QueueClient, linear_motor_tasks::tmc2160_driver::QueueClient>& handler)
+    MotorInterruptHandlerType<linear_motor_tasks::QueueClient,
+                              linear_motor_tasks::tmc2160_driver::QueueClient>&
+        handler)
     -> motor_interrupt_driver::MotorInterruptDriver<
-        linear_motor_tasks::QueueClient, linear_motor_tasks::tmc2160_driver::QueueClient, motor_messages::Move,
+        linear_motor_tasks::QueueClient,
+        linear_motor_tasks::tmc2160_driver::QueueClient, motor_messages::Move,
         sim_motor_hardware_iface::SimMotorHardwareIface> {
-    return motor_interrupt_driver::MotorInterruptDriver(queues.plunger_queue, handler, hw,
-                                                        queues.plunger_update_queue);
+    return motor_interrupt_driver::MotorInterruptDriver(
+        queues.plunger_queue, handler, hw, queues.plunger_update_queue);
 }
 
 auto linear_motor::get_motor_hardware()
@@ -124,10 +136,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(),
-            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(),
-            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),
     };
 }
 

From e8e0cf0dad094e8eb0471d122a45c14110165fc9 Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Thu, 26 Oct 2023 00:11:02 -0400
Subject: [PATCH 10/68] fixed test and lint errors

---
 gantry/firmware/main_proto.cpp                                | 2 +-
 gantry/firmware/main_rev1.cpp                                 | 2 +-
 include/gantry/core/interfaces_proto.hpp                      | 2 +-
 include/gantry/core/interfaces_rev1.hpp                       | 2 +-
 .../core/brushed_motor/brushed_motor_interrupt_handler.hpp    | 2 +-
 .../motor-control/core/stepper_motor/motion_controller.hpp    | 2 +-
 .../core/stepper_motor/motor_interrupt_handler.hpp            | 2 +-
 include/motor-control/core/tasks/motion_controller_task.hpp   | 4 ++--
 motor-control/tests/test_brushed_motor_interrupt_handler.cpp  | 2 +-
 9 files changed, 10 insertions(+), 10 deletions(-)

diff --git a/gantry/firmware/main_proto.cpp b/gantry/firmware/main_proto.cpp
index 7a57f3bab..55d10cc16 100644
--- a/gantry/firmware/main_proto.cpp
+++ b/gantry/firmware/main_proto.cpp
@@ -16,7 +16,7 @@
 #include "gantry/core/tasks_proto.hpp"
 #include "i2c/firmware/i2c_comms.hpp"
 
-static interfaces::diag0_handler call_diag0_handler = NULL;
+static interfaces::diag0_handler call_diag0_handler = nullptr;
 
 static auto i2c_comms2 = i2c::hardware::I2C();
 static auto i2c_handles = I2CHandlerStruct{};
diff --git a/gantry/firmware/main_rev1.cpp b/gantry/firmware/main_rev1.cpp
index e2f33185a..77b198539 100644
--- a/gantry/firmware/main_rev1.cpp
+++ b/gantry/firmware/main_rev1.cpp
@@ -16,7 +16,7 @@
 #include "gantry/core/tasks_rev1.hpp"
 #include "i2c/firmware/i2c_comms.hpp"
 
-static interfaces::diag0_handler call_diag0_handler = NULL;
+static interfaces::diag0_handler call_diag0_handler = nullptr;
 
 static auto i2c_comms2 = i2c::hardware::I2C();
 static auto i2c_handles = I2CHandlerStruct{};
diff --git a/include/gantry/core/interfaces_proto.hpp b/include/gantry/core/interfaces_proto.hpp
index 264c3fd6e..bf8230644 100644
--- a/include/gantry/core/interfaces_proto.hpp
+++ b/include/gantry/core/interfaces_proto.hpp
@@ -9,7 +9,7 @@
 
 namespace interfaces {
 
-extern "C" typedef void (*diag0_handler)(void);
+extern "C" using diag0_handler = void (*)(); // typedef void (*diag0_handler)(void)
 
 /**
  * Initialize the hardware portability layer.
diff --git a/include/gantry/core/interfaces_rev1.hpp b/include/gantry/core/interfaces_rev1.hpp
index fd5af0640..7af8e2e1b 100644
--- a/include/gantry/core/interfaces_rev1.hpp
+++ b/include/gantry/core/interfaces_rev1.hpp
@@ -9,7 +9,7 @@
 
 namespace interfaces {
 
-extern "C" typedef void (*diag0_handler)(void);
+extern "C" using diag0_handler = void (*)(); // typedef void (*diag0_handler)(void)
 
 /**
  * Initialize the hardware portability layer.
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 5fabf2b0a..a0458e9a5 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
@@ -201,7 +201,7 @@ 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 (hardware.has_cancel_request() != 0u) {
             if (!hardware.get_stay_enabled()) {
                 hardware.set_motor_state(BrushedMotorState::UNHOMED);
             }
diff --git a/include/motor-control/core/stepper_motor/motion_controller.hpp b/include/motor-control/core/stepper_motor/motion_controller.hpp
index 75badedf8..e8efc8ed7 100644
--- a/include/motor-control/core/stepper_motor/motion_controller.hpp
+++ b/include/motor-control/core/stepper_motor/motion_controller.hpp
@@ -115,7 +115,7 @@ class MotionController {
         }
     }
 
-    bool is_timer_interrupt_running() {
+    auto is_timer_interrupt_running() -> bool {
         return hardware.is_timer_interrupt_running();
     }
 
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 cf870597a..b41036758 100644
--- a/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp
+++ b/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp
@@ -206,7 +206,7 @@ class MotorInterruptHandler {
         } else if (estop_triggered()) {
             cancel_and_clear_moves(can::ids::ErrorCode::estop_detected);
             in_estop = true;
-        } else if (has_cancel_request) {
+        } else if (has_cancel_request != 0u) {
             if (has_cancel_request ==
                 static_cast<uint8_t>(can::ids::ErrorSeverity::unrecoverable)) {
                 cancel_and_clear_moves();
diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp
index 073e21b6c..940c0f904 100644
--- a/include/motor-control/core/tasks/motion_controller_task.hpp
+++ b/include/motor-control/core/tasks/motion_controller_task.hpp
@@ -213,7 +213,7 @@ class MotionControllerTask {
     [[nodiscard]] auto get_queue() const -> QueueType& { return queue; }
 
     // also create top level query msg
-    void run_diag0_interrupt(void) {
+    void run_diag0_interrupt() {
         if (!driver_error_handled()) {
             static_cast<void>(
                 queue.try_write_isr(can::messages::MotorDriverErrorEncountered{
@@ -221,7 +221,7 @@ class MotionControllerTask {
         }
     }
 
-    bool driver_error_handled() {
+    auto driver_error_handled() -> bool {
         return driver_error_handled_flag.exchange(true);
     }
 
diff --git a/motor-control/tests/test_brushed_motor_interrupt_handler.cpp b/motor-control/tests/test_brushed_motor_interrupt_handler.cpp
index 7b2cbbf3a..a8f08c1d9 100644
--- a/motor-control/tests/test_brushed_motor_interrupt_handler.cpp
+++ b/motor-control/tests/test_brushed_motor_interrupt_handler.cpp
@@ -512,7 +512,7 @@ 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.request_cancel(1);
             test_objs.handler.run_interrupt();
             THEN(
                 "motor state should become un-homed only if stay engaged is "

From 041cded60347d7b4dc05fa23b8fab680c7ed3dfa Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Thu, 26 Oct 2023 00:20:42 -0400
Subject: [PATCH 11/68] formatted

---
 include/gantry/core/interfaces_proto.hpp | 3 ++-
 include/gantry/core/interfaces_rev1.hpp  | 3 ++-
 2 files changed, 4 insertions(+), 2 deletions(-)

diff --git a/include/gantry/core/interfaces_proto.hpp b/include/gantry/core/interfaces_proto.hpp
index bf8230644..cf3072873 100644
--- a/include/gantry/core/interfaces_proto.hpp
+++ b/include/gantry/core/interfaces_proto.hpp
@@ -9,7 +9,8 @@
 
 namespace interfaces {
 
-extern "C" using diag0_handler = void (*)(); // typedef void (*diag0_handler)(void)
+extern "C" using diag0_handler =
+    void(*)();  // typedef void (*diag0_handler)(void)
 
 /**
  * Initialize the hardware portability layer.
diff --git a/include/gantry/core/interfaces_rev1.hpp b/include/gantry/core/interfaces_rev1.hpp
index 7af8e2e1b..120c63ce1 100644
--- a/include/gantry/core/interfaces_rev1.hpp
+++ b/include/gantry/core/interfaces_rev1.hpp
@@ -9,7 +9,8 @@
 
 namespace interfaces {
 
-extern "C" using diag0_handler = void (*)(); // typedef void (*diag0_handler)(void)
+extern "C" using diag0_handler =
+    void(*)();  // typedef void (*diag0_handler)(void)
 
 /**
  * Initialize the hardware portability layer.

From 457da97cb2f2c8679cd400a8e9fe869539a01658 Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Thu, 26 Oct 2023 00:40:18 -0400
Subject: [PATCH 12/68] fixed lint errors

---
 .../core/brushed_motor/brushed_motor_interrupt_handler.hpp      | 2 +-
 .../core/stepper_motor/motor_interrupt_handler.hpp              | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

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 a0458e9a5..798010913 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
@@ -201,7 +201,7 @@ class BrushedMotorInterruptHandler {
         } else if (estop_triggered()) {
             in_estop = true;
             cancel_and_clear_moves(can::ids::ErrorCode::estop_detected);
-        } else if (hardware.has_cancel_request() != 0u) {
+        } else if (hardware.has_cancel_request() != 0U) {
             if (!hardware.get_stay_enabled()) {
                 hardware.set_motor_state(BrushedMotorState::UNHOMED);
             }
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 b41036758..6f6894054 100644
--- a/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp
+++ b/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp
@@ -206,7 +206,7 @@ class MotorInterruptHandler {
         } else if (estop_triggered()) {
             cancel_and_clear_moves(can::ids::ErrorCode::estop_detected);
             in_estop = true;
-        } else if (has_cancel_request != 0u) {
+        } else if (has_cancel_request != 0U) {
             if (has_cancel_request ==
                 static_cast<uint8_t>(can::ids::ErrorSeverity::unrecoverable)) {
                 cancel_and_clear_moves();

From 36257fadbe547d073deb08c574d40ae4ba9c259a Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Sat, 28 Oct 2023 14:36:37 -0400
Subject: [PATCH 13/68] PR review fixes part 1

---
 head/core/can_task.cpp                        |  3 +-
 include/can/core/ids.hpp                      |  3 +-
 include/can/core/message_handlers/motion.hpp  |  2 +-
 include/can/core/messages.hpp                 |  3 ++
 include/gantry/core/can_task.hpp              |  3 +-
 include/gantry/core/interfaces_proto.hpp      |  3 +-
 include/gantry/core/interfaces_rev1.hpp       |  3 +-
 include/gripper/core/can_task.hpp             |  3 +-
 .../core/motor_hardware_interface.hpp         |  1 +
 .../core/stepper_motor/motion_controller.hpp  |  5 ++
 .../stepper_motor/motor_interrupt_handler.hpp |  4 +-
 .../core/stepper_motor/tmc2130_driver.hpp     |  4 +-
 .../core/stepper_motor/tmc2160_driver.hpp     |  4 +-
 .../tasks/gear_tmc2160_motor_driver_task.hpp  | 11 ++--
 include/motor-control/core/tasks/messages.hpp |  3 +-
 .../core/tasks/motion_controller_task.hpp     | 52 ++++++++++---------
 .../core/tasks/tmc2130_motor_driver_task.hpp  | 13 ++---
 .../core/tasks/tmc2160_motor_driver_task.hpp  | 11 ++--
 .../brushed_motor/brushed_motor_hardware.hpp  |  3 ++
 .../firmware/stepper_motor/motor_hardware.hpp |  9 ++--
 .../tests/mock_brushed_motor_components.hpp   |  3 ++
 .../tests/mock_motor_hardware.hpp             |  3 ++
 include/pipettes/core/dispatch_builder.hpp    |  3 +-
 23 files changed, 85 insertions(+), 67 deletions(-)

diff --git a/head/core/can_task.cpp b/head/core/can_task.cpp
index 358c537b5..726a7891a 100644
--- a/head/core/can_task.cpp
+++ b/head/core/can_task.cpp
@@ -45,7 +45,8 @@ using MotionControllerDispatchTarget = can::dispatch::DispatchParseTarget<
     can::messages::MotorPositionRequest,
     can::messages::UpdateMotorPositionEstimationRequest,
     can::messages::GetMotorUsageRequest,
-    can::messages::MotorDriverErrorEncountered>;
+    can::messages::MotorDriverErrorEncountered,
+    can::messages::ResetMotorDriverErrorHandling>;
 using SystemDispatchTarget = can::dispatch::DispatchParseTarget<
     can::message_handlers::system::SystemMessageHandler<
         head_tasks::HeadQueueClient>,
diff --git a/include/can/core/ids.hpp b/include/can/core/ids.hpp
index e7be2ef8b..a3b6764fa 100644
--- a/include/can/core/ids.hpp
+++ b/include/can/core/ids.hpp
@@ -64,6 +64,7 @@ enum class MessageId {
     motor_driver_error_encountered = 0x36,
     read_motor_driver_error_status = 0x37,
     read_motor_driver_error_register_response = 0x38,
+    reset_motor_driver_error_handling = 0x39,
     set_brushed_motor_vref_request = 0x40,
     set_brushed_motor_pwm_request = 0x41,
     gripper_grip_request = 0x42,
@@ -154,7 +155,7 @@ enum class ErrorCode {
     motor_busy = 0xb,
     stop_requested = 0xc,
     over_pressure = 0xd,
-    // motor_driver_error = 0xe,
+    motor_driver_error_detected = 0xe,
 };
 
 /** Error Severity levels. */
diff --git a/include/can/core/message_handlers/motion.hpp b/include/can/core/message_handlers/motion.hpp
index b01ad0e9d..e360f91ec 100644
--- a/include/can/core/message_handlers/motion.hpp
+++ b/include/can/core/message_handlers/motion.hpp
@@ -18,7 +18,7 @@ class MotionHandler {
                      GetMotionConstraintsRequest, SetMotionConstraints,
                      ReadLimitSwitchRequest, MotorPositionRequest,
                      UpdateMotorPositionEstimationRequest, GetMotorUsageRequest,
-                     MotorDriverErrorEncountered>;
+                     MotorDriverErrorEncountered, ResetMotorDriverErrorHandling>;
 
     MotionHandler(MotionTaskClient &motion_client)
         : motion_client{motion_client} {}
diff --git a/include/can/core/messages.hpp b/include/can/core/messages.hpp
index 60c2decb5..410580743 100644
--- a/include/can/core/messages.hpp
+++ b/include/can/core/messages.hpp
@@ -201,6 +201,9 @@ using UpdateMotorPositionEstimationRequest =
 using MotorDriverErrorEncountered =
     Empty<MessageId::motor_driver_error_encountered>;
 
+using ResetMotorDriverErrorHandling =
+    Empty<MessageId::reset_motor_driver_error_handling>;
+
 struct WriteToEEPromRequest : BaseMessage<MessageId::write_eeprom> {
     uint32_t message_index;
     eeprom::types::address address;
diff --git a/include/gantry/core/can_task.hpp b/include/gantry/core/can_task.hpp
index 4117d2ab7..5cea6d627 100644
--- a/include/gantry/core/can_task.hpp
+++ b/include/gantry/core/can_task.hpp
@@ -38,7 +38,8 @@ using MotionControllerDispatchTarget = can::dispatch::DispatchParseTarget<
     can::messages::MotorPositionRequest,
     can::messages::UpdateMotorPositionEstimationRequest,
     can::messages::GetMotorUsageRequest,
-    can::messages::MotorDriverErrorEncountered>;
+    can::messages::MotorDriverErrorEncountered,
+    can::messages::ResetMotorDriverErrorHandling>;
 using SystemDispatchTarget = can::dispatch::DispatchParseTarget<
     can::message_handlers::system::SystemMessageHandler<
         gantry::queues::QueueClient>,
diff --git a/include/gantry/core/interfaces_proto.hpp b/include/gantry/core/interfaces_proto.hpp
index cf3072873..ad72967b2 100644
--- a/include/gantry/core/interfaces_proto.hpp
+++ b/include/gantry/core/interfaces_proto.hpp
@@ -9,8 +9,7 @@
 
 namespace interfaces {
 
-extern "C" using diag0_handler =
-    void(*)();  // typedef void (*diag0_handler)(void)
+extern "C" using diag0_handler = void(*)();
 
 /**
  * Initialize the hardware portability layer.
diff --git a/include/gantry/core/interfaces_rev1.hpp b/include/gantry/core/interfaces_rev1.hpp
index 120c63ce1..fa276cbff 100644
--- a/include/gantry/core/interfaces_rev1.hpp
+++ b/include/gantry/core/interfaces_rev1.hpp
@@ -9,8 +9,7 @@
 
 namespace interfaces {
 
-extern "C" using diag0_handler =
-    void(*)();  // typedef void (*diag0_handler)(void)
+extern "C" using diag0_handler = void(*)();
 
 /**
  * Initialize the hardware portability layer.
diff --git a/include/gripper/core/can_task.hpp b/include/gripper/core/can_task.hpp
index 53d8ebddd..c08d190e1 100644
--- a/include/gripper/core/can_task.hpp
+++ b/include/gripper/core/can_task.hpp
@@ -34,7 +34,8 @@ using MotionControllerDispatchTarget = can::dispatch::DispatchParseTarget<
     can::messages::MotorPositionRequest,
     can::messages::UpdateMotorPositionEstimationRequest,
     can::messages::GetMotorUsageRequest,
-    can::messages::MotorDriverErrorEncountered>;
+    can::messages::MotorDriverErrorEncountered,
+    can::messages::ResetMotorDriverErrorHandling>;
 using SystemDispatchTarget = can::dispatch::DispatchParseTarget<
     can::message_handlers::system::SystemMessageHandler<
         gripper_tasks::QueueClient>,
diff --git a/include/motor-control/core/motor_hardware_interface.hpp b/include/motor-control/core/motor_hardware_interface.hpp
index 54eb1178f..f375cd68c 100644
--- a/include/motor-control/core/motor_hardware_interface.hpp
+++ b/include/motor-control/core/motor_hardware_interface.hpp
@@ -100,6 +100,7 @@ class MotorHardwareIface {
 
     virtual auto has_cancel_request() -> uint8_t = 0;
     virtual void request_cancel(uint8_t error_severity) = 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 e8efc8ed7..a5021f3be 100644
--- a/include/motor-control/core/stepper_motor/motion_controller.hpp
+++ b/include/motor-control/core/stepper_motor/motion_controller.hpp
@@ -115,6 +115,10 @@ class MotionController {
         }
     }
 
+    void clear_cancel_request() {
+        hardware.clear_cancel_request();
+    }
+
     auto is_timer_interrupt_running() -> bool {
         return hardware.is_timer_interrupt_running();
     }
@@ -134,6 +138,7 @@ class MotionController {
     auto check_read_sync_line() -> bool { return hardware.check_sync_in(); }
 
     void enable_motor() {
+        // check motor driver error register
         hardware.start_timer_interrupt();
         hardware.activate_motor();
         enabled = true;
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 6f6894054..6b3829f70 100644
--- a/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp
+++ b/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp
@@ -209,7 +209,7 @@ class MotorInterruptHandler {
         } else if (has_cancel_request != 0U) {
             if (has_cancel_request ==
                 static_cast<uint8_t>(can::ids::ErrorSeverity::unrecoverable)) {
-                cancel_and_clear_moves();
+                cancel_and_clear_moves(can::ids::ErrorCode::motor_driver_error_detected); // use cancel_request.code?
             } else {
                 cancel_and_clear_moves(can::ids::ErrorCode::stop_requested,
                                        can::ids::ErrorSeverity::warning);
@@ -431,7 +431,7 @@ class MotorInterruptHandler {
             can::messages::ErrorMessage{.message_index = message_index,
                                         .severity = severity,
                                         .error_code = err_code});
-        if (err_code == can::ids::ErrorCode::hardware) {
+        if (err_code == can::ids::ErrorCode::motor_driver_error_detected) {
             driver_client.send_motor_driver_queue(
                 can::messages::ReadMotorDriverErrorStatus{.message_index =
                                                               message_index});
diff --git a/include/motor-control/core/stepper_motor/tmc2130_driver.hpp b/include/motor-control/core/stepper_motor/tmc2130_driver.hpp
index 5e28596fe..887f9cd02 100644
--- a/include/motor-control/core/stepper_motor/tmc2130_driver.hpp
+++ b/include/motor-control/core/stepper_motor/tmc2130_driver.hpp
@@ -52,8 +52,10 @@ class TMC2130 {
     auto operator=(const TMC2130&& c) = delete;
     ~TMC2130() = default;
 
-    auto read(uint32_t token, uint32_t command_data, uint32_t message_index)
+    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);
+        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_driver.hpp b/include/motor-control/core/stepper_motor/tmc2160_driver.hpp
index bc78efbbb..99b991345 100644
--- a/include/motor-control/core/stepper_motor/tmc2160_driver.hpp
+++ b/include/motor-control/core/stepper_motor/tmc2160_driver.hpp
@@ -53,8 +53,10 @@ class TMC2160 {
     auto operator=(const TMC2160&& c) = delete;
     ~TMC2160() = default;
 
-    auto read(uint32_t token, uint32_t command_data, uint32_t message_index)
+    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);
+        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/tasks/gear_tmc2160_motor_driver_task.hpp b/include/motor-control/core/tasks/gear_tmc2160_motor_driver_task.hpp
index 56230e1cd..966925a3e 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
@@ -82,21 +82,16 @@ class MotorDriverMessageHandler {
         LOG("Received read motor driver request: addr=%d", m.reg_address);
         uint32_t data = 0;
         if (tmc2160::registers::is_valid_address(m.reg_address)) {
-            auto converted_addr = static_cast<uint8_t>(m.reg_address);
-            uint32_t token = spi::utils::build_token(converted_addr);
-            driver.read(token, data, m.message_index);
+            driver.read(tmc2160::registers::Registers(m.reg_address), data, m.message_index);
         }
     }
 
     void handle(const can::messages::ReadMotorDriverErrorStatus& m) {
         LOG("Received read motor driver error register request");
         uint32_t data = 0;
-        auto converted_addr =
-            static_cast<uint8_t>(tmc2160::registers::Registers::DRVSTATUS);
         std::array tags{spi::utils::ResponseTag::IS_ERROR_RESPONSE};
-        uint32_t token = spi::utils::build_token(
-            converted_addr, spi::utils::byte_from_tags(tags));
-        driver.read(token, data, m.message_index);
+        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) {
diff --git a/include/motor-control/core/tasks/messages.hpp b/include/motor-control/core/tasks/messages.hpp
index d051d09a8..8ac661f88 100644
--- a/include/motor-control/core/tasks/messages.hpp
+++ b/include/motor-control/core/tasks/messages.hpp
@@ -15,7 +15,8 @@ using MotionControlTaskMessage = std::variant<
     can::messages::HomeRequest,
     can::messages::UpdateMotorPositionEstimationRequest,
     can::messages::GetMotorUsageRequest,
-    can::messages::MotorDriverErrorEncountered>;
+    can::messages::MotorDriverErrorEncountered,
+    can::messages::ResetMotorDriverErrorHandling>;
 
 using MotorDriverTaskMessage =
     std::variant<std::monostate, can::messages::ReadMotorDriverRegister,
diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp
index 940c0f904..08af6c3fe 100644
--- a/include/motor-control/core/tasks/motion_controller_task.hpp
+++ b/include/motor-control/core/tasks/motion_controller_task.hpp
@@ -145,27 +145,38 @@ class MotionControllerMessageHandler {
     }
 
     void handle(const can::messages::MotorDriverErrorEncountered& m) {
-        // check if gpio is low before making controller calls
-        controller.stop(can::ids::ErrorSeverity::unrecoverable);
-        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::
-                        hardware});  // make this motor_driver_error instead of
-                                     // hardware?
-            driver_client.send_motor_driver_queue(
-                can::messages::ReadMotorDriverErrorStatus{.message_index =
-                                                              m.message_index});
+        if (!driver_error_handled()) {
+            // check if gpio is low before making controller calls
+            controller.stop(can::ids::ErrorSeverity::unrecoverable);
+            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::ReadMotorDriverErrorStatus{.message_index =
+                                                                m.message_index});
+            }
         }
     }
 
+    void handle(const can::messages::ResetMotorDriverErrorHandling& m) {
+        static_cast<void>(m);
+        driver_error_handled_flag.exchange(false);
+        controller.clear_cancel_request();
+    }
+
+    auto driver_error_handled() -> bool {
+        return driver_error_handled_flag.exchange(true);
+    }
+
     MotorControllerType& controller;
     CanClient& can_client;
     UsageClient& usage_client;
     DriverClient& driver_client;
+    std::atomic<bool> driver_error_handled_flag = false;
 };
 
 /**
@@ -214,20 +225,13 @@ class MotionControllerTask {
 
     // also create top level query msg
     void run_diag0_interrupt() {
-        if (!driver_error_handled()) {
-            static_cast<void>(
-                queue.try_write_isr(can::messages::MotorDriverErrorEncountered{
-                    .message_index = 0}));
-        }
-    }
-
-    auto driver_error_handled() -> bool {
-        return driver_error_handled_flag.exchange(true);
+        static_cast<void>(
+            queue.try_write_isr(can::messages::MotorDriverErrorEncountered{
+                .message_index = 0}));
     }
 
   private:
     QueueType& queue;
-    std::atomic<bool> driver_error_handled_flag = false;
 };
 
 /**
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 19d87c05b..3821d5f1d 100644
--- a/include/motor-control/core/tasks/tmc2130_motor_driver_task.hpp
+++ b/include/motor-control/core/tasks/tmc2130_motor_driver_task.hpp
@@ -81,21 +81,16 @@ class MotorDriverMessageHandler {
         LOG("Received read motor driver request: addr=%d", m.reg_address);
         uint32_t data = 0;
         if (tmc2130::registers::is_valid_address(m.reg_address)) {
-            auto converted_addr = static_cast<uint8_t>(m.reg_address);
-            uint32_t token = spi::utils::build_token(converted_addr);
-            driver.read(token, data, m.message_index);
+            driver.read(tmc2130::registers::Registers(m.reg_address), data, m.message_index);
         }
     }
 
     void handle(const can::messages::ReadMotorDriverErrorStatus& m) {
         LOG("Received read motor driver error register request");
-        uint32_t data = 0;
-        auto converted_addr =
-            static_cast<uint8_t>(tmc2130::registers::Registers::DRVSTATUS);
+        uint32_t data = 0;        
         std::array tags{spi::utils::ResponseTag::IS_ERROR_RESPONSE};
-        uint32_t token = spi::utils::build_token(
-            converted_addr, spi::utils::byte_from_tags(tags));
-        driver.read(token, data, m.message_index);
+        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) {
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 4c4edb2bd..9030248b6 100644
--- a/include/motor-control/core/tasks/tmc2160_motor_driver_task.hpp
+++ b/include/motor-control/core/tasks/tmc2160_motor_driver_task.hpp
@@ -94,21 +94,16 @@ class MotorDriverMessageHandler {
         LOG("Received read motor driver request: addr=%d", m.reg_address);
         uint32_t data = 0;
         if (tmc2160::registers::is_valid_address(m.reg_address)) {
-            auto converted_addr = static_cast<uint8_t>(m.reg_address);
-            uint32_t token = spi::utils::build_token(converted_addr);
-            driver.read(token, data, m.message_index);
+            driver.read(tmc2160::registers::Registers(m.reg_address), data, m.message_index);
         }
     }
 
     void handle(const can::messages::ReadMotorDriverErrorStatus& m) {
         LOG("Received read motor driver error register request");
         uint32_t data = 0;
-        auto converted_addr =
-            static_cast<uint8_t>(tmc2160::registers::Registers::DRVSTATUS);
         std::array tags{spi::utils::ResponseTag::IS_ERROR_RESPONSE};
-        uint32_t token = spi::utils::build_token(
-            converted_addr, spi::utils::byte_from_tags(tags));
-        driver.read(token, data, m.message_index);
+        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) {
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 dde450562..109642127 100644
--- a/include/motor-control/firmware/brushed_motor/brushed_motor_hardware.hpp
+++ b/include/motor-control/firmware/brushed_motor/brushed_motor_hardware.hpp
@@ -84,6 +84,9 @@ class BrushedMotorHardware : public BrushedMotorHardwareIface {
     void request_cancel(uint8_t error_severity) final {
         cancel_request.store(error_severity);
     }
+    void clear_cancel_request() final {
+        cancel_request.store(0);
+    }
     auto get_usage_eeprom_config() -> const UsageEEpromConfig& final {
         return eeprom_config;
     }
diff --git a/include/motor-control/firmware/stepper_motor/motor_hardware.hpp b/include/motor-control/firmware/stepper_motor/motor_hardware.hpp
index 245588afa..00e31933c 100644
--- a/include/motor-control/firmware/stepper_motor/motor_hardware.hpp
+++ b/include/motor-control/firmware/stepper_motor/motor_hardware.hpp
@@ -54,15 +54,18 @@ class MotorHardware : public StepperMotorHardwareIface {
     void set_LED(bool status) final;
     auto get_encoder_pulses() -> int32_t final;
     void reset_encoder_pulses() final;
-    auto has_cancel_request() -> uint8_t final {
-        return cancel_request.exchange(0);
-    }
     void disable_encoder() final;
     void enable_encoder() final;
 
+    auto has_cancel_request() -> uint8_t final {
+        return cancel_request.exchange(0);
+    }
     void request_cancel(uint8_t error_severity) final {
         cancel_request.store(error_severity);
     }
+    void clear_cancel_request() final {
+        cancel_request.store(0);
+    }
 
     auto get_usage_eeprom_config() -> const UsageEEpromConfig& final {
         return eeprom_config;
diff --git a/include/motor-control/tests/mock_brushed_motor_components.hpp b/include/motor-control/tests/mock_brushed_motor_components.hpp
index a62522d7e..e0c467061 100644
--- a/include/motor-control/tests/mock_brushed_motor_components.hpp
+++ b/include/motor-control/tests/mock_brushed_motor_components.hpp
@@ -79,6 +79,9 @@ class MockBrushedMotorHardware : public BrushedMotorHardwareIface {
     void request_cancel(uint8_t error_severity) final {
         cancel_request = error_severity;
     }
+    void clear_cancel_request() final {
+        cancel_request = 0;
+    }
     void set_timer_interrupt_running(bool is_running) {
         timer_interrupt_running = is_running;
     }
diff --git a/include/motor-control/tests/mock_motor_hardware.hpp b/include/motor-control/tests/mock_motor_hardware.hpp
index 7eeb361e1..6c681e6cd 100644
--- a/include/motor-control/tests/mock_motor_hardware.hpp
+++ b/include/motor-control/tests/mock_motor_hardware.hpp
@@ -47,6 +47,9 @@ class MockMotorHardware : public motor_hardware::StepperMotorHardwareIface {
     void request_cancel(uint8_t error_severity) final {
         cancel_request = error_severity;
     }
+    void clear_cancel_request() final {
+        cancel_request = 0;
+    }
     void sim_set_timer_interrupt_running(bool is_running) {
         mock_timer_interrupt_running = is_running;
     }
diff --git a/include/pipettes/core/dispatch_builder.hpp b/include/pipettes/core/dispatch_builder.hpp
index 0acd404bf..f0177d265 100644
--- a/include/pipettes/core/dispatch_builder.hpp
+++ b/include/pipettes/core/dispatch_builder.hpp
@@ -66,7 +66,8 @@ using MotionControllerDispatchTarget = can::dispatch::DispatchParseTarget<
     can::messages::MotorPositionRequest,
     can::messages::UpdateMotorPositionEstimationRequest,
     can::messages::GetMotorUsageRequest,
-    can::messages::MotorDriverErrorEncountered>;
+    can::messages::MotorDriverErrorEncountered,
+    can::messages::ResetMotorDriverErrorHandling>;
 
 using GearMotionControllerDispatchTarget = can::dispatch::DispatchParseTarget<
     gear_motion_handler::GearMotorMotionHandler<gear_motor_tasks::QueueClient>,

From bfa0b85b1154998ef653f2697ae5b8cec1106976 Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Tue, 31 Oct 2023 21:59:14 -0400
Subject: [PATCH 14/68] PR review changes part 2

---
 include/can/core/ids.hpp                      |  1 +
 include/can/core/message_handlers/motion.hpp  |  3 +-
 .../brushed_motion_controller.hpp             |  8 ++-
 .../brushed_motor_interrupt_handler.hpp       |  2 +-
 .../core/motor_hardware_interface.hpp         | 11 +++-
 .../core/stepper_motor/motion_controller.hpp  | 20 ++++---
 .../stepper_motor/motor_interrupt_handler.hpp | 17 +++---
 .../core/stepper_motor/tmc2130_driver.hpp     |  4 +-
 .../core/stepper_motor/tmc2160_driver.hpp     |  4 +-
 .../tasks/gear_tmc2160_motor_driver_task.hpp  |  6 +-
 .../core/tasks/motion_controller_task.hpp     | 15 ++---
 .../core/tasks/tmc2130_motor_driver_task.hpp  |  8 ++-
 .../core/tasks/tmc2160_motor_driver_task.hpp  |  6 +-
 .../brushed_motor/brushed_motor_hardware.hpp  | 20 +++++--
 .../firmware/stepper_motor/motor_hardware.hpp | 22 ++++---
 .../simulation/sim_motor_hardware_iface.hpp   | 58 ++++++++++++++-----
 .../tests/mock_brushed_motor_components.hpp   | 20 ++++---
 .../tests/mock_motor_hardware.hpp             | 20 ++++---
 .../test_brushed_motor_interrupt_handler.cpp  |  3 +-
 .../tests/test_motor_interrupt_handler.cpp    |  3 +-
 20 files changed, 163 insertions(+), 88 deletions(-)

diff --git a/include/can/core/ids.hpp b/include/can/core/ids.hpp
index a3b6764fa..8c5c0d338 100644
--- a/include/can/core/ids.hpp
+++ b/include/can/core/ids.hpp
@@ -160,6 +160,7 @@ enum class ErrorCode {
 
 /** Error Severity levels. */
 enum class ErrorSeverity {
+    none = 0x0,
     warning = 0x1,
     recoverable = 0x2,
     unrecoverable = 0x3,
diff --git a/include/can/core/message_handlers/motion.hpp b/include/can/core/message_handlers/motion.hpp
index e360f91ec..f9e23cb80 100644
--- a/include/can/core/message_handlers/motion.hpp
+++ b/include/can/core/message_handlers/motion.hpp
@@ -18,7 +18,8 @@ class MotionHandler {
                      GetMotionConstraintsRequest, SetMotionConstraints,
                      ReadLimitSwitchRequest, MotorPositionRequest,
                      UpdateMotorPositionEstimationRequest, GetMotorUsageRequest,
-                     MotorDriverErrorEncountered, ResetMotorDriverErrorHandling>;
+                     MotorDriverErrorEncountered,
+                     ResetMotorDriverErrorHandling>;
 
     MotionHandler(MotionTaskClient &motion_client)
         : motion_client{motion_client} {}
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 0c57df98f..379d74b6d 100644
--- a/include/motor-control/core/brushed_motor/brushed_motion_controller.hpp
+++ b/include/motor-control/core/brushed_motor/brushed_motion_controller.hpp
@@ -101,13 +101,15 @@ class MotionController {
         enabled = false;
     }
 
-    void stop(can::ids::ErrorSeverity error_severity =
-                  can::ids::ErrorSeverity::warning) {
+    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(static_cast<uint8_t>(error_severity));
+                hardware.request_cancel(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 798010913..06b02100f 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
@@ -201,7 +201,7 @@ class BrushedMotorInterruptHandler {
         } else if (estop_triggered()) {
             in_estop = true;
             cancel_and_clear_moves(can::ids::ErrorCode::estop_detected);
-        } else if (hardware.has_cancel_request() != 0U) {
+        } else if (hardware.has_cancel_request().code != 0U) {
             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 f375cd68c..b888df384 100644
--- a/include/motor-control/core/motor_hardware_interface.hpp
+++ b/include/motor-control/core/motor_hardware_interface.hpp
@@ -72,6 +72,12 @@ class UsageEEpromConfig {
     size_t num_keys = 0;
 };
 
+// std::optional usage? See HardwareConfig struct
+struct __attribute__((packed)) CancelRequest {
+    uint8_t severity;
+    uint8_t code;
+};
+
 class MotorHardwareIface {
   public:
     MotorHardwareIface() = default;
@@ -98,8 +104,9 @@ class MotorHardwareIface {
     virtual void enable_encoder() = 0;
     virtual void disable_encoder() = 0;
 
-    virtual auto has_cancel_request() -> uint8_t = 0;
-    virtual void request_cancel(uint8_t error_severity) = 0;
+    virtual auto has_cancel_request() -> CancelRequest = 0;
+    virtual void request_cancel(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;
 
diff --git a/include/motor-control/core/stepper_motor/motion_controller.hpp b/include/motor-control/core/stepper_motor/motion_controller.hpp
index a5021f3be..4d990317e 100644
--- a/include/motor-control/core/stepper_motor/motion_controller.hpp
+++ b/include/motor-control/core/stepper_motor/motion_controller.hpp
@@ -107,17 +107,17 @@ class MotionController {
         return update_queue.try_write(can_msg);
     }
 
-    void stop(can::ids::ErrorSeverity error_severity =
-                  can::ids::ErrorSeverity::warning) {
+    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(static_cast<uint8_t>(error_severity));
+            hardware.request_cancel(error_severity, error_code);
         }
     }
 
-    void clear_cancel_request() {
-        hardware.clear_cancel_request();
-    }
+    void clear_cancel_request() { hardware.clear_cancel_request(); }
 
     auto is_timer_interrupt_running() -> bool {
         return hardware.is_timer_interrupt_running();
@@ -268,14 +268,16 @@ class PipetteMotionController {
         return false;
     }
 
-    void stop(can::ids::ErrorSeverity error_severity =
-                  can::ids::ErrorSeverity::warning) {
+    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(static_cast<uint8_t>(error_severity));
+            hardware.request_cancel(error_severity, error_code);
         }
     }
 
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 6b3829f70..1d5bcdaca 100644
--- a/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp
+++ b/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp
@@ -187,7 +187,8 @@ class MotorInterruptHandler {
 
     void run_interrupt() {
         // handle various error states
-        uint8_t has_cancel_request = hardware.has_cancel_request();
+        motor_hardware::CancelRequest cancel_request =
+            hardware.has_cancel_request();
         if (clear_queue_until_empty) {
             // If we were executing a move when estop asserted, and
             // what's in the queue is the remaining enqueued moves from
@@ -206,14 +207,12 @@ class MotorInterruptHandler {
         } else if (estop_triggered()) {
             cancel_and_clear_moves(can::ids::ErrorCode::estop_detected);
             in_estop = true;
-        } else if (has_cancel_request != 0U) {
-            if (has_cancel_request ==
-                static_cast<uint8_t>(can::ids::ErrorSeverity::unrecoverable)) {
-                cancel_and_clear_moves(can::ids::ErrorCode::motor_driver_error_detected); // use cancel_request.code?
-            } else {
-                cancel_and_clear_moves(can::ids::ErrorCode::stop_requested,
-                                       can::ids::ErrorSeverity::warning);
-            }
+        } else if (cancel_request.code !=
+                   0U) {  // is this correct? Should be zero-initialized. Check,
+                          // confirm!
+            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();
diff --git a/include/motor-control/core/stepper_motor/tmc2130_driver.hpp b/include/motor-control/core/stepper_motor/tmc2130_driver.hpp
index 887f9cd02..7c3a74e7d 100644
--- a/include/motor-control/core/stepper_motor/tmc2130_driver.hpp
+++ b/include/motor-control/core/stepper_motor/tmc2130_driver.hpp
@@ -52,8 +52,8 @@ class TMC2130 {
     auto operator=(const TMC2130&& c) = delete;
     ~TMC2130() = default;
 
-    auto read(Registers addr, uint32_t command_data, uint32_t message_index, uint8_t tags = 0)
-        -> 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);
         uint32_t token = spi::utils::build_token(converted_addr, tags);
         _spi_manager.read(token, command_data, _task_queue, _cs_intf,
diff --git a/include/motor-control/core/stepper_motor/tmc2160_driver.hpp b/include/motor-control/core/stepper_motor/tmc2160_driver.hpp
index 99b991345..63f9ad20e 100644
--- a/include/motor-control/core/stepper_motor/tmc2160_driver.hpp
+++ b/include/motor-control/core/stepper_motor/tmc2160_driver.hpp
@@ -53,8 +53,8 @@ class TMC2160 {
     auto operator=(const TMC2160&& c) = delete;
     ~TMC2160() = default;
 
-    auto read(Registers addr, uint32_t command_data, uint32_t message_index, uint8_t tags = 0)
-        -> 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);
         uint32_t token = spi::utils::build_token(converted_addr, tags);
         _spi_manager.read(token, command_data, _task_queue, _cs_intf,
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 966925a3e..1760a93c4 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
@@ -82,7 +82,8 @@ class MotorDriverMessageHandler {
         LOG("Received read motor driver request: addr=%d", m.reg_address);
         uint32_t data = 0;
         if (tmc2160::registers::is_valid_address(m.reg_address)) {
-            driver.read(tmc2160::registers::Registers(m.reg_address), data, m.message_index);
+            driver.read(tmc2160::registers::Registers(m.reg_address), data,
+                        m.message_index);
         }
     }
 
@@ -91,7 +92,8 @@ class MotorDriverMessageHandler {
         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);
+        driver.read(tmc2160::registers::Registers::DRVSTATUS, data,
+                    m.message_index, tag_byte);
     }
 
     void handle(const can::messages::GearWriteMotorCurrentRequest& m) {
diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp
index 08af6c3fe..adaf15c82 100644
--- a/include/motor-control/core/tasks/motion_controller_task.hpp
+++ b/include/motor-control/core/tasks/motion_controller_task.hpp
@@ -147,17 +147,19 @@ class MotionControllerMessageHandler {
     void handle(const can::messages::MotorDriverErrorEncountered& m) {
         if (!driver_error_handled()) {
             // check if gpio is low before making controller calls
-            controller.stop(can::ids::ErrorSeverity::unrecoverable);
+            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});
+                        .error_code =
+                            can::ids::ErrorCode::motor_driver_error_detected});
                 driver_client.send_motor_driver_queue(
-                    can::messages::ReadMotorDriverErrorStatus{.message_index =
-                                                                m.message_index});
+                    can::messages::ReadMotorDriverErrorStatus{
+                        .message_index = m.message_index});
             }
         }
     }
@@ -225,9 +227,8 @@ class MotionControllerTask {
 
     // also create top level query msg
     void run_diag0_interrupt() {
-        static_cast<void>(
-            queue.try_write_isr(can::messages::MotorDriverErrorEncountered{
-                .message_index = 0}));
+        static_cast<void>(queue.try_write_isr(
+            can::messages::MotorDriverErrorEncountered{.message_index = 0}));
     }
 
   private:
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 3821d5f1d..764356033 100644
--- a/include/motor-control/core/tasks/tmc2130_motor_driver_task.hpp
+++ b/include/motor-control/core/tasks/tmc2130_motor_driver_task.hpp
@@ -81,16 +81,18 @@ class MotorDriverMessageHandler {
         LOG("Received read motor driver request: addr=%d", m.reg_address);
         uint32_t data = 0;
         if (tmc2130::registers::is_valid_address(m.reg_address)) {
-            driver.read(tmc2130::registers::Registers(m.reg_address), data, m.message_index);
+            driver.read(tmc2130::registers::Registers(m.reg_address), data,
+                        m.message_index);
         }
     }
 
     void handle(const can::messages::ReadMotorDriverErrorStatus& m) {
         LOG("Received read motor driver error register request");
-        uint32_t data = 0;        
+        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);
+        driver.read(tmc2130::registers::Registers::DRVSTATUS, data,
+                    m.message_index, tag_byte);
     }
 
     void handle(const can::messages::WriteMotorCurrentRequest& m) {
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 9030248b6..568c7d4c6 100644
--- a/include/motor-control/core/tasks/tmc2160_motor_driver_task.hpp
+++ b/include/motor-control/core/tasks/tmc2160_motor_driver_task.hpp
@@ -94,7 +94,8 @@ class MotorDriverMessageHandler {
         LOG("Received read motor driver request: addr=%d", m.reg_address);
         uint32_t data = 0;
         if (tmc2160::registers::is_valid_address(m.reg_address)) {
-            driver.read(tmc2160::registers::Registers(m.reg_address), data, m.message_index);
+            driver.read(tmc2160::registers::Registers(m.reg_address), data,
+                        m.message_index);
         }
     }
 
@@ -103,7 +104,8 @@ class MotorDriverMessageHandler {
         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);
+        driver.read(tmc2160::registers::Registers::DRVSTATUS, data,
+                    m.message_index, tag_byte);
     }
 
     void handle(const can::messages::WriteMotorCurrentRequest& m) {
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 109642127..c42b2dba8 100644
--- a/include/motor-control/firmware/brushed_motor/brushed_motor_hardware.hpp
+++ b/include/motor-control/firmware/brushed_motor/brushed_motor_hardware.hpp
@@ -45,6 +45,7 @@ class BrushedMotorHardware : public BrushedMotorHardwareIface {
           controller_loop{config.pid_kp,  config.pid_ki,
                           config.pid_kd,  1.F / config.encoder_interrupt_freq,
                           config.wl_high, config.wl_low},
+          cancel_request(),
           eeprom_config{eeprom_config},
           stopwatch_handle{stopwatch_handle} {}
     BrushedMotorHardware(const BrushedMotorHardware&) = delete;
@@ -78,14 +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() -> uint8_t final {
-        return cancel_request.exchange(0);
+    // need any std::optional usage?
+    auto has_cancel_request() -> CancelRequest final {
+        CancelRequest exchange_request;
+        return cancel_request.exchange(exchange_request);
     }
-    void request_cancel(uint8_t error_severity) final {
-        cancel_request.store(error_severity);
+    void request_cancel(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 {
-        cancel_request.store(0);
+        CancelRequest clear_request;
+        cancel_request.store(clear_request);
     }
     auto get_usage_eeprom_config() -> const UsageEEpromConfig& final {
         return eeprom_config;
@@ -106,7 +114,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<uint8_t> cancel_request = 0;
+    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 00e31933c..4846d3400 100644
--- a/include/motor-control/firmware/stepper_motor/motor_hardware.hpp
+++ b/include/motor-control/firmware/stepper_motor/motor_hardware.hpp
@@ -31,7 +31,8 @@ class MotorHardware : public StepperMotorHardwareIface {
         : pins(config),
           tim_handle(timer_handle),
           enc_handle(encoder_handle),
-          eeprom_config(eeprom_config) {}
+          eeprom_config(eeprom_config),
+          cancel_request() {}
     MotorHardware(const MotorHardware&) = delete;
     auto operator=(const MotorHardware&) -> MotorHardware& = delete;
     MotorHardware(MotorHardware&&) = delete;
@@ -57,14 +58,21 @@ class MotorHardware : public StepperMotorHardwareIface {
     void disable_encoder() final;
     void enable_encoder() final;
 
-    auto has_cancel_request() -> uint8_t final {
-        return cancel_request.exchange(0);
+    // need any std::optional usage?
+    auto has_cancel_request() -> CancelRequest final {
+        CancelRequest exchange_request;
+        return cancel_request.exchange(exchange_request);
     }
-    void request_cancel(uint8_t error_severity) final {
-        cancel_request.store(error_severity);
+    void request_cancel(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 {
-        cancel_request.store(0);
+        CancelRequest clear_request;
+        cancel_request.store(clear_request);
     }
 
     auto get_usage_eeprom_config() -> const UsageEEpromConfig& final {
@@ -83,7 +91,7 @@ class MotorHardware : public StepperMotorHardwareIface {
     void* enc_handle;
     const UsageEEpromConfig& eeprom_config;
     std::atomic<int32_t> motor_encoder_overflow_count = 0;
-    std::atomic<uint8_t> cancel_request = 0;
+    std::atomic<CancelRequest> cancel_request;
     static constexpr uint32_t ENCODER_OVERFLOW_PULSES_BIT = 0x1 << 31;
 };
 
diff --git a/include/motor-control/simulation/sim_motor_hardware_iface.hpp b/include/motor-control/simulation/sim_motor_hardware_iface.hpp
index c858d6fa3..fd244af27 100644
--- a/include/motor-control/simulation/sim_motor_hardware_iface.hpp
+++ b/include/motor-control/simulation/sim_motor_hardware_iface.hpp
@@ -87,11 +87,20 @@ 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() -> uint8_t final {
-        return cancel_request.exchange(0);
+    auto has_cancel_request() -> motor_hardware::CancelRequest final {
+        motor_hardware::CancelRequest exchange_request;
+        return cancel_request.exchange(exchange_request);
     }
-    void request_cancel(uint8_t error_severity) final {
-        cancel_request.store(error_severity);
+    void request_cancel(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);
     }
     auto get_usage_eeprom_config()
         -> motor_hardware::UsageEEpromConfig & final {
@@ -108,7 +117,7 @@ class SimMotorHardwareIface : public motor_hardware::StepperMotorHardwareIface {
     Direction _direction = Direction::POSITIVE;
     float _encoder_ticks_per_pulse = 0;
     bool estop_detected = false;
-    std::atomic<uint8_t> cancel_request = 0;
+    std::atomic<motor_hardware::CancelRequest> cancel_request = {};
     motor_hardware::UsageEEpromConfig eeprom_config = {
         std::array<UsageRequestSet, 1>{UsageRequestSet{
             .eeprom_key = 0,
@@ -179,13 +188,21 @@ class SimBrushedMotorHardwareIface
         return ret;
     }
 
-    auto has_cancel_request() -> uint8_t final {
-        return cancel_request.exchange(0);
+    auto has_cancel_request() -> motor_hardware::CancelRequest final {
+        motor_hardware::CancelRequest exchange_request;
+        return cancel_request.exchange(exchange_request);
     }
-    void request_cancel(uint8_t error_severity) final {
-        cancel_request.store(error_severity);
+    void request_cancel(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 disable_encoder() final {}
     void enable_encoder() final {}
 
@@ -205,7 +222,7 @@ class SimBrushedMotorHardwareIface
     StateManagerHandle _state_manager = nullptr;
     MoveMessageHardware _id;
     bool estop_detected = false;
-    std::atomic<uint8_t> cancel_request = 0;
+    std::atomic<motor_hardware::CancelRequest> cancel_request = {};
     BrushedMotorState motor_state = BrushedMotorState::UNHOMED;
     motor_hardware::UsageEEpromConfig eeprom_config{
         std::array<UsageRequestSet, 1>{UsageRequestSet{
@@ -271,11 +288,20 @@ class SimGearMotorHardwareIface
     bool check_estop_in() final { return estop_detected; }
 
     void set_estop(bool estop_pressed) { estop_detected = estop_pressed; }
-    auto has_cancel_request() -> uint8_t final {
-        return cancel_request.exchange(0);
+    auto has_cancel_request() -> motor_hardware::CancelRequest final {
+        motor_hardware::CancelRequest exchange_request;
+        return cancel_request.exchange(exchange_request);
+    }
+    void request_cancel(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 request_cancel(uint8_t error_severity) final {
-        cancel_request.store(error_severity);
+    void clear_cancel_request() final {
+        motor_hardware::CancelRequest clear_request;
+        cancel_request.store(clear_request);
     }
 
     auto get_usage_eeprom_config()
@@ -293,7 +319,7 @@ class SimGearMotorHardwareIface
     Direction _direction = Direction::POSITIVE;
     float _encoder_ticks_per_pulse = 0;
     bool estop_detected = false;
-    std::atomic<uint8_t> cancel_request = 0;
+    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 e0c467061..95b95d045 100644
--- a/include/motor-control/tests/mock_brushed_motor_components.hpp
+++ b/include/motor-control/tests/mock_brushed_motor_components.hpp
@@ -71,16 +71,22 @@ 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() -> uint8_t final {
-        uint8_t old_request = cancel_request;
-        cancel_request = 0;
+    auto has_cancel_request() -> CancelRequest final {
+        CancelRequest old_request = cancel_request;
+        CancelRequest exchange_request{};
+        cancel_request = exchange_request;
         return old_request;
     }
-    void request_cancel(uint8_t error_severity) final {
-        cancel_request = error_severity;
+    void request_cancel(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 {
-        cancel_request = 0;
+        CancelRequest clear_request{};
+        cancel_request = clear_request;
     }
     void set_timer_interrupt_running(bool is_running) {
         timer_interrupt_running = is_running;
@@ -113,7 +119,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};
-    uint8_t cancel_request = 0;
+    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_hardware.hpp b/include/motor-control/tests/mock_motor_hardware.hpp
index 6c681e6cd..96d0fb42d 100644
--- a/include/motor-control/tests/mock_motor_hardware.hpp
+++ b/include/motor-control/tests/mock_motor_hardware.hpp
@@ -39,16 +39,22 @@ 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() -> uint8_t final {
-        uint8_t old_request = cancel_request;
-        cancel_request = 0;
+    auto has_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(uint8_t error_severity) final {
-        cancel_request = error_severity;
+    void request_cancel(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 {
-        cancel_request = 0;
+        motor_hardware::CancelRequest clear_request{};
+        cancel_request = clear_request;
     }
     void sim_set_timer_interrupt_running(bool is_running) {
         mock_timer_interrupt_running = is_running;
@@ -70,7 +76,7 @@ class MockMotorHardware : public motor_hardware::StepperMotorHardwareIface {
     bool mock_dir_value = false;
     uint8_t finished_move_id = 0x0;
     int32_t test_pulses = 0x0;
-    uint8_t cancel_request = 0;
+    motor_hardware::CancelRequest cancel_request = {};
     bool mock_timer_interrupt_running = true;
     motor_hardware::UsageEEpromConfig eeprom_config =
         motor_hardware::UsageEEpromConfig{
diff --git a/motor-control/tests/test_brushed_motor_interrupt_handler.cpp b/motor-control/tests/test_brushed_motor_interrupt_handler.cpp
index a8f08c1d9..767a85ce8 100644
--- a/motor-control/tests/test_brushed_motor_interrupt_handler.cpp
+++ b/motor-control/tests/test_brushed_motor_interrupt_handler.cpp
@@ -512,7 +512,8 @@ SCENARIO("handler recovers from error state") {
         test_objs.handler.error_handled = true;
 
         WHEN("a cancel request is received") {
-            test_objs.hw.request_cancel(1);
+            test_objs.hw.request_cancel(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_motor_interrupt_handler.cpp b/motor-control/tests/test_motor_interrupt_handler.cpp
index b0f2bd75c..5cd8d135d 100644
--- a/motor-control/tests/test_motor_interrupt_handler.cpp
+++ b/motor-control/tests/test_motor_interrupt_handler.cpp
@@ -44,7 +44,8 @@ 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(1);
+            test_objs.hw.request_cancel(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);

From 352f8c8b532786c11203c6311135475baabc24ed Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Tue, 31 Oct 2023 22:38:02 -0400
Subject: [PATCH 15/68] lint fixes

---
 .../firmware/brushed_motor/brushed_motor_hardware.hpp    | 7 +++----
 .../firmware/stepper_motor/motor_hardware.hpp            | 9 ++++-----
 2 files changed, 7 insertions(+), 9 deletions(-)

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 c42b2dba8..47a5c19bc 100644
--- a/include/motor-control/firmware/brushed_motor/brushed_motor_hardware.hpp
+++ b/include/motor-control/firmware/brushed_motor/brushed_motor_hardware.hpp
@@ -45,7 +45,6 @@ class BrushedMotorHardware : public BrushedMotorHardwareIface {
           controller_loop{config.pid_kp,  config.pid_ki,
                           config.pid_kd,  1.F / config.encoder_interrupt_freq,
                           config.wl_high, config.wl_low},
-          cancel_request(),
           eeprom_config{eeprom_config},
           stopwatch_handle{stopwatch_handle} {}
     BrushedMotorHardware(const BrushedMotorHardware&) = delete;
@@ -81,7 +80,7 @@ class BrushedMotorHardware : public BrushedMotorHardwareIface {
     auto get_stay_enabled() -> bool final { return stay_enabled; }
     // need any std::optional usage?
     auto has_cancel_request() -> CancelRequest final {
-        CancelRequest exchange_request;
+        CancelRequest exchange_request = {};
         return cancel_request.exchange(exchange_request);
     }
     void request_cancel(can::ids::ErrorSeverity error_severity,
@@ -92,7 +91,7 @@ class BrushedMotorHardware : public BrushedMotorHardwareIface {
         cancel_request.store(update_request);
     }
     void clear_cancel_request() final {
-        CancelRequest clear_request;
+        CancelRequest clear_request = {};
         cancel_request.store(clear_request);
     }
     auto get_usage_eeprom_config() -> const UsageEEpromConfig& final {
@@ -114,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<CancelRequest> cancel_request;
+    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 4846d3400..ef43e909b 100644
--- a/include/motor-control/firmware/stepper_motor/motor_hardware.hpp
+++ b/include/motor-control/firmware/stepper_motor/motor_hardware.hpp
@@ -31,8 +31,7 @@ class MotorHardware : public StepperMotorHardwareIface {
         : pins(config),
           tim_handle(timer_handle),
           enc_handle(encoder_handle),
-          eeprom_config(eeprom_config),
-          cancel_request() {}
+          eeprom_config(eeprom_config) {}
     MotorHardware(const MotorHardware&) = delete;
     auto operator=(const MotorHardware&) -> MotorHardware& = delete;
     MotorHardware(MotorHardware&&) = delete;
@@ -60,7 +59,7 @@ class MotorHardware : public StepperMotorHardwareIface {
 
     // need any std::optional usage?
     auto has_cancel_request() -> CancelRequest final {
-        CancelRequest exchange_request;
+        CancelRequest exchange_request = {};
         return cancel_request.exchange(exchange_request);
     }
     void request_cancel(can::ids::ErrorSeverity error_severity,
@@ -71,7 +70,7 @@ class MotorHardware : public StepperMotorHardwareIface {
         cancel_request.store(update_request);
     }
     void clear_cancel_request() final {
-        CancelRequest clear_request;
+        CancelRequest clear_request = {};
         cancel_request.store(clear_request);
     }
 
@@ -91,7 +90,7 @@ class MotorHardware : public StepperMotorHardwareIface {
     void* enc_handle;
     const UsageEEpromConfig& eeprom_config;
     std::atomic<int32_t> motor_encoder_overflow_count = 0;
-    std::atomic<CancelRequest> cancel_request;
+    std::atomic<CancelRequest> cancel_request = {};
     static constexpr uint32_t ENCODER_OVERFLOW_PULSES_BIT = 0x1 << 31;
 };
 

From d2fd44ca8c9201f890c72a4e46344657c1899f08 Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Thu, 2 Nov 2023 23:15:55 -0400
Subject: [PATCH 16/68] PR review changes part 3

---
 gantry/firmware/motor_hardware.c              |  2 +-
 gantry/firmware/stm32g4xx_it.c                |  2 +-
 head/core/can_task.cpp                        |  1 +
 include/can/core/ids.hpp                      |  1 +
 include/can/core/message_handlers/motion.hpp  |  2 +-
 include/can/core/messages.hpp                 |  3 +++
 include/gantry/core/can_task.hpp              |  1 +
 include/gripper/core/can_task.hpp             |  1 +
 .../brushed_motion_controller.hpp             |  2 +-
 .../brushed_motor_interrupt_handler.hpp       |  6 +++--
 .../core/motor_hardware_interface.hpp         |  7 +++---
 .../core/stepper_motor/motion_controller.hpp  |  8 ++++--
 .../stepper_motor/motor_interrupt_handler.hpp |  9 ++++---
 .../core/stepper_motor/tmc2160_driver.hpp     |  7 ++----
 include/motor-control/core/tasks/messages.hpp |  1 +
 .../core/tasks/motion_controller_task.hpp     | 15 ++++++++---
 .../core/tasks/tmc2130_motor_driver_task.hpp  | 25 ++++++++++++++-----
 .../brushed_motor/brushed_motor_hardware.hpp  |  7 +++---
 .../firmware/stepper_motor/motor_hardware.hpp |  7 +++---
 .../tests/mock_brushed_motor_components.hpp   |  6 +++--
 .../tests/mock_motor_hardware.hpp             |  6 +++--
 include/pipettes/core/dispatch_builder.hpp    |  1 +
 .../brushed_motor/brushed_motor_hardware.cpp  |  4 +++
 .../firmware/stepper_motor/motor_hardware.cpp |  4 +++
 .../test_brushed_motor_interrupt_handler.cpp  |  2 +-
 .../tests/test_motor_interrupt_handler.cpp    |  2 +-
 26 files changed, 91 insertions(+), 41 deletions(-)

diff --git a/gantry/firmware/motor_hardware.c b/gantry/firmware/motor_hardware.c
index f2f83eb9f..46b10a31a 100644
--- a/gantry/firmware/motor_hardware.c
+++ b/gantry/firmware/motor_hardware.c
@@ -59,7 +59,7 @@ void HAL_SPI_MspInit(SPI_HandleTypeDef* hspi) {
 
         // Diag0
         GPIO_InitStruct.Pin = GPIO_PIN_5;
-        GPIO_InitStruct.Mode = GPIO_MODE_IT_FALLING;
+        GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING_FALLING;
         GPIO_InitStruct.Pull = GPIO_PULLUP;
         HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
     }
diff --git a/gantry/firmware/stm32g4xx_it.c b/gantry/firmware/stm32g4xx_it.c
index 1a608a8ec..07463106c 100644
--- a/gantry/firmware/stm32g4xx_it.c
+++ b/gantry/firmware/stm32g4xx_it.c
@@ -147,7 +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); } // source this from somewhere?
+void EXTI9_5_IRQHandler(void) { HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_5); }
 
 
 extern void xPortSysTickHandler(void);
diff --git a/head/core/can_task.cpp b/head/core/can_task.cpp
index 726a7891a..b0fe76666 100644
--- a/head/core/can_task.cpp
+++ b/head/core/can_task.cpp
@@ -45,6 +45,7 @@ using MotionControllerDispatchTarget = can::dispatch::DispatchParseTarget<
     can::messages::MotorPositionRequest,
     can::messages::UpdateMotorPositionEstimationRequest,
     can::messages::GetMotorUsageRequest,
+    can::messages::RouteMotorDriverInterrupt,
     can::messages::MotorDriverErrorEncountered,
     can::messages::ResetMotorDriverErrorHandling>;
 using SystemDispatchTarget = can::dispatch::DispatchParseTarget<
diff --git a/include/can/core/ids.hpp b/include/can/core/ids.hpp
index 8c5c0d338..7e9620ebf 100644
--- a/include/can/core/ids.hpp
+++ b/include/can/core/ids.hpp
@@ -65,6 +65,7 @@ enum class MessageId {
     read_motor_driver_error_status = 0x37,
     read_motor_driver_error_register_response = 0x38,
     reset_motor_driver_error_handling = 0x39,
+    route_motor_driver_interrupt = 0x3a,
     set_brushed_motor_vref_request = 0x40,
     set_brushed_motor_pwm_request = 0x41,
     gripper_grip_request = 0x42,
diff --git a/include/can/core/message_handlers/motion.hpp b/include/can/core/message_handlers/motion.hpp
index f9e23cb80..6a59f55e5 100644
--- a/include/can/core/message_handlers/motion.hpp
+++ b/include/can/core/message_handlers/motion.hpp
@@ -18,7 +18,7 @@ class MotionHandler {
                      GetMotionConstraintsRequest, SetMotionConstraints,
                      ReadLimitSwitchRequest, MotorPositionRequest,
                      UpdateMotorPositionEstimationRequest, GetMotorUsageRequest,
-                     MotorDriverErrorEncountered,
+                     RouteMotorDriverInterrupt, MotorDriverErrorEncountered,
                      ResetMotorDriverErrorHandling>;
 
     MotionHandler(MotionTaskClient &motion_client)
diff --git a/include/can/core/messages.hpp b/include/can/core/messages.hpp
index 410580743..1d19cadbe 100644
--- a/include/can/core/messages.hpp
+++ b/include/can/core/messages.hpp
@@ -198,6 +198,9 @@ using MotorPositionRequest = Empty<MessageId::motor_position_request>;
 using UpdateMotorPositionEstimationRequest =
     Empty<MessageId::update_motor_position_estimation_request>;
 
+using RouteMotorDriverInterrupt =
+    Empty<MessageId::route_motor_driver_interrupt>;
+
 using MotorDriverErrorEncountered =
     Empty<MessageId::motor_driver_error_encountered>;
 
diff --git a/include/gantry/core/can_task.hpp b/include/gantry/core/can_task.hpp
index 5cea6d627..e432580b0 100644
--- a/include/gantry/core/can_task.hpp
+++ b/include/gantry/core/can_task.hpp
@@ -38,6 +38,7 @@ using MotionControllerDispatchTarget = can::dispatch::DispatchParseTarget<
     can::messages::MotorPositionRequest,
     can::messages::UpdateMotorPositionEstimationRequest,
     can::messages::GetMotorUsageRequest,
+    can::messages::RouteMotorDriverInterrupt,
     can::messages::MotorDriverErrorEncountered,
     can::messages::ResetMotorDriverErrorHandling>;
 using SystemDispatchTarget = can::dispatch::DispatchParseTarget<
diff --git a/include/gripper/core/can_task.hpp b/include/gripper/core/can_task.hpp
index c08d190e1..e3a61ddef 100644
--- a/include/gripper/core/can_task.hpp
+++ b/include/gripper/core/can_task.hpp
@@ -34,6 +34,7 @@ using MotionControllerDispatchTarget = can::dispatch::DispatchParseTarget<
     can::messages::MotorPositionRequest,
     can::messages::UpdateMotorPositionEstimationRequest,
     can::messages::GetMotorUsageRequest,
+    can::messages::RouteMotorDriverInterrupt,
     can::messages::MotorDriverErrorEncountered,
     can::messages::ResetMotorDriverErrorHandling>;
 using SystemDispatchTarget = can::dispatch::DispatchParseTarget<
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 379d74b6d..2bad0aa77 100644
--- a/include/motor-control/core/brushed_motor/brushed_motion_controller.hpp
+++ b/include/motor-control/core/brushed_motor/brushed_motion_controller.hpp
@@ -109,7 +109,7 @@ class MotionController {
         // 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(error_severity, error_code);
+                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 06b02100f..5e4e0b28f 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
@@ -184,7 +184,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;
@@ -201,7 +201,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().code != 0U) {
+        } 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 b888df384..96f5f60ca 100644
--- a/include/motor-control/core/motor_hardware_interface.hpp
+++ b/include/motor-control/core/motor_hardware_interface.hpp
@@ -96,6 +96,7 @@ class MotorHardwareIface {
     virtual void read_limit_switch() = 0;
     virtual void read_estop_in() = 0;
     virtual void read_sync_in() = 0;
+    virtual bool read_tmc_diag0() = 0;
     virtual auto get_encoder_pulses() -> int32_t = 0;
     virtual void reset_encoder_pulses() = 0;
     virtual void start_timer_interrupt() = 0;
@@ -104,9 +105,9 @@ class MotorHardwareIface {
     virtual void enable_encoder() = 0;
     virtual void disable_encoder() = 0;
 
-    virtual auto has_cancel_request() -> CancelRequest = 0;
-    virtual void request_cancel(can::ids::ErrorSeverity error_severity,
-                                can::ids::ErrorCode error_code) = 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;
 
diff --git a/include/motor-control/core/stepper_motor/motion_controller.hpp b/include/motor-control/core/stepper_motor/motion_controller.hpp
index 4d990317e..3c46cfd5a 100644
--- a/include/motor-control/core/stepper_motor/motion_controller.hpp
+++ b/include/motor-control/core/stepper_motor/motion_controller.hpp
@@ -113,7 +113,7 @@ class MotionController {
         can::ids::ErrorCode error_code = can::ids::ErrorCode::stop_requested) {
         queue.reset();
         if (hardware.is_timer_interrupt_running()) {
-            hardware.request_cancel(error_severity, error_code);
+            hardware.set_cancel_request(error_severity, error_code);
         }
     }
 
@@ -137,6 +137,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() {
         // check motor driver error register
         hardware.start_timer_interrupt();
@@ -277,7 +279,7 @@ class PipetteMotionController {
         // 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(error_severity, error_code);
+            hardware.set_cancel_request(error_severity, error_code);
         }
     }
 
@@ -285,6 +287,8 @@ class PipetteMotionController {
 
     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 1d5bcdaca..12f15900d 100644
--- a/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp
+++ b/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp
@@ -188,7 +188,7 @@ class MotorInterruptHandler {
     void run_interrupt() {
         // handle various error states
         motor_hardware::CancelRequest cancel_request =
-            hardware.has_cancel_request();
+            hardware.get_cancel_request();
         if (clear_queue_until_empty) {
             // If we were executing a move when estop asserted, and
             // what's in the queue is the remaining enqueued moves from
@@ -207,9 +207,10 @@ class MotorInterruptHandler {
         } else if (estop_triggered()) {
             cancel_and_clear_moves(can::ids::ErrorCode::estop_detected);
             in_estop = true;
-        } else if (cancel_request.code !=
-                   0U) {  // is this correct? Should be zero-initialized. Check,
-                          // confirm!
+        } else if (static_cast<can::ids::ErrorCode>(cancel_request.code) !=
+                   can::ids::ErrorCode::ok) {  // is this correct? Should be
+                                               // zero-initialized. Check,
+                                               // confirm!
             cancel_and_clear_moves(
                 static_cast<can::ids::ErrorCode>(cancel_request.code),
                 static_cast<can::ids::ErrorSeverity>(cancel_request.severity));
diff --git a/include/motor-control/core/stepper_motor/tmc2160_driver.hpp b/include/motor-control/core/stepper_motor/tmc2160_driver.hpp
index 63f9ad20e..5fde77f07 100644
--- a/include/motor-control/core/stepper_motor/tmc2160_driver.hpp
+++ b/include/motor-control/core/stepper_motor/tmc2160_driver.hpp
@@ -163,9 +163,6 @@ class TMC2160 {
             case Registers::DRVSTATUS:
                 update_driver_status(0);
                 break;
-            // case Registers::DRV_CONF:
-            //     update_drv_conf(0);
-            //     break;
             default:
                 break;
         }
@@ -318,9 +315,9 @@ class TMC2160 {
      * be read, so this function gets it from the actual device.
      */
     auto update_glob_scaler(uint32_t data) -> void {
-        auto ret = read_register<GConfig>(data);
+        auto ret = read_register<GConfig>(data);  // should be GlobalScalar
         if (ret.has_value()) {
-            _registers.gconfig = ret.value();
+            _registers.gconfig = ret.value();  // should be glob_scale
         }
     }
 
diff --git a/include/motor-control/core/tasks/messages.hpp b/include/motor-control/core/tasks/messages.hpp
index 8ac661f88..688b8ca68 100644
--- a/include/motor-control/core/tasks/messages.hpp
+++ b/include/motor-control/core/tasks/messages.hpp
@@ -15,6 +15,7 @@ using MotionControlTaskMessage = std::variant<
     can::messages::HomeRequest,
     can::messages::UpdateMotorPositionEstimationRequest,
     can::messages::GetMotorUsageRequest,
+    can::messages::RouteMotorDriverInterrupt,
     can::messages::MotorDriverErrorEncountered,
     can::messages::ResetMotorDriverErrorHandling>;
 
diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp
index adaf15c82..17a5480b6 100644
--- a/include/motor-control/core/tasks/motion_controller_task.hpp
+++ b/include/motor-control/core/tasks/motion_controller_task.hpp
@@ -144,9 +144,19 @@ class MotionControllerMessageHandler {
         controller.send_usage_data(m.message_index, usage_client);
     }
 
+    void handle(const can::messages::RouteMotorDriverInterrupt& m) {
+        static_cast<void>(m);
+        if (controller.read_tmc_diag0()) {  // debounce needed?
+            handle_message(
+                can::messages::MotorDriverErrorEncountered{.message_index = 0});
+        } else {
+            handle_message(can::messages::ResetMotorDriverErrorHandling{
+                .message_index = 0});
+        }
+    }
+
     void handle(const can::messages::MotorDriverErrorEncountered& m) {
         if (!driver_error_handled()) {
-            // check if gpio is low before making controller calls
             controller.stop(can::ids::ErrorSeverity::unrecoverable,
                             can::ids::ErrorCode::motor_driver_error_detected);
             if (!controller.is_timer_interrupt_running()) {
@@ -225,10 +235,9 @@ class MotionControllerTask {
 
     [[nodiscard]] auto get_queue() const -> QueueType& { return queue; }
 
-    // also create top level query msg
     void run_diag0_interrupt() {
         static_cast<void>(queue.try_write_isr(
-            can::messages::MotorDriverErrorEncountered{.message_index = 0}));
+            can::messages::RouteMotorDriverInterrupt{.message_index = 0}));
     }
 
   private:
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 764356033..85eb32cf6 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,25 @@ 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::ReadMotorDriverErrorRegisterResponse
+                    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);
+            }
         }
     }
 
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 47a5c19bc..369223dc6 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;
+    bool read_tmc_diag0() final;
     void grip() final;
     void ungrip() final;
     void stop_pwm() final;
@@ -79,12 +80,12 @@ class BrushedMotorHardware : public BrushedMotorHardwareIface {
     void set_stay_enabled(bool state) final { stay_enabled = state; }
     auto get_stay_enabled() -> bool final { return stay_enabled; }
     // need any std::optional usage?
-    auto has_cancel_request() -> CancelRequest final {
+    auto get_cancel_request() -> CancelRequest final {
         CancelRequest exchange_request = {};
         return cancel_request.exchange(exchange_request);
     }
-    void request_cancel(can::ids::ErrorSeverity error_severity,
-                        can::ids::ErrorCode error_code) final {
+    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)};
diff --git a/include/motor-control/firmware/stepper_motor/motor_hardware.hpp b/include/motor-control/firmware/stepper_motor/motor_hardware.hpp
index ef43e909b..f40295c0a 100644
--- a/include/motor-control/firmware/stepper_motor/motor_hardware.hpp
+++ b/include/motor-control/firmware/stepper_motor/motor_hardware.hpp
@@ -51,6 +51,7 @@ class MotorHardware : public StepperMotorHardwareIface {
     void read_limit_switch() final;
     void read_estop_in() final;
     void read_sync_in() final;
+    bool read_tmc_diag0() final;
     void set_LED(bool status) final;
     auto get_encoder_pulses() -> int32_t final;
     void reset_encoder_pulses() final;
@@ -58,12 +59,12 @@ class MotorHardware : public StepperMotorHardwareIface {
     void enable_encoder() final;
 
     // need any std::optional usage?
-    auto has_cancel_request() -> CancelRequest final {
+    auto get_cancel_request() -> CancelRequest final {
         CancelRequest exchange_request = {};
         return cancel_request.exchange(exchange_request);
     }
-    void request_cancel(can::ids::ErrorSeverity error_severity,
-                        can::ids::ErrorCode error_code) final {
+    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)};
diff --git a/include/motor-control/tests/mock_brushed_motor_components.hpp b/include/motor-control/tests/mock_brushed_motor_components.hpp
index 95b95d045..b1b7c8af2 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,13 +72,13 @@ 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() -> CancelRequest final {
+    auto get_cancel_request() -> CancelRequest final {
         CancelRequest old_request = cancel_request;
         CancelRequest exchange_request{};
         cancel_request = exchange_request;
         return old_request;
     }
-    void request_cancel(can::ids::ErrorSeverity error_severity,
+    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),
@@ -108,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;
diff --git a/include/motor-control/tests/mock_motor_hardware.hpp b/include/motor-control/tests/mock_motor_hardware.hpp
index 96d0fb42d..b42e2a86d 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,13 +40,13 @@ 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() -> motor_hardware::CancelRequest final {
+    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(can::ids::ErrorSeverity error_severity,
+    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),
@@ -72,6 +73,7 @@ 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;
diff --git a/include/pipettes/core/dispatch_builder.hpp b/include/pipettes/core/dispatch_builder.hpp
index f0177d265..cf2d6d95d 100644
--- a/include/pipettes/core/dispatch_builder.hpp
+++ b/include/pipettes/core/dispatch_builder.hpp
@@ -66,6 +66,7 @@ using MotionControllerDispatchTarget = can::dispatch::DispatchParseTarget<
     can::messages::MotorPositionRequest,
     can::messages::UpdateMotorPositionEstimationRequest,
     can::messages::GetMotorUsageRequest,
+    can::messages::RouteMotorDriverInterrupt,
     can::messages::MotorDriverErrorEncountered,
     can::messages::ResetMotorDriverErrorHandling>;
 
diff --git a/motor-control/firmware/brushed_motor/brushed_motor_hardware.cpp b/motor-control/firmware/brushed_motor/brushed_motor_hardware.cpp
index 2b5d1314d..99d452504 100644
--- a/motor-control/firmware/brushed_motor/brushed_motor_hardware.cpp
+++ b/motor-control/firmware/brushed_motor/brushed_motor_hardware.cpp
@@ -58,6 +58,10 @@ 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 9b4d38151..2effd5f1f 100644
--- a/motor-control/firmware/stepper_motor/motor_hardware.cpp
+++ b/motor-control/firmware/stepper_motor/motor_hardware.cpp
@@ -48,6 +48,10 @@ 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 767a85ce8..e79411fd6 100644
--- a/motor-control/tests/test_brushed_motor_interrupt_handler.cpp
+++ b/motor-control/tests/test_brushed_motor_interrupt_handler.cpp
@@ -512,7 +512,7 @@ SCENARIO("handler recovers from error state") {
         test_objs.handler.error_handled = true;
 
         WHEN("a cancel request is received") {
-            test_objs.hw.request_cancel(can::ids::ErrorSeverity::warning,
+            test_objs.hw.set_cancel_request(can::ids::ErrorSeverity::warning,
                                         can::ids::ErrorCode::stop_requested);
             test_objs.handler.run_interrupt();
             THEN(
diff --git a/motor-control/tests/test_motor_interrupt_handler.cpp b/motor-control/tests/test_motor_interrupt_handler.cpp
index 5cd8d135d..84f8fc45d 100644
--- a/motor-control/tests/test_motor_interrupt_handler.cpp
+++ b/motor-control/tests/test_motor_interrupt_handler.cpp
@@ -44,7 +44,7 @@ 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(can::ids::ErrorSeverity::warning,
+            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") {

From 394ea1d49c47ac7f0d19e4b3709f0acb953842c8 Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Thu, 2 Nov 2023 23:37:55 -0400
Subject: [PATCH 17/68] made comments

---
 include/motor-control/core/tasks/motion_controller_task.hpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp
index 17a5480b6..4abd21fe5 100644
--- a/include/motor-control/core/tasks/motion_controller_task.hpp
+++ b/include/motor-control/core/tasks/motion_controller_task.hpp
@@ -178,6 +178,7 @@ class MotionControllerMessageHandler {
         static_cast<void>(m);
         driver_error_handled_flag.exchange(false);
         controller.clear_cancel_request();
+        // send MotorDriverErrorCleared msg to host
     }
 
     auto driver_error_handled() -> bool {
@@ -188,7 +189,7 @@ class MotionControllerMessageHandler {
     CanClient& can_client;
     UsageClient& usage_client;
     DriverClient& driver_client;
-    std::atomic<bool> driver_error_handled_flag = false;
+    std::atomic<bool> driver_error_handled_flag = false; // can get rid of this if indefinite interrupt trigger issue gets resolved
 };
 
 /**

From 90c6a50bf255411708539bf3829bbf911c8b5f0e Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Fri, 3 Nov 2023 00:20:10 -0400
Subject: [PATCH 18/68] lint and build fixes

---
 .../core/motor_hardware_interface.hpp             |  2 +-
 .../core/tasks/motion_controller_task.hpp         |  4 +++-
 .../brushed_motor/brushed_motor_hardware.hpp      |  2 +-
 .../firmware/stepper_motor/motor_hardware.hpp     |  2 +-
 .../simulation/sim_motor_hardware_iface.hpp       | 15 +++++++++------
 .../tests/mock_brushed_motor_components.hpp       |  2 +-
 .../motor-control/tests/mock_motor_hardware.hpp   |  2 +-
 .../brushed_motor/brushed_motor_hardware.cpp      |  4 +---
 .../firmware/stepper_motor/motor_hardware.cpp     |  4 +---
 .../test_brushed_motor_interrupt_handler.cpp      |  5 +++--
 .../tests/test_motor_interrupt_handler.cpp        |  5 +++--
 11 files changed, 25 insertions(+), 22 deletions(-)

diff --git a/include/motor-control/core/motor_hardware_interface.hpp b/include/motor-control/core/motor_hardware_interface.hpp
index 96f5f60ca..21a889642 100644
--- a/include/motor-control/core/motor_hardware_interface.hpp
+++ b/include/motor-control/core/motor_hardware_interface.hpp
@@ -96,7 +96,7 @@ class MotorHardwareIface {
     virtual void read_limit_switch() = 0;
     virtual void read_estop_in() = 0;
     virtual void read_sync_in() = 0;
-    virtual bool read_tmc_diag0() = 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;
diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp
index 4abd21fe5..135962495 100644
--- a/include/motor-control/core/tasks/motion_controller_task.hpp
+++ b/include/motor-control/core/tasks/motion_controller_task.hpp
@@ -189,7 +189,9 @@ class MotionControllerMessageHandler {
     CanClient& can_client;
     UsageClient& usage_client;
     DriverClient& driver_client;
-    std::atomic<bool> driver_error_handled_flag = false; // can get rid of this if indefinite interrupt trigger issue gets resolved
+    std::atomic<bool> driver_error_handled_flag =
+        false;  // can get rid of this if indefinite interrupt trigger issue
+                // gets resolved
 };
 
 /**
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 369223dc6..727c0f9e7 100644
--- a/include/motor-control/firmware/brushed_motor/brushed_motor_hardware.hpp
+++ b/include/motor-control/firmware/brushed_motor/brushed_motor_hardware.hpp
@@ -62,7 +62,7 @@ class BrushedMotorHardware : public BrushedMotorHardwareIface {
     void read_limit_switch() final;
     void read_estop_in() final;
     void read_sync_in() final;
-    bool read_tmc_diag0() final;
+    auto read_tmc_diag0() -> bool final;
     void grip() final;
     void ungrip() final;
     void stop_pwm() final;
diff --git a/include/motor-control/firmware/stepper_motor/motor_hardware.hpp b/include/motor-control/firmware/stepper_motor/motor_hardware.hpp
index f40295c0a..75c68010f 100644
--- a/include/motor-control/firmware/stepper_motor/motor_hardware.hpp
+++ b/include/motor-control/firmware/stepper_motor/motor_hardware.hpp
@@ -51,7 +51,7 @@ class MotorHardware : public StepperMotorHardwareIface {
     void read_limit_switch() final;
     void read_estop_in() final;
     void read_sync_in() final;
-    bool read_tmc_diag0() 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;
diff --git a/include/motor-control/simulation/sim_motor_hardware_iface.hpp b/include/motor-control/simulation/sim_motor_hardware_iface.hpp
index fd244af27..48db76f82 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,11 +88,11 @@ 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() -> motor_hardware::CancelRequest final {
+    auto get_cancel_request() -> motor_hardware::CancelRequest final {
         motor_hardware::CancelRequest exchange_request;
         return cancel_request.exchange(exchange_request);
     }
-    void request_cancel(can::ids::ErrorSeverity error_severity,
+    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),
@@ -148,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 {}
@@ -188,11 +190,11 @@ class SimBrushedMotorHardwareIface
         return ret;
     }
 
-    auto has_cancel_request() -> motor_hardware::CancelRequest final {
+    auto get_cancel_request() -> motor_hardware::CancelRequest final {
         motor_hardware::CancelRequest exchange_request;
         return cancel_request.exchange(exchange_request);
     }
-    void request_cancel(can::ids::ErrorSeverity error_severity,
+    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),
@@ -256,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 {
@@ -288,11 +291,11 @@ class SimGearMotorHardwareIface
     bool check_estop_in() final { return estop_detected; }
 
     void set_estop(bool estop_pressed) { estop_detected = estop_pressed; }
-    auto has_cancel_request() -> motor_hardware::CancelRequest final {
+    auto get_cancel_request() -> motor_hardware::CancelRequest final {
         motor_hardware::CancelRequest exchange_request;
         return cancel_request.exchange(exchange_request);
     }
-    void request_cancel(can::ids::ErrorSeverity error_severity,
+    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),
diff --git a/include/motor-control/tests/mock_brushed_motor_components.hpp b/include/motor-control/tests/mock_brushed_motor_components.hpp
index b1b7c8af2..c5210e27d 100644
--- a/include/motor-control/tests/mock_brushed_motor_components.hpp
+++ b/include/motor-control/tests/mock_brushed_motor_components.hpp
@@ -79,7 +79,7 @@ class MockBrushedMotorHardware : public BrushedMotorHardwareIface {
         return old_request;
     }
     void set_cancel_request(can::ids::ErrorSeverity error_severity,
-                        can::ids::ErrorCode error_code) final {
+                            can::ids::ErrorCode error_code) final {
         CancelRequest update_request{
             .severity = static_cast<uint8_t>(error_severity),
             .code = static_cast<uint8_t>(error_code)};
diff --git a/include/motor-control/tests/mock_motor_hardware.hpp b/include/motor-control/tests/mock_motor_hardware.hpp
index b42e2a86d..fc04f03b2 100644
--- a/include/motor-control/tests/mock_motor_hardware.hpp
+++ b/include/motor-control/tests/mock_motor_hardware.hpp
@@ -47,7 +47,7 @@ class MockMotorHardware : public motor_hardware::StepperMotorHardwareIface {
         return old_request;
     }
     void set_cancel_request(can::ids::ErrorSeverity error_severity,
-                        can::ids::ErrorCode error_code) final {
+                            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)};
diff --git a/motor-control/firmware/brushed_motor/brushed_motor_hardware.cpp b/motor-control/firmware/brushed_motor/brushed_motor_hardware.cpp
index 99d452504..1d7690cfd 100644
--- a/motor-control/firmware/brushed_motor/brushed_motor_hardware.cpp
+++ b/motor-control/firmware/brushed_motor/brushed_motor_hardware.cpp
@@ -58,9 +58,7 @@ void BrushedMotorHardware::read_sync_in() {
     sync.debounce_update(gpio::is_set(pins.sync_in));
 }
 
-bool BrushedMotorHardware::read_tmc_diag0() {
-    return 0;
-}
+bool BrushedMotorHardware::read_tmc_diag0() { return 0; }
 
 int32_t BrushedMotorHardware::get_encoder_pulses() {
     if (!enc_handle) {
diff --git a/motor-control/firmware/stepper_motor/motor_hardware.cpp b/motor-control/firmware/stepper_motor/motor_hardware.cpp
index 2effd5f1f..74fd9ec9a 100644
--- a/motor-control/firmware/stepper_motor/motor_hardware.cpp
+++ b/motor-control/firmware/stepper_motor/motor_hardware.cpp
@@ -48,9 +48,7 @@ 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);
-}
+bool MotorHardware::read_tmc_diag0() { return gpio::is_set(pins.diag0); }
 
 void MotorHardware::set_LED(bool status) {
     if (status) {
diff --git a/motor-control/tests/test_brushed_motor_interrupt_handler.cpp b/motor-control/tests/test_brushed_motor_interrupt_handler.cpp
index e79411fd6..9558fef81 100644
--- a/motor-control/tests/test_brushed_motor_interrupt_handler.cpp
+++ b/motor-control/tests/test_brushed_motor_interrupt_handler.cpp
@@ -512,8 +512,9 @@ SCENARIO("handler recovers from error state") {
         test_objs.handler.error_handled = true;
 
         WHEN("a cancel request is received") {
-            test_objs.hw.set_cancel_request(can::ids::ErrorSeverity::warning,
-                                        can::ids::ErrorCode::stop_requested);
+            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_motor_interrupt_handler.cpp b/motor-control/tests/test_motor_interrupt_handler.cpp
index 84f8fc45d..3c44db5f4 100644
--- a/motor-control/tests/test_motor_interrupt_handler.cpp
+++ b/motor-control/tests/test_motor_interrupt_handler.cpp
@@ -44,8 +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.set_cancel_request(can::ids::ErrorSeverity::warning,
-                                        can::ids::ErrorCode::stop_requested);
+            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);

From 514017b6ab4b1529bc46f8c2280e87a75b9fbd80 Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Fri, 3 Nov 2023 00:23:52 -0400
Subject: [PATCH 19/68] format

---
 .../motor-control/simulation/sim_motor_hardware_iface.hpp   | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/include/motor-control/simulation/sim_motor_hardware_iface.hpp b/include/motor-control/simulation/sim_motor_hardware_iface.hpp
index 48db76f82..01f45fb21 100644
--- a/include/motor-control/simulation/sim_motor_hardware_iface.hpp
+++ b/include/motor-control/simulation/sim_motor_hardware_iface.hpp
@@ -93,7 +93,7 @@ class SimMotorHardwareIface : public motor_hardware::StepperMotorHardwareIface {
         return cancel_request.exchange(exchange_request);
     }
     void set_cancel_request(can::ids::ErrorSeverity error_severity,
-                        can::ids::ErrorCode error_code) final {
+                            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)};
@@ -195,7 +195,7 @@ class SimBrushedMotorHardwareIface
         return cancel_request.exchange(exchange_request);
     }
     void set_cancel_request(can::ids::ErrorSeverity error_severity,
-                        can::ids::ErrorCode error_code) final {
+                            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)};
@@ -296,7 +296,7 @@ class SimGearMotorHardwareIface
         return cancel_request.exchange(exchange_request);
     }
     void set_cancel_request(can::ids::ErrorSeverity error_severity,
-                        can::ids::ErrorCode error_code) final {
+                            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)};

From 38bd2b8e9ccd159143e5b7cfdb8f1b4e65086cb6 Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Wed, 8 Nov 2023 14:55:11 -0500
Subject: [PATCH 20/68] PR review changes part 3

---
 include/can/core/ids.hpp                            |  1 +
 include/can/core/messages.hpp                       |  5 ++++-
 .../motor-control/core/motor_hardware_interface.hpp |  1 -
 .../core/stepper_motor/motion_controller.hpp        |  7 +++----
 .../core/tasks/motion_controller_task.hpp           | 13 +++++++++----
 .../brushed_motor/brushed_motor_hardware.hpp        |  1 -
 .../firmware/stepper_motor/motor_hardware.hpp       |  1 -
 .../pipettes/core/tasks/motion_controller_task.hpp  | 12 +++++++++---
 include/spi/core/utils.hpp                          |  6 ++----
 9 files changed, 28 insertions(+), 19 deletions(-)

diff --git a/include/can/core/ids.hpp b/include/can/core/ids.hpp
index 7e9620ebf..5c017fe6f 100644
--- a/include/can/core/ids.hpp
+++ b/include/can/core/ids.hpp
@@ -66,6 +66,7 @@ enum class MessageId {
     read_motor_driver_error_register_response = 0x38,
     reset_motor_driver_error_handling = 0x39,
     route_motor_driver_interrupt = 0x3a,
+    motor_driver_in_error_state = 0x3b,
     set_brushed_motor_vref_request = 0x40,
     set_brushed_motor_pwm_request = 0x41,
     gripper_grip_request = 0x42,
diff --git a/include/can/core/messages.hpp b/include/can/core/messages.hpp
index 1d19cadbe..d7620d01a 100644
--- a/include/can/core/messages.hpp
+++ b/include/can/core/messages.hpp
@@ -207,6 +207,8 @@ using MotorDriverErrorEncountered =
 using ResetMotorDriverErrorHandling =
     Empty<MessageId::reset_motor_driver_error_handling>;
 
+using MotorDriverInErrorState = Empty<MessageId::motor_driver_in_error_state>;
+
 struct WriteToEEPromRequest : BaseMessage<MessageId::write_eeprom> {
     uint32_t message_index;
     eeprom::types::address address;
@@ -1580,6 +1582,7 @@ using ResponseMessageType = std::variant<
     PeripheralStatusResponse, BrushedMotorConfResponse,
     UpdateMotorPositionEstimationResponse, BaselineSensorResponse,
     PushTipPresenceNotification, GetMotorUsageResponse,
-    ReadMotorDriverErrorRegisterResponse, GripperJawStateResponse>;
+    ReadMotorDriverErrorRegisterResponse, GripperJawStateResponse,
+    MotorDriverInErrorState>;
 
 }  // namespace can::messages
diff --git a/include/motor-control/core/motor_hardware_interface.hpp b/include/motor-control/core/motor_hardware_interface.hpp
index 21a889642..647603250 100644
--- a/include/motor-control/core/motor_hardware_interface.hpp
+++ b/include/motor-control/core/motor_hardware_interface.hpp
@@ -72,7 +72,6 @@ class UsageEEpromConfig {
     size_t num_keys = 0;
 };
 
-// std::optional usage? See HardwareConfig struct
 struct __attribute__((packed)) CancelRequest {
     uint8_t severity;
     uint8_t code;
diff --git a/include/motor-control/core/stepper_motor/motion_controller.hpp b/include/motor-control/core/stepper_motor/motion_controller.hpp
index 3c46cfd5a..519cc957b 100644
--- a/include/motor-control/core/stepper_motor/motion_controller.hpp
+++ b/include/motor-control/core/stepper_motor/motion_controller.hpp
@@ -73,7 +73,7 @@ class MotionController {
             .seq_id = can_msg.seq_id,
             .stop_condition = can_msg.request_stop_condition,
             .usage_key = hardware.get_usage_eeprom_config().get_distance_key()};
-        if (!enabled) {
+        if (!enabled && !read_tmc_diag0()) {
             enable_motor();
         }
         queue.try_write(msg);
@@ -92,7 +92,7 @@ class MotionController {
             .stop_condition =
                 static_cast<uint8_t>(MoveStopCondition::limit_switch),
             .usage_key = hardware.get_usage_eeprom_config().get_distance_key()};
-        if (!enabled) {
+        if (!enabled && !read_tmc_diag0()) {
             enable_motor();
         }
         queue.try_write(msg);
@@ -140,7 +140,6 @@ class MotionController {
     auto read_tmc_diag0() -> bool { return hardware.read_tmc_diag0(); }
 
     void enable_motor() {
-        // check motor driver error register
         hardware.start_timer_interrupt();
         hardware.activate_motor();
         enabled = true;
@@ -258,7 +257,7 @@ class PipetteMotionController {
             can_msg.action,
             gear_motor_id};
 
-        if (!enabled) {
+        if (!enabled && !read_tmc_diag0()) {
             enable_motor();
         }
         queue.try_write(msg);
diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp
index 135962495..ef7978c60 100644
--- a/include/motor-control/core/tasks/motion_controller_task.hpp
+++ b/include/motor-control/core/tasks/motion_controller_task.hpp
@@ -56,9 +56,15 @@ 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()) {
+            controller.enable_motor();
+            can_client.send_can_message(can::ids::NodeId::host,
+                                        can::messages::ack_from_request(m));
+        } else {
+            can_client.send_can_message(
+                can::ids::NodeId::host,
+                can::messages::MotorDriverInErrorState{.message_index = 0});
+        }
     }
 
     void handle(const can::messages::DisableMotorRequest& m) {
@@ -178,7 +184,6 @@ class MotionControllerMessageHandler {
         static_cast<void>(m);
         driver_error_handled_flag.exchange(false);
         controller.clear_cancel_request();
-        // send MotorDriverErrorCleared msg to host
     }
 
     auto driver_error_handled() -> bool {
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 727c0f9e7..3049d8f41 100644
--- a/include/motor-control/firmware/brushed_motor/brushed_motor_hardware.hpp
+++ b/include/motor-control/firmware/brushed_motor/brushed_motor_hardware.hpp
@@ -79,7 +79,6 @@ 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; }
-    // need any std::optional usage?
     auto get_cancel_request() -> CancelRequest final {
         CancelRequest exchange_request = {};
         return cancel_request.exchange(exchange_request);
diff --git a/include/motor-control/firmware/stepper_motor/motor_hardware.hpp b/include/motor-control/firmware/stepper_motor/motor_hardware.hpp
index 75c68010f..157e0b90e 100644
--- a/include/motor-control/firmware/stepper_motor/motor_hardware.hpp
+++ b/include/motor-control/firmware/stepper_motor/motor_hardware.hpp
@@ -58,7 +58,6 @@ class MotorHardware : public StepperMotorHardwareIface {
     void disable_encoder() final;
     void enable_encoder() final;
 
-    // need any std::optional usage?
     auto get_cancel_request() -> CancelRequest final {
         CancelRequest exchange_request = {};
         return cancel_request.exchange(exchange_request);
diff --git a/include/pipettes/core/tasks/motion_controller_task.hpp b/include/pipettes/core/tasks/motion_controller_task.hpp
index 662094711..14120160c 100644
--- a/include/pipettes/core/tasks/motion_controller_task.hpp
+++ b/include/pipettes/core/tasks/motion_controller_task.hpp
@@ -61,9 +61,15 @@ 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()) {
+            controller.enable_motor();
+            can_client.send_can_message(can::ids::NodeId::host,
+                                        can::messages::ack_from_request(m));
+        } else {
+            can_client.send_can_message(
+                can::ids::NodeId::host,
+                can::messages::MotorDriverInErrorState{.message_index = 0});
+        }
     }
 
     void handle(const can::messages::GearDisableMotorRequest& m) {
diff --git a/include/spi/core/utils.hpp b/include/spi/core/utils.hpp
index eed939ef9..360d1acab 100644
--- a/include/spi/core/utils.hpp
+++ b/include/spi/core/utils.hpp
@@ -35,8 +35,7 @@ template <std::ranges::range Tags>
 }
 
 [[nodiscard]] inline constexpr auto tag_in_token(uint32_t token,
-                                                 ResponseTag tag)
-    -> bool {  // utilize
+                                                 ResponseTag tag) -> bool {
     return bool(token &
                 (1 << (static_cast<size_t>(tag) + static_cast<size_t>(8))));
 }
@@ -47,8 +46,7 @@ template <std::ranges::range Tags>
 }
 
 template <typename RegType>
-[[nodiscard]] inline constexpr auto reg_from_token(uint32_t id)
-    -> RegType {  // utilize? If not, discard
+[[nodiscard]] inline constexpr auto reg_from_token(uint32_t id) -> RegType {
     return static_cast<RegType>(static_cast<uint8_t>(id & 0xff));
 }
 }  // namespace utils

From 9267d1cd2526251ef9b216bfd805429593353fcb Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Wed, 8 Nov 2023 15:02:05 -0500
Subject: [PATCH 21/68] change for debugging

---
 include/motor-control/core/tasks/motion_controller_task.hpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp
index ef7978c60..6fe2a7126 100644
--- a/include/motor-control/core/tasks/motion_controller_task.hpp
+++ b/include/motor-control/core/tasks/motion_controller_task.hpp
@@ -162,7 +162,7 @@ class MotionControllerMessageHandler {
     }
 
     void handle(const can::messages::MotorDriverErrorEncountered& m) {
-        if (!driver_error_handled()) {
+        // if (!driver_error_handled()) {
             controller.stop(can::ids::ErrorSeverity::unrecoverable,
                             can::ids::ErrorCode::motor_driver_error_detected);
             if (!controller.is_timer_interrupt_running()) {
@@ -177,7 +177,7 @@ class MotionControllerMessageHandler {
                     can::messages::ReadMotorDriverErrorStatus{
                         .message_index = m.message_index});
             }
-        }
+        // }
     }
 
     void handle(const can::messages::ResetMotorDriverErrorHandling& m) {

From 2776462f45eb712788abe7699385807389528b10 Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Wed, 8 Nov 2023 15:43:54 -0500
Subject: [PATCH 22/68] debugging commit

---
 .../core/stepper_motor/motion_controller.hpp  |  6 +-
 .../core/tasks/motion_controller_task.hpp     | 64 ++++++++++---------
 .../core/tasks/motion_controller_task.hpp     | 18 ++++--
 3 files changed, 48 insertions(+), 40 deletions(-)

diff --git a/include/motor-control/core/stepper_motor/motion_controller.hpp b/include/motor-control/core/stepper_motor/motion_controller.hpp
index 519cc957b..9cdb71a73 100644
--- a/include/motor-control/core/stepper_motor/motion_controller.hpp
+++ b/include/motor-control/core/stepper_motor/motion_controller.hpp
@@ -73,7 +73,7 @@ class MotionController {
             .seq_id = can_msg.seq_id,
             .stop_condition = can_msg.request_stop_condition,
             .usage_key = hardware.get_usage_eeprom_config().get_distance_key()};
-        if (!enabled && !read_tmc_diag0()) {
+        if (!enabled) {
             enable_motor();
         }
         queue.try_write(msg);
@@ -92,7 +92,7 @@ class MotionController {
             .stop_condition =
                 static_cast<uint8_t>(MoveStopCondition::limit_switch),
             .usage_key = hardware.get_usage_eeprom_config().get_distance_key()};
-        if (!enabled && !read_tmc_diag0()) {
+        if (!enabled) {
             enable_motor();
         }
         queue.try_write(msg);
@@ -257,7 +257,7 @@ class PipetteMotionController {
             can_msg.action,
             gear_motor_id};
 
-        if (!enabled && !read_tmc_diag0()) {
+        if (!enabled) {
             enable_motor();
         }
         queue.try_write(msg);
diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp
index 6fe2a7126..0304dfdf2 100644
--- a/include/motor-control/core/tasks/motion_controller_task.hpp
+++ b/include/motor-control/core/tasks/motion_controller_task.hpp
@@ -56,14 +56,14 @@ class MotionControllerMessageHandler {
 
     void handle(const can::messages::EnableMotorRequest& m) {
         LOG("Received enable motor request");
-        if (!controller.read_tmc_diag0()) {
-            controller.enable_motor();
-            can_client.send_can_message(can::ids::NodeId::host,
-                                        can::messages::ack_from_request(m));
-        } else {
+        if (controller.read_tmc_diag0()) {
             can_client.send_can_message(
                 can::ids::NodeId::host,
                 can::messages::MotorDriverInErrorState{.message_index = 0});
+        } else {
+            controller.enable_motor();
+            can_client.send_can_message(can::ids::NodeId::host,
+                                        can::messages::ack_from_request(m));
         }
     }
 
@@ -102,14 +102,26 @@ 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::MotorDriverInErrorState{.message_index = 0});
+        } else {
+            controller.move(m);
+        }
     }
 
     void handle(const can::messages::HomeRequest& m) {
         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::MotorDriverInErrorState{.message_index = 0});
+        } else {
+            controller.move(m);
+        }
     }
 
     void handle(const can::messages::ReadLimitSwitchRequest& m) {
@@ -162,41 +174,31 @@ class MotionControllerMessageHandler {
     }
 
     void handle(const can::messages::MotorDriverErrorEncountered& m) {
-        // if (!driver_error_handled()) {
-            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::ReadMotorDriverErrorStatus{
-                        .message_index = m.message_index});
-            }
-        // }
+        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::ReadMotorDriverErrorStatus{.message_index =
+                                                              m.message_index});
+        }
     }
 
     void handle(const can::messages::ResetMotorDriverErrorHandling& m) {
         static_cast<void>(m);
-        driver_error_handled_flag.exchange(false);
         controller.clear_cancel_request();
     }
 
-    auto driver_error_handled() -> bool {
-        return driver_error_handled_flag.exchange(true);
-    }
-
     MotorControllerType& controller;
     CanClient& can_client;
     UsageClient& usage_client;
     DriverClient& driver_client;
-    std::atomic<bool> driver_error_handled_flag =
-        false;  // can get rid of this if indefinite interrupt trigger issue
-                // gets resolved
 };
 
 /**
diff --git a/include/pipettes/core/tasks/motion_controller_task.hpp b/include/pipettes/core/tasks/motion_controller_task.hpp
index 14120160c..aa6378143 100644
--- a/include/pipettes/core/tasks/motion_controller_task.hpp
+++ b/include/pipettes/core/tasks/motion_controller_task.hpp
@@ -61,14 +61,14 @@ class MotionControllerMessageHandler {
         LOG("Received enable motor request");
         // TODO only toggle the enable pin once since all motors share
         // a single enable pin line.
-        if (!controller.read_tmc_diag0()) {
-            controller.enable_motor();
-            can_client.send_can_message(can::ids::NodeId::host,
-                                        can::messages::ack_from_request(m));
-        } else {
+        if (controller.read_tmc_diag0()) {
             can_client.send_can_message(
                 can::ids::NodeId::host,
                 can::messages::MotorDriverInErrorState{.message_index = 0});
+        } else {
+            controller.enable_motor();
+            can_client.send_can_message(can::ids::NodeId::host,
+                                        can::messages::ack_from_request(m));
         }
     }
 
@@ -108,7 +108,13 @@ 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::MotorDriverInErrorState{.message_index = 0});
+        } else {
+            controller.move(m);
+        }
     }
 
     void handle(const can::messages::ReadLimitSwitchRequest& m) {

From fa94240991f74ce1eed2c1d2415c72cdf3290ee0 Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Wed, 8 Nov 2023 16:16:13 -0500
Subject: [PATCH 23/68] cleanup

---
 include/bootloader/core/ids.h | 1 -
 include/can/core/ids.hpp      | 1 -
 2 files changed, 2 deletions(-)

diff --git a/include/bootloader/core/ids.h b/include/bootloader/core/ids.h
index 812d0134f..71ee3a3dc 100644
--- a/include/bootloader/core/ids.h
+++ b/include/bootloader/core/ids.h
@@ -95,7 +95,6 @@ typedef enum {
     can_messageid_gear_set_current_request = 0x505,
     can_messageid_gear_write_motor_driver_request = 0x506,
     can_messageid_gear_read_motor_driver_request = 0x507,
-    can_messageid_gear_read_motor_driver_error_status = 0x508,
     can_messageid_read_sensor_request = 0x82,
     can_messageid_write_sensor_request = 0x83,
     can_messageid_baseline_sensor_request = 0x84,
diff --git a/include/can/core/ids.hpp b/include/can/core/ids.hpp
index 5c017fe6f..04c078026 100644
--- a/include/can/core/ids.hpp
+++ b/include/can/core/ids.hpp
@@ -103,7 +103,6 @@ enum class MessageId {
     gear_set_current_request = 0x505,
     gear_write_motor_driver_request = 0x506,
     gear_read_motor_driver_request = 0x507,
-    gear_read_motor_driver_error_status = 0x508,
     read_sensor_request = 0x82,
     write_sensor_request = 0x83,
     baseline_sensor_request = 0x84,

From ad9dcedcfdf7cd5ee2a39aa81d86e19b4bbbbe64 Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Wed, 8 Nov 2023 17:32:21 -0500
Subject: [PATCH 24/68] testing updates

---
 include/motor-control/core/tasks/motion_controller_task.hpp | 6 +++---
 include/pipettes/core/tasks/motion_controller_task.hpp      | 4 ++--
 2 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp
index 0304dfdf2..08f691114 100644
--- a/include/motor-control/core/tasks/motion_controller_task.hpp
+++ b/include/motor-control/core/tasks/motion_controller_task.hpp
@@ -59,7 +59,7 @@ class MotionControllerMessageHandler {
         if (controller.read_tmc_diag0()) {
             can_client.send_can_message(
                 can::ids::NodeId::host,
-                can::messages::MotorDriverInErrorState{.message_index = 0});
+                can::messages::MotorDriverInErrorState{.message_index = m.message_index});
         } else {
             controller.enable_motor();
             can_client.send_can_message(can::ids::NodeId::host,
@@ -105,7 +105,7 @@ class MotionControllerMessageHandler {
         if (controller.read_tmc_diag0()) {
             can_client.send_can_message(
                 can::ids::NodeId::host,
-                can::messages::MotorDriverInErrorState{.message_index = 0});
+                can::messages::MotorDriverInErrorState{.message_index = m.message_index});
         } else {
             controller.move(m);
         }
@@ -118,7 +118,7 @@ class MotionControllerMessageHandler {
         if (controller.read_tmc_diag0()) {
             can_client.send_can_message(
                 can::ids::NodeId::host,
-                can::messages::MotorDriverInErrorState{.message_index = 0});
+                can::messages::MotorDriverInErrorState{.message_index = m.message_index});
         } else {
             controller.move(m);
         }
diff --git a/include/pipettes/core/tasks/motion_controller_task.hpp b/include/pipettes/core/tasks/motion_controller_task.hpp
index aa6378143..39d93ca82 100644
--- a/include/pipettes/core/tasks/motion_controller_task.hpp
+++ b/include/pipettes/core/tasks/motion_controller_task.hpp
@@ -64,7 +64,7 @@ class MotionControllerMessageHandler {
         if (controller.read_tmc_diag0()) {
             can_client.send_can_message(
                 can::ids::NodeId::host,
-                can::messages::MotorDriverInErrorState{.message_index = 0});
+                can::messages::MotorDriverInErrorState{.message_index = m.message_index});
         } else {
             controller.enable_motor();
             can_client.send_can_message(can::ids::NodeId::host,
@@ -111,7 +111,7 @@ class MotionControllerMessageHandler {
         if (controller.read_tmc_diag0()) {
             can_client.send_can_message(
                 can::ids::NodeId::host,
-                can::messages::MotorDriverInErrorState{.message_index = 0});
+                can::messages::MotorDriverInErrorState{.message_index = m.message_index});
         } else {
             controller.move(m);
         }

From 0d4fde6585dbce5fe9600f2f6dd808d7d1963b96 Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Wed, 8 Nov 2023 17:58:44 -0500
Subject: [PATCH 25/68] testing proposed fix

---
 include/motor-control/core/tasks/motion_controller_task.hpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp
index 08f691114..9ee636068 100644
--- a/include/motor-control/core/tasks/motion_controller_task.hpp
+++ b/include/motor-control/core/tasks/motion_controller_task.hpp
@@ -192,6 +192,7 @@ class MotionControllerMessageHandler {
 
     void handle(const can::messages::ResetMotorDriverErrorHandling& m) {
         static_cast<void>(m);
+        controller.enable_motor();
         controller.clear_cancel_request();
     }
 

From 0985ced90c352facee4bf49fa3d56a55e2e34e3c Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Mon, 13 Nov 2023 15:54:24 -0500
Subject: [PATCH 26/68] commit for debug

---
 include/motor-control/core/tasks/motion_controller_task.hpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp
index 9ee636068..adc78518e 100644
--- a/include/motor-control/core/tasks/motion_controller_task.hpp
+++ b/include/motor-control/core/tasks/motion_controller_task.hpp
@@ -192,8 +192,9 @@ class MotionControllerMessageHandler {
 
     void handle(const can::messages::ResetMotorDriverErrorHandling& m) {
         static_cast<void>(m);
-        controller.enable_motor();
+        controller.enable_motor(); // needed?
         controller.clear_cancel_request();
+        can_client.send_can_message(can::ids::NodeId::host, can::messages::ErrorMessage{.message_index = 0}); // delete
     }
 
     MotorControllerType& controller;

From a91c30995600029f361210d044b0aa593e50146a Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Mon, 13 Nov 2023 16:38:47 -0500
Subject: [PATCH 27/68] debug change

---
 .../motor-control/core/tasks/motion_controller_task.hpp   | 8 +++++++-
 1 file changed, 7 insertions(+), 1 deletion(-)

diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp
index adc78518e..f6576ab0b 100644
--- a/include/motor-control/core/tasks/motion_controller_task.hpp
+++ b/include/motor-control/core/tasks/motion_controller_task.hpp
@@ -194,7 +194,13 @@ class MotionControllerMessageHandler {
         static_cast<void>(m);
         controller.enable_motor(); // needed?
         controller.clear_cancel_request();
-        can_client.send_can_message(can::ids::NodeId::host, can::messages::ErrorMessage{.message_index = 0}); // delete
+        can_client.send_can_message(
+            can::ids::NodeId::host,
+            can::messages::ErrorMessage{
+                .message_index = 0,
+                .severity = can::ids::ErrorSeverity::none,
+                .error_code =
+                    can::ids::ErrorCode::motor_driver_error_detected}); // delete
     }
 
     MotorControllerType& controller;

From c3e3130cc3d8fc86dd4d133d1228ccf7dd99247c Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Mon, 13 Nov 2023 17:14:32 -0500
Subject: [PATCH 28/68] debounce for debug

---
 .../core/tasks/motion_controller_task.hpp        | 16 ++++++++++------
 1 file changed, 10 insertions(+), 6 deletions(-)

diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp
index f6576ab0b..9f15142c5 100644
--- a/include/motor-control/core/tasks/motion_controller_task.hpp
+++ b/include/motor-control/core/tasks/motion_controller_task.hpp
@@ -164,12 +164,15 @@ class MotionControllerMessageHandler {
 
     void handle(const can::messages::RouteMotorDriverInterrupt& m) {
         static_cast<void>(m);
-        if (controller.read_tmc_diag0()) {  // debounce needed?
-            handle_message(
-                can::messages::MotorDriverErrorEncountered{.message_index = 0});
-        } else {
-            handle_message(can::messages::ResetMotorDriverErrorHandling{
-                .message_index = 0});
+        diag0_debounce_count++;
+        if (diag0_debounce_count > 50) {
+            if (controller.read_tmc_diag0()) {  // debounce needed? But need to act immediately?!
+                handle_message(
+                    can::messages::MotorDriverErrorEncountered{.message_index = 0});
+            } else {
+                handle_message(can::messages::ResetMotorDriverErrorHandling{
+                    .message_index = 0});
+            }
         }
     }
 
@@ -207,6 +210,7 @@ class MotionControllerMessageHandler {
     CanClient& can_client;
     UsageClient& usage_client;
     DriverClient& driver_client;
+    std::atomic<uint8_t> diag0_debounce_count = 0;
 };
 
 /**

From 3c8cdd4628bb93188169f2c8414c2ddde74f618d Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Mon, 13 Nov 2023 17:19:57 -0500
Subject: [PATCH 29/68] debounce update

---
 include/motor-control/core/tasks/motion_controller_task.hpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp
index 9f15142c5..00b375fda 100644
--- a/include/motor-control/core/tasks/motion_controller_task.hpp
+++ b/include/motor-control/core/tasks/motion_controller_task.hpp
@@ -165,7 +165,8 @@ class MotionControllerMessageHandler {
     void handle(const can::messages::RouteMotorDriverInterrupt& m) {
         static_cast<void>(m);
         diag0_debounce_count++;
-        if (diag0_debounce_count > 50) {
+        if (diag0_debounce_count > 500) {
+            diag0_debounce_count = 0;
             if (controller.read_tmc_diag0()) {  // debounce needed? But need to act immediately?!
                 handle_message(
                     can::messages::MotorDriverErrorEncountered{.message_index = 0});

From f93f13e8348168ee6a1fec02978741b2d753d908 Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Mon, 13 Nov 2023 17:20:53 -0500
Subject: [PATCH 30/68] fix

---
 include/motor-control/core/tasks/motion_controller_task.hpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp
index 00b375fda..0567f5585 100644
--- a/include/motor-control/core/tasks/motion_controller_task.hpp
+++ b/include/motor-control/core/tasks/motion_controller_task.hpp
@@ -211,7 +211,7 @@ class MotionControllerMessageHandler {
     CanClient& can_client;
     UsageClient& usage_client;
     DriverClient& driver_client;
-    std::atomic<uint8_t> diag0_debounce_count = 0;
+    std::atomic<uint16_t> diag0_debounce_count = 0;
 };
 
 /**

From 110b08a3c1b29ecd3551f15913af3a93b3cbbcc1 Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Mon, 13 Nov 2023 17:35:31 -0500
Subject: [PATCH 31/68] debug fix

---
 .../core/tasks/motion_controller_task.hpp            | 12 +++++++-----
 1 file changed, 7 insertions(+), 5 deletions(-)

diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp
index 0567f5585..807894553 100644
--- a/include/motor-control/core/tasks/motion_controller_task.hpp
+++ b/include/motor-control/core/tasks/motion_controller_task.hpp
@@ -164,16 +164,18 @@ class MotionControllerMessageHandler {
 
     void handle(const can::messages::RouteMotorDriverInterrupt& m) {
         static_cast<void>(m);
-        diag0_debounce_count++;
-        if (diag0_debounce_count > 500) {
-            diag0_debounce_count = 0;
-            if (controller.read_tmc_diag0()) {  // debounce needed? But need to act immediately?!
+        if (diag0_debounced) {
+            diag0_debounced = false;
+            if (controller.read_tmc_diag0()) {  // debounce needed?
                 handle_message(
                     can::messages::MotorDriverErrorEncountered{.message_index = 0});
             } else {
                 handle_message(can::messages::ResetMotorDriverErrorHandling{
                     .message_index = 0});
             }
+        } else {
+            vTaskDelay(3000); // Is this ok to use? Need to act immediately?! Just decrease this?
+            diag0_debounced = true;
         }
     }
 
@@ -211,7 +213,7 @@ class MotionControllerMessageHandler {
     CanClient& can_client;
     UsageClient& usage_client;
     DriverClient& driver_client;
-    std::atomic<uint16_t> diag0_debounce_count = 0;
+    std::atomic<bool> diag0_debounced = false;
 };
 
 /**

From e1627b2af61bc2a8dba7e1a25ffc7ef258be31a1 Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Tue, 14 Nov 2023 15:49:04 -0500
Subject: [PATCH 32/68] debug changes

---
 gantry/core/tasks_proto.cpp                   |  2 +-
 gantry/core/tasks_rev1.cpp                    |  2 +-
 gripper/core/tasks_z.cpp                      |  2 +-
 head/core/can_task.cpp                        |  3 +-
 head/core/tasks_proto.cpp                     |  4 +-
 head/core/tasks_rev1.cpp                      |  4 +-
 include/can/core/ids.hpp                      |  1 +
 include/can/core/message_handlers/motion.hpp  |  2 +-
 include/can/core/messages.hpp                 |  2 +
 include/gantry/core/can_task.hpp              |  3 +-
 include/gripper/core/can_task.hpp             |  3 +-
 include/motor-control/core/tasks/messages.hpp |  3 +-
 .../core/tasks/motion_controller_task.hpp     | 72 +++++++++++--------
 include/pipettes/core/dispatch_builder.hpp    |  3 +-
 pipettes/core/linear_motor_tasks.cpp          |  4 +-
 15 files changed, 67 insertions(+), 43 deletions(-)

diff --git a/gantry/core/tasks_proto.cpp b/gantry/core/tasks_proto.cpp
index 143c18c12..c7577a431 100644
--- a/gantry/core/tasks_proto.cpp
+++ b/gantry/core/tasks_proto.cpp
@@ -68,7 +68,7 @@ auto gantry::tasks::start_tasks(
     can_task::start_reader(can_bus);
     auto& motion =
         mc_task_builder.start(5, "motion controller", motion_controller,
-                              ::queues, ::queues, ::queues);
+                              ::queues, ::queues, ::queues, ::queues);
     auto& tmc2130_driver = motor_driver_task_builder.start(
         5, "tmc2130 driver", driver_configs, ::queues, spi_task_client);
     auto& move_group =
diff --git a/gantry/core/tasks_rev1.cpp b/gantry/core/tasks_rev1.cpp
index d7a294a1a..f376b1e39 100644
--- a/gantry/core/tasks_rev1.cpp
+++ b/gantry/core/tasks_rev1.cpp
@@ -67,7 +67,7 @@ auto gantry::tasks::start_tasks(
     can_task::start_reader(can_bus);
     auto& motion =
         mc_task_builder.start(5, "motion controller", motion_controller,
-                              ::queues, ::queues, ::queues);
+                              ::queues, ::queues, ::queues, ::queues);
     auto& tmc2160_driver = motor_driver_task_builder.start(
         5, "tmc2160 driver", driver_configs, ::queues, spi_task_client);
     auto& move_group =
diff --git a/gripper/core/tasks_z.cpp b/gripper/core/tasks_z.cpp
index 59a2411d0..b367ede7a 100644
--- a/gripper/core/tasks_z.cpp
+++ b/gripper/core/tasks_z.cpp
@@ -42,7 +42,7 @@ void z_tasks::start_task(
     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, z_queues);
+                                         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(
diff --git a/head/core/can_task.cpp b/head/core/can_task.cpp
index b0fe76666..048204b8c 100644
--- a/head/core/can_task.cpp
+++ b/head/core/can_task.cpp
@@ -47,7 +47,8 @@ using MotionControllerDispatchTarget = can::dispatch::DispatchParseTarget<
     can::messages::GetMotorUsageRequest,
     can::messages::RouteMotorDriverInterrupt,
     can::messages::MotorDriverErrorEncountered,
-    can::messages::ResetMotorDriverErrorHandling>;
+    can::messages::ResetMotorDriverErrorHandling,
+    can::messages::DebounceMotorDriverError>;
 using SystemDispatchTarget = can::dispatch::DispatchParseTarget<
     can::message_handlers::system::SystemMessageHandler<
         head_tasks::HeadQueueClient>,
diff --git a/head/core/tasks_proto.cpp b/head/core/tasks_proto.cpp
index 6c3d4efd6..199278f07 100644
--- a/head/core/tasks_proto.cpp
+++ b/head/core/tasks_proto.cpp
@@ -142,7 +142,7 @@ 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, left_queues);
+                                   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);
@@ -172,7 +172,7 @@ 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, right_queues);
+                                    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);
diff --git a/head/core/tasks_rev1.cpp b/head/core/tasks_rev1.cpp
index c875df463..c0df9536e 100644
--- a/head/core/tasks_rev1.cpp
+++ b/head/core/tasks_rev1.cpp
@@ -147,7 +147,7 @@ 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, left_queues);
+                                   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);
@@ -185,7 +185,7 @@ 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, right_queues);
+                                    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);
diff --git a/include/can/core/ids.hpp b/include/can/core/ids.hpp
index 04c078026..f61d086e3 100644
--- a/include/can/core/ids.hpp
+++ b/include/can/core/ids.hpp
@@ -67,6 +67,7 @@ enum class MessageId {
     reset_motor_driver_error_handling = 0x39,
     route_motor_driver_interrupt = 0x3a,
     motor_driver_in_error_state = 0x3b,
+    debounce_motor_driver_error = 0x3c,
     set_brushed_motor_vref_request = 0x40,
     set_brushed_motor_pwm_request = 0x41,
     gripper_grip_request = 0x42,
diff --git a/include/can/core/message_handlers/motion.hpp b/include/can/core/message_handlers/motion.hpp
index 6a59f55e5..42ad17949 100644
--- a/include/can/core/message_handlers/motion.hpp
+++ b/include/can/core/message_handlers/motion.hpp
@@ -19,7 +19,7 @@ class MotionHandler {
                      ReadLimitSwitchRequest, MotorPositionRequest,
                      UpdateMotorPositionEstimationRequest, GetMotorUsageRequest,
                      RouteMotorDriverInterrupt, MotorDriverErrorEncountered,
-                     ResetMotorDriverErrorHandling>;
+                     ResetMotorDriverErrorHandling, DebounceMotorDriverError>;
 
     MotionHandler(MotionTaskClient &motion_client)
         : motion_client{motion_client} {}
diff --git a/include/can/core/messages.hpp b/include/can/core/messages.hpp
index d7620d01a..6005d1203 100644
--- a/include/can/core/messages.hpp
+++ b/include/can/core/messages.hpp
@@ -209,6 +209,8 @@ using ResetMotorDriverErrorHandling =
 
 using MotorDriverInErrorState = Empty<MessageId::motor_driver_in_error_state>;
 
+using DebounceMotorDriverError = Empty<MessageId::debounce_motor_driver_error>;
+
 struct WriteToEEPromRequest : BaseMessage<MessageId::write_eeprom> {
     uint32_t message_index;
     eeprom::types::address address;
diff --git a/include/gantry/core/can_task.hpp b/include/gantry/core/can_task.hpp
index e432580b0..f78b7146e 100644
--- a/include/gantry/core/can_task.hpp
+++ b/include/gantry/core/can_task.hpp
@@ -40,7 +40,8 @@ using MotionControllerDispatchTarget = can::dispatch::DispatchParseTarget<
     can::messages::GetMotorUsageRequest,
     can::messages::RouteMotorDriverInterrupt,
     can::messages::MotorDriverErrorEncountered,
-    can::messages::ResetMotorDriverErrorHandling>;
+    can::messages::ResetMotorDriverErrorHandling,
+    can::messages::DebounceMotorDriverError>;
 using SystemDispatchTarget = can::dispatch::DispatchParseTarget<
     can::message_handlers::system::SystemMessageHandler<
         gantry::queues::QueueClient>,
diff --git a/include/gripper/core/can_task.hpp b/include/gripper/core/can_task.hpp
index e3a61ddef..a00c5c050 100644
--- a/include/gripper/core/can_task.hpp
+++ b/include/gripper/core/can_task.hpp
@@ -36,7 +36,8 @@ using MotionControllerDispatchTarget = can::dispatch::DispatchParseTarget<
     can::messages::GetMotorUsageRequest,
     can::messages::RouteMotorDriverInterrupt,
     can::messages::MotorDriverErrorEncountered,
-    can::messages::ResetMotorDriverErrorHandling>;
+    can::messages::ResetMotorDriverErrorHandling,
+    can::messages::DebounceMotorDriverError>;
 using SystemDispatchTarget = can::dispatch::DispatchParseTarget<
     can::message_handlers::system::SystemMessageHandler<
         gripper_tasks::QueueClient>,
diff --git a/include/motor-control/core/tasks/messages.hpp b/include/motor-control/core/tasks/messages.hpp
index 688b8ca68..4e0d95586 100644
--- a/include/motor-control/core/tasks/messages.hpp
+++ b/include/motor-control/core/tasks/messages.hpp
@@ -17,7 +17,8 @@ using MotionControlTaskMessage = std::variant<
     can::messages::GetMotorUsageRequest,
     can::messages::RouteMotorDriverInterrupt,
     can::messages::MotorDriverErrorEncountered,
-    can::messages::ResetMotorDriverErrorHandling>;
+    can::messages::ResetMotorDriverErrorHandling,
+    can::messages::DebounceMotorDriverError>;
 
 using MotorDriverTaskMessage =
     std::variant<std::monostate, can::messages::ReadMotorDriverRegister,
diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp
index 807894553..7177ee07b 100644
--- a/include/motor-control/core/tasks/motion_controller_task.hpp
+++ b/include/motor-control/core/tasks/motion_controller_task.hpp
@@ -14,24 +14,36 @@ 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,
-          tmc::tasks::TaskClient DriverClient>
+          tmc::tasks::TaskClient DriverClient,
+          TaskClient MotionClient>
 class MotionControllerMessageHandler {
   public:
     using MotorControllerType = motion_controller::MotionController<MEConfig>;
     MotionControllerMessageHandler(MotorControllerType& controller,
                                    CanClient& can_client,
                                    UsageClient& usage_client,
-                                   DriverClient& driver_client)
+                                   DriverClient& driver_client,
+                                   MotionClient& motion_client)
         : controller{controller},
           can_client{can_client},
           usage_client{usage_client},
-          driver_client{driver_client} {}
+          driver_client{driver_client},
+          motion_client{motion_client} {}
     MotionControllerMessageHandler(const MotionControllerMessageHandler& c) =
         delete;
     MotionControllerMessageHandler(const MotionControllerMessageHandler&& c) =
@@ -163,18 +175,9 @@ class MotionControllerMessageHandler {
     }
 
     void handle(const can::messages::RouteMotorDriverInterrupt& m) {
-        static_cast<void>(m);
-        if (diag0_debounced) {
-            diag0_debounced = false;
-            if (controller.read_tmc_diag0()) {  // debounce needed?
-                handle_message(
-                    can::messages::MotorDriverErrorEncountered{.message_index = 0});
-            } else {
-                handle_message(can::messages::ResetMotorDriverErrorHandling{
-                    .message_index = 0});
-            }
-        } else {
-            vTaskDelay(3000); // Is this ok to use? Need to act immediately?! Just decrease this?
+        // delay after first message, check/act on pin after delay. Deal with/discard all messgaes during delay
+        if (!diag0_debounced) {
+            motion_client.send_motion_controller_queue(can::messages::DebounceMotorDriverError{.message_index = m.message_index});
             diag0_debounced = true;
         }
     }
@@ -198,8 +201,8 @@ class MotionControllerMessageHandler {
 
     void handle(const can::messages::ResetMotorDriverErrorHandling& m) {
         static_cast<void>(m);
-        controller.enable_motor(); // needed?
         controller.clear_cancel_request();
+        controller.enable_motor(); // needed?
         can_client.send_can_message(
             can::ids::NodeId::host,
             can::messages::ErrorMessage{
@@ -209,11 +212,32 @@ class MotionControllerMessageHandler {
                     can::ids::ErrorCode::motor_driver_error_detected}); // delete
     }
 
+    // combine with Route msg?
+    void handle(const can::messages::DebounceMotorDriverError& m) {
+        vTaskDelay(300); // Need to act immediately?! Just decrease this?
+        debounce_count++;
+        if (debounce_count > 10) {
+            if (controller.read_tmc_diag0()) {
+                handle_message(
+                    can::messages::MotorDriverErrorEncountered{.message_index = m.message_index});
+            } else {
+                handle_message(can::messages::ResetMotorDriverErrorHandling{
+                    .message_index = m.message_index});
+            }
+            diag0_debounced = false;
+            debounce_count = 0;
+        }
+        // Pull queue in via constructor?! Run by Seth
+        motion_client.send_motion_controller_queue(can::messages::DebounceMotorDriverError{});
+    }
+
     MotorControllerType& controller;
     CanClient& can_client;
     UsageClient& usage_client;
     DriverClient& driver_client;
+    MotionClient& motion_client;
     std::atomic<bool> diag0_debounced = false;
+    std::atomic<uint8_t> debounce_count = 0;
 };
 
 /**
@@ -238,13 +262,14 @@ class MotionControllerTask {
     template <lms::MotorMechanicalConfig MEConfig,
               can::message_writer_task::TaskClient CanClient,
               usage_storage_task::TaskClient UsageClient,
-              tmc::tasks::TaskClient DriverClient>
+              tmc::tasks::TaskClient DriverClient,
+              TaskClient MotionClient>
     [[noreturn]] void operator()(
         motion_controller::MotionController<MEConfig>* controller,
         CanClient* can_client, UsageClient* usage_client,
-        DriverClient* driver_client) {
+        DriverClient* driver_client, MotionClient* motion_client) {
         auto handler = MotionControllerMessageHandler{
-            *controller, *can_client, *usage_client, *driver_client};
+            *controller, *can_client, *usage_client, *driver_client, *motion_client};
         TaskMessage message{};
         bool first_run = true;
         for (;;) {
@@ -269,13 +294,4 @@ class MotionControllerTask {
     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)};
-};
-
 }  // namespace motion_controller_task
diff --git a/include/pipettes/core/dispatch_builder.hpp b/include/pipettes/core/dispatch_builder.hpp
index cf2d6d95d..be1695ba5 100644
--- a/include/pipettes/core/dispatch_builder.hpp
+++ b/include/pipettes/core/dispatch_builder.hpp
@@ -68,7 +68,8 @@ using MotionControllerDispatchTarget = can::dispatch::DispatchParseTarget<
     can::messages::GetMotorUsageRequest,
     can::messages::RouteMotorDriverInterrupt,
     can::messages::MotorDriverErrorEncountered,
-    can::messages::ResetMotorDriverErrorHandling>;
+    can::messages::ResetMotorDriverErrorHandling,
+    can::messages::DebounceMotorDriverError>;
 
 using GearMotionControllerDispatchTarget = can::dispatch::DispatchParseTarget<
     gear_motion_handler::GearMotorMotionHandler<gear_motor_tasks::QueueClient>,
diff --git a/pipettes/core/linear_motor_tasks.cpp b/pipettes/core/linear_motor_tasks.cpp
index 5674238ec..a0fa09a1d 100644
--- a/pipettes/core/linear_motor_tasks.cpp
+++ b/pipettes/core/linear_motor_tasks.cpp
@@ -51,7 +51,7 @@ void linear_motor_tasks::start_tasks(
     // Linear Motor Tasks
     auto& motion =
         mc_task_builder.start(5, "motion controller", motion_controller, queues,
-                              queues, tmc2130_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 =
@@ -103,7 +103,7 @@ void linear_motor_tasks::start_tasks(
     // Linear Motor Tasks
     auto& motion =
         mc_task_builder.start(5, "motion controller", motion_controller, queues,
-                              queues, tmc2160_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 =

From daf6e2d980c6b1926c3149d82a4a493b8d9a7791 Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Tue, 14 Nov 2023 16:23:57 -0500
Subject: [PATCH 33/68] debug update

---
 include/motor-control/core/tasks/motion_controller_task.hpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp
index 7177ee07b..cc3510509 100644
--- a/include/motor-control/core/tasks/motion_controller_task.hpp
+++ b/include/motor-control/core/tasks/motion_controller_task.hpp
@@ -228,7 +228,7 @@ class MotionControllerMessageHandler {
             debounce_count = 0;
         }
         // Pull queue in via constructor?! Run by Seth
-        motion_client.send_motion_controller_queue(can::messages::DebounceMotorDriverError{});
+        motion_client.send_motion_controller_queue(can::messages::DebounceMotorDriverError{.message_index = m.message_index});
     }
 
     MotorControllerType& controller;

From 4c8596784c7c087651e574de29decf8293ff050d Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Tue, 14 Nov 2023 16:43:22 -0500
Subject: [PATCH 34/68] debug update

---
 .../core/tasks/motion_controller_task.hpp          | 14 ++++++++++++++
 1 file changed, 14 insertions(+)

diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp
index cc3510509..1a15daa16 100644
--- a/include/motor-control/core/tasks/motion_controller_task.hpp
+++ b/include/motor-control/core/tasks/motion_controller_task.hpp
@@ -177,6 +177,13 @@ class MotionControllerMessageHandler {
     void handle(const can::messages::RouteMotorDriverInterrupt& m) {
         // delay after first message, check/act on pin after delay. Deal with/discard all messgaes during delay
         if (!diag0_debounced) {
+            can_client.send_can_message(
+                can::ids::NodeId::host,
+                can::messages::ErrorMessage{
+                    .message_index = 1,
+                    .severity = can::ids::ErrorSeverity::unrecoverable,
+                    .error_code =
+                        can::ids::ErrorCode::motor_driver_error_detected}); // delete
             motion_client.send_motion_controller_queue(can::messages::DebounceMotorDriverError{.message_index = m.message_index});
             diag0_debounced = true;
         }
@@ -214,6 +221,13 @@ class MotionControllerMessageHandler {
 
     // combine with Route msg?
     void handle(const can::messages::DebounceMotorDriverError& m) {
+        can_client.send_can_message(
+                can::ids::NodeId::host,
+                can::messages::ErrorMessage{
+                    .message_index = static_cast<uint32_t>(debounce_count) + 2,
+                    .severity = can::ids::ErrorSeverity::unrecoverable,
+                    .error_code =
+                        can::ids::ErrorCode::motor_driver_error_detected}); // delete
         vTaskDelay(300); // Need to act immediately?! Just decrease this?
         debounce_count++;
         if (debounce_count > 10) {

From 33cca8876e003fdefb27fccf49f60a65d003b8b9 Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Tue, 14 Nov 2023 16:47:46 -0500
Subject: [PATCH 35/68] debug update

---
 include/motor-control/core/tasks/motion_controller_task.hpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp
index 1a15daa16..60f9963db 100644
--- a/include/motor-control/core/tasks/motion_controller_task.hpp
+++ b/include/motor-control/core/tasks/motion_controller_task.hpp
@@ -228,9 +228,9 @@ class MotionControllerMessageHandler {
                     .severity = can::ids::ErrorSeverity::unrecoverable,
                     .error_code =
                         can::ids::ErrorCode::motor_driver_error_detected}); // delete
-        vTaskDelay(300); // Need to act immediately?! Just decrease this?
+        vTaskDelay(30); // Need to act immediately?! Just decrease this?
         debounce_count++;
-        if (debounce_count > 10) {
+        if (debounce_count > 100) {
             if (controller.read_tmc_diag0()) {
                 handle_message(
                     can::messages::MotorDriverErrorEncountered{.message_index = m.message_index});

From a1f2ec29d7b2bddd3ccea15dbe20fbf79e842d7f Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Tue, 14 Nov 2023 16:55:47 -0500
Subject: [PATCH 36/68] debug change

---
 include/motor-control/core/tasks/motion_controller_task.hpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp
index 60f9963db..624a20503 100644
--- a/include/motor-control/core/tasks/motion_controller_task.hpp
+++ b/include/motor-control/core/tasks/motion_controller_task.hpp
@@ -228,9 +228,9 @@ class MotionControllerMessageHandler {
                     .severity = can::ids::ErrorSeverity::unrecoverable,
                     .error_code =
                         can::ids::ErrorCode::motor_driver_error_detected}); // delete
-        vTaskDelay(30); // Need to act immediately?! Just decrease this?
+        vTaskDelay(20); // Need to act immediately?! Just decrease this?
         debounce_count++;
-        if (debounce_count > 100) {
+        if (debounce_count > 50) {
             if (controller.read_tmc_diag0()) {
                 handle_message(
                     can::messages::MotorDriverErrorEncountered{.message_index = m.message_index});

From f3bd1d31db4ab741c798f8a6f2f4a3cbeb69edfd Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Tue, 14 Nov 2023 17:35:50 -0500
Subject: [PATCH 37/68] debug change

---
 include/motor-control/core/tasks/motion_controller_task.hpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp
index 624a20503..73e6175fa 100644
--- a/include/motor-control/core/tasks/motion_controller_task.hpp
+++ b/include/motor-control/core/tasks/motion_controller_task.hpp
@@ -228,9 +228,9 @@ class MotionControllerMessageHandler {
                     .severity = can::ids::ErrorSeverity::unrecoverable,
                     .error_code =
                         can::ids::ErrorCode::motor_driver_error_detected}); // delete
-        vTaskDelay(20); // Need to act immediately?! Just decrease this?
+        vTaskDelay(200); // Need to act immediately?! Just decrease this?
         debounce_count++;
-        if (debounce_count > 50) {
+        if (debounce_count > 10) { // send msg immediately, reset flags after delay?!
             if (controller.read_tmc_diag0()) {
                 handle_message(
                     can::messages::MotorDriverErrorEncountered{.message_index = m.message_index});

From 41c1560bbaebfc7ce898af04a5736f219c3d1e50 Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Tue, 14 Nov 2023 17:54:24 -0500
Subject: [PATCH 38/68] debug update

---
 include/motor-control/core/tasks/motion_controller_task.hpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp
index 73e6175fa..4d20b8d9e 100644
--- a/include/motor-control/core/tasks/motion_controller_task.hpp
+++ b/include/motor-control/core/tasks/motion_controller_task.hpp
@@ -228,7 +228,7 @@ class MotionControllerMessageHandler {
                     .severity = can::ids::ErrorSeverity::unrecoverable,
                     .error_code =
                         can::ids::ErrorCode::motor_driver_error_detected}); // delete
-        vTaskDelay(200); // Need to act immediately?! Just decrease this?
+        vTaskDelay(pdMS_TO_TICKS(200)); // Need to act immediately?! Just decrease this?
         debounce_count++;
         if (debounce_count > 10) { // send msg immediately, reset flags after delay?!
             if (controller.read_tmc_diag0()) {

From 7d270e10c07c2060f91467b663e10502bcc84fad Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Tue, 14 Nov 2023 18:08:44 -0500
Subject: [PATCH 39/68] debug changes

---
 .../motor-control/core/tasks/motion_controller_task.hpp    | 7 +++++--
 1 file changed, 5 insertions(+), 2 deletions(-)

diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp
index 4d20b8d9e..90812c95f 100644
--- a/include/motor-control/core/tasks/motion_controller_task.hpp
+++ b/include/motor-control/core/tasks/motion_controller_task.hpp
@@ -228,6 +228,8 @@ class MotionControllerMessageHandler {
                     .severity = can::ids::ErrorSeverity::unrecoverable,
                     .error_code =
                         can::ids::ErrorCode::motor_driver_error_detected}); // delete
+        // TickTypet currentTick = xTaskGetTickCount();
+        // while(xTaskGetTickCount() – currentTick < pdMSTO_TICKS(1000))
         vTaskDelay(pdMS_TO_TICKS(200)); // Need to act immediately?! Just decrease this?
         debounce_count++;
         if (debounce_count > 10) { // send msg immediately, reset flags after delay?!
@@ -240,9 +242,10 @@ class MotionControllerMessageHandler {
             }
             diag0_debounced = false;
             debounce_count = 0;
+        } else {
+            // Run this by Seth
+            motion_client.send_motion_controller_queue(can::messages::DebounceMotorDriverError{.message_index = m.message_index});
         }
-        // Pull queue in via constructor?! Run by Seth
-        motion_client.send_motion_controller_queue(can::messages::DebounceMotorDriverError{.message_index = m.message_index});
     }
 
     MotorControllerType& controller;

From c9947fc76fb54169ec4a783b333d728b3b3250f2 Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Wed, 15 Nov 2023 14:58:29 -0500
Subject: [PATCH 40/68] debug update

---
 gantry/firmware/motor_hardware.c              |  2 +-
 .../core/tasks/motion_controller_task.hpp     | 22 ++++++++++++-------
 2 files changed, 15 insertions(+), 9 deletions(-)

diff --git a/gantry/firmware/motor_hardware.c b/gantry/firmware/motor_hardware.c
index 46b10a31a..637087c76 100644
--- a/gantry/firmware/motor_hardware.c
+++ b/gantry/firmware/motor_hardware.c
@@ -60,7 +60,7 @@ void HAL_SPI_MspInit(SPI_HandleTypeDef* hspi) {
         // Diag0
         GPIO_InitStruct.Pin = GPIO_PIN_5;
         GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING_FALLING;
-        GPIO_InitStruct.Pull = GPIO_PULLUP;
+        GPIO_InitStruct.Pull = GPIO_NOPULL;
         HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
     }
 }
diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp
index 90812c95f..1a981d739 100644
--- a/include/motor-control/core/tasks/motion_controller_task.hpp
+++ b/include/motor-control/core/tasks/motion_controller_task.hpp
@@ -38,12 +38,14 @@ class MotionControllerMessageHandler {
                                    CanClient& can_client,
                                    UsageClient& usage_client,
                                    DriverClient& driver_client,
-                                   MotionClient& motion_client)
+                                   MotionClient& motion_client,
+                                   std::atomic<bool>& diag0_debounced)
         : controller{controller},
           can_client{can_client},
           usage_client{usage_client},
           driver_client{driver_client},
-          motion_client{motion_client} {}
+          motion_client{motion_client},
+          diag0_debounced{diag0_debounced} {}
     MotionControllerMessageHandler(const MotionControllerMessageHandler& c) =
         delete;
     MotionControllerMessageHandler(const MotionControllerMessageHandler&& c) =
@@ -230,7 +232,7 @@ class MotionControllerMessageHandler {
                         can::ids::ErrorCode::motor_driver_error_detected}); // delete
         // TickTypet currentTick = xTaskGetTickCount();
         // while(xTaskGetTickCount() – currentTick < pdMSTO_TICKS(1000))
-        vTaskDelay(pdMS_TO_TICKS(200)); // Need to act immediately?! Just decrease this?
+        vTaskDelay(pdMS_TO_TICKS(200)); // Need to act immediately?! Just decrease this?!
         debounce_count++;
         if (debounce_count > 10) { // send msg immediately, reset flags after delay?!
             if (controller.read_tmc_diag0()) {
@@ -243,7 +245,7 @@ class MotionControllerMessageHandler {
             diag0_debounced = false;
             debounce_count = 0;
         } else {
-            // Run this by Seth
+            // Run pulling motion_client by Seth
             motion_client.send_motion_controller_queue(can::messages::DebounceMotorDriverError{.message_index = m.message_index});
         }
     }
@@ -253,7 +255,7 @@ class MotionControllerMessageHandler {
     UsageClient& usage_client;
     DriverClient& driver_client;
     MotionClient& motion_client;
-    std::atomic<bool> diag0_debounced = false;
+    std::atomic<bool>& diag0_debounced;
     std::atomic<uint8_t> debounce_count = 0;
 };
 
@@ -286,7 +288,7 @@ class MotionControllerTask {
         CanClient* can_client, UsageClient* usage_client,
         DriverClient* driver_client, MotionClient* motion_client) {
         auto handler = MotionControllerMessageHandler{
-            *controller, *can_client, *usage_client, *driver_client, *motion_client};
+            *controller, *can_client, *usage_client, *driver_client, *motion_client, diag0_debounced};
         TaskMessage message{};
         bool first_run = true;
         for (;;) {
@@ -303,12 +305,16 @@ class MotionControllerTask {
     [[nodiscard]] auto get_queue() const -> QueueType& { return queue; }
 
     void run_diag0_interrupt() {
-        static_cast<void>(queue.try_write_isr(
-            can::messages::RouteMotorDriverInterrupt{.message_index = 0}));
+        if (!diag0_debounced) {
+            static_cast<void>(queue.try_write_isr(
+                can::messages::DebounceMotorDriverError{.message_index = 0}));
+            diag0_debounced = true;
+        }
     }
 
   private:
     QueueType& queue;
+    std::atomic<bool> diag0_debounced = false;
 };
 
 }  // namespace motion_controller_task

From 60176a3340bd7b433b749969738d6f4abd740074 Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Wed, 15 Nov 2023 15:18:55 -0500
Subject: [PATCH 41/68] debug change

---
 .../motor-control/core/tasks/motion_controller_task.hpp   | 8 ++------
 1 file changed, 2 insertions(+), 6 deletions(-)

diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp
index 1a981d739..329fd88cb 100644
--- a/include/motor-control/core/tasks/motion_controller_task.hpp
+++ b/include/motor-control/core/tasks/motion_controller_task.hpp
@@ -230,17 +230,13 @@ class MotionControllerMessageHandler {
                     .severity = can::ids::ErrorSeverity::unrecoverable,
                     .error_code =
                         can::ids::ErrorCode::motor_driver_error_detected}); // delete
-        // TickTypet currentTick = xTaskGetTickCount();
-        // while(xTaskGetTickCount() – currentTick < pdMSTO_TICKS(1000))
         vTaskDelay(pdMS_TO_TICKS(200)); // Need to act immediately?! Just decrease this?!
         debounce_count++;
         if (debounce_count > 10) { // send msg immediately, reset flags after delay?!
             if (controller.read_tmc_diag0()) {
-                handle_message(
-                    can::messages::MotorDriverErrorEncountered{.message_index = m.message_index});
+                motion_client.send_motion_controller_queue(can::messages::MotorDriverErrorEncountered{.message_index = m.message_index});
             } else {
-                handle_message(can::messages::ResetMotorDriverErrorHandling{
-                    .message_index = m.message_index});
+                motion_client.send_motion_controller_queue(can::messages::ResetMotorDriverErrorHandling{.message_index = m.message_index});
             }
             diag0_debounced = false;
             debounce_count = 0;

From 39aa8ced6044e90f2d0573eb32f410d2ee180ec6 Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Wed, 15 Nov 2023 16:07:02 -0500
Subject: [PATCH 42/68] debug changes

---
 .../core/tasks/motion_controller_task.hpp     | 26 ++++---------------
 1 file changed, 5 insertions(+), 21 deletions(-)

diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp
index 329fd88cb..85878bcb4 100644
--- a/include/motor-control/core/tasks/motion_controller_task.hpp
+++ b/include/motor-control/core/tasks/motion_controller_task.hpp
@@ -176,16 +176,9 @@ class MotionControllerMessageHandler {
         controller.send_usage_data(m.message_index, usage_client);
     }
 
+    //get rid of this method
     void handle(const can::messages::RouteMotorDriverInterrupt& m) {
-        // delay after first message, check/act on pin after delay. Deal with/discard all messgaes during delay
         if (!diag0_debounced) {
-            can_client.send_can_message(
-                can::ids::NodeId::host,
-                can::messages::ErrorMessage{
-                    .message_index = 1,
-                    .severity = can::ids::ErrorSeverity::unrecoverable,
-                    .error_code =
-                        can::ids::ErrorCode::motor_driver_error_detected}); // delete
             motion_client.send_motion_controller_queue(can::messages::DebounceMotorDriverError{.message_index = m.message_index});
             diag0_debounced = true;
         }
@@ -211,28 +204,20 @@ class MotionControllerMessageHandler {
     void handle(const can::messages::ResetMotorDriverErrorHandling& m) {
         static_cast<void>(m);
         controller.clear_cancel_request();
-        controller.enable_motor(); // needed?
         can_client.send_can_message(
             can::ids::NodeId::host,
             can::messages::ErrorMessage{
-                .message_index = 0,
+                .message_index = m.message_index,
                 .severity = can::ids::ErrorSeverity::none,
                 .error_code =
                     can::ids::ErrorCode::motor_driver_error_detected}); // delete
     }
 
-    // combine with Route msg?
     void handle(const can::messages::DebounceMotorDriverError& m) {
-        can_client.send_can_message(
-                can::ids::NodeId::host,
-                can::messages::ErrorMessage{
-                    .message_index = static_cast<uint32_t>(debounce_count) + 2,
-                    .severity = can::ids::ErrorSeverity::unrecoverable,
-                    .error_code =
-                        can::ids::ErrorCode::motor_driver_error_detected}); // delete
-        vTaskDelay(pdMS_TO_TICKS(200)); // Need to act immediately?! Just decrease this?!
+        // delay after first message, check/act on pin after delay. Deal with/discard all messgaes during delay
+        vTaskDelay(pdMS_TO_TICKS(100)); // Need to act immediately?! Just decrease this?!
         debounce_count++;
-        if (debounce_count > 10) { // send msg immediately, reset flags after delay?!
+        if (debounce_count > 9) { // send msg immediately, reset flags after delay?!
             if (controller.read_tmc_diag0()) {
                 motion_client.send_motion_controller_queue(can::messages::MotorDriverErrorEncountered{.message_index = m.message_index});
             } else {
@@ -241,7 +226,6 @@ class MotionControllerMessageHandler {
             diag0_debounced = false;
             debounce_count = 0;
         } else {
-            // Run pulling motion_client by Seth
             motion_client.send_motion_controller_queue(can::messages::DebounceMotorDriverError{.message_index = m.message_index});
         }
     }

From 6ae3cde135636b1f880b6f26e720f80815faf756 Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Wed, 15 Nov 2023 17:24:28 -0500
Subject: [PATCH 43/68] cleanup

---
 .../motor-control/core/tasks/motion_controller_task.hpp  | 9 +++++----
 1 file changed, 5 insertions(+), 4 deletions(-)

diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp
index 85878bcb4..129c1a29f 100644
--- a/include/motor-control/core/tasks/motion_controller_task.hpp
+++ b/include/motor-control/core/tasks/motion_controller_task.hpp
@@ -176,7 +176,7 @@ class MotionControllerMessageHandler {
         controller.send_usage_data(m.message_index, usage_client);
     }
 
-    //get rid of this method
+    // use this method, get rid of debounce message
     void handle(const can::messages::RouteMotorDriverInterrupt& m) {
         if (!diag0_debounced) {
             motion_client.send_motion_controller_queue(can::messages::DebounceMotorDriverError{.message_index = m.message_index});
@@ -213,11 +213,12 @@ class MotionControllerMessageHandler {
                     can::ids::ErrorCode::motor_driver_error_detected}); // delete
     }
 
+    // no debouncing needed in final product
+    // make messages non-CAN, should be trivial, everything takes std::monostate
     void handle(const can::messages::DebounceMotorDriverError& m) {
-        // delay after first message, check/act on pin after delay. Deal with/discard all messgaes during delay
-        vTaskDelay(pdMS_TO_TICKS(100)); // Need to act immediately?! Just decrease this?!
+        vTaskDelay(pdMS_TO_TICKS(100));
         debounce_count++;
-        if (debounce_count > 9) { // send msg immediately, reset flags after delay?!
+        if (debounce_count > 9) {
             if (controller.read_tmc_diag0()) {
                 motion_client.send_motion_controller_queue(can::messages::MotorDriverErrorEncountered{.message_index = m.message_index});
             } else {

From 2c59dccaed94975288c4529d5b86023812d77fbd Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Tue, 5 Dec 2023 13:45:22 -0500
Subject: [PATCH 44/68] proposed refactor

---
 head/core/can_task.cpp                        |   6 +-
 include/can/core/ids.hpp                      |  10 +-
 include/can/core/message_handlers/motion.hpp  |  10 +-
 include/can/core/messages.hpp                 |  11 --
 include/common/core/message_utils.hpp         |  21 ++++
 include/gantry/core/can_task.hpp              |   6 +-
 include/gripper/core/can_task.hpp             |   6 +-
 include/motor-control/core/tasks/messages.hpp | 102 +++++++++++++++++-
 .../core/tasks/motion_controller_task.hpp     |  89 +++++++++++----
 include/pipettes/core/dispatch_builder.hpp    |   6 +-
 10 files changed, 199 insertions(+), 68 deletions(-)

diff --git a/head/core/can_task.cpp b/head/core/can_task.cpp
index 048204b8c..d2f4e7380 100644
--- a/head/core/can_task.cpp
+++ b/head/core/can_task.cpp
@@ -44,11 +44,7 @@ using MotionControllerDispatchTarget = can::dispatch::DispatchParseTarget<
     can::messages::SetMotionConstraints, can::messages::ReadLimitSwitchRequest,
     can::messages::MotorPositionRequest,
     can::messages::UpdateMotorPositionEstimationRequest,
-    can::messages::GetMotorUsageRequest,
-    can::messages::RouteMotorDriverInterrupt,
-    can::messages::MotorDriverErrorEncountered,
-    can::messages::ResetMotorDriverErrorHandling,
-    can::messages::DebounceMotorDriverError>;
+    can::messages::GetMotorUsageRequest>;
 using SystemDispatchTarget = can::dispatch::DispatchParseTarget<
     can::message_handlers::system::SystemMessageHandler<
         head_tasks::HeadQueueClient>,
diff --git a/include/can/core/ids.hpp b/include/can/core/ids.hpp
index f61d086e3..17c60ff6f 100644
--- a/include/can/core/ids.hpp
+++ b/include/can/core/ids.hpp
@@ -61,13 +61,9 @@ enum class MessageId {
     write_motor_current_request = 0x33,
     read_motor_current_request = 0x34,
     read_motor_current_response = 0x35,
-    motor_driver_error_encountered = 0x36,
-    read_motor_driver_error_status = 0x37,
-    read_motor_driver_error_register_response = 0x38,
-    reset_motor_driver_error_handling = 0x39,
-    route_motor_driver_interrupt = 0x3a,
-    motor_driver_in_error_state = 0x3b,
-    debounce_motor_driver_error = 0x3c,
+    read_motor_driver_error_status = 0x36,
+    read_motor_driver_error_register_response = 0x37,
+    motor_driver_in_error_state = 0x38,
     set_brushed_motor_vref_request = 0x40,
     set_brushed_motor_pwm_request = 0x41,
     gripper_grip_request = 0x42,
diff --git a/include/can/core/message_handlers/motion.hpp b/include/can/core/message_handlers/motion.hpp
index 42ad17949..0787cf138 100644
--- a/include/can/core/message_handlers/motion.hpp
+++ b/include/can/core/message_handlers/motion.hpp
@@ -17,9 +17,7 @@ class MotionHandler {
         std::variant<std::monostate, DisableMotorRequest, EnableMotorRequest,
                      GetMotionConstraintsRequest, SetMotionConstraints,
                      ReadLimitSwitchRequest, MotorPositionRequest,
-                     UpdateMotorPositionEstimationRequest, GetMotorUsageRequest,
-                     RouteMotorDriverInterrupt, MotorDriverErrorEncountered,
-                     ResetMotorDriverErrorHandling, DebounceMotorDriverError>;
+                     UpdateMotorPositionEstimationRequest, GetMotorUsageRequest>;
 
     MotionHandler(MotionTaskClient &motion_client)
         : motion_client{motion_client} {}
@@ -36,6 +34,12 @@ class MotionHandler {
   private:
     void handle_message(std::monostate &m) { static_cast<void>(m); }
 
+    /*
+    void handle_message(const auto &m) {
+        motion_client.send_motion_controller_queue(utils::variant_cast(m)); // try to compile, talk with Seth
+    }
+    */
+    
     void handle_message(const auto &m) {
         motion_client.send_motion_controller_queue(m);
     }
diff --git a/include/can/core/messages.hpp b/include/can/core/messages.hpp
index 6005d1203..6781b228a 100644
--- a/include/can/core/messages.hpp
+++ b/include/can/core/messages.hpp
@@ -198,19 +198,8 @@ using MotorPositionRequest = Empty<MessageId::motor_position_request>;
 using UpdateMotorPositionEstimationRequest =
     Empty<MessageId::update_motor_position_estimation_request>;
 
-using RouteMotorDriverInterrupt =
-    Empty<MessageId::route_motor_driver_interrupt>;
-
-using MotorDriverErrorEncountered =
-    Empty<MessageId::motor_driver_error_encountered>;
-
-using ResetMotorDriverErrorHandling =
-    Empty<MessageId::reset_motor_driver_error_handling>;
-
 using MotorDriverInErrorState = Empty<MessageId::motor_driver_in_error_state>;
 
-using DebounceMotorDriverError = Empty<MessageId::debounce_motor_driver_error>;
-
 struct WriteToEEPromRequest : BaseMessage<MessageId::write_eeprom> {
     uint32_t message_index;
     eeprom::types::address address;
diff --git a/include/common/core/message_utils.hpp b/include/common/core/message_utils.hpp
index e1fcee583..8eb431dc6 100644
--- a/include/common/core/message_utils.hpp
+++ b/include/common/core/message_utils.hpp
@@ -73,4 +73,25 @@ struct VariantCat<std::variant<VariantAContents...>,
     using type = std::variant<VariantAContents..., VariantBContents...>;
 };
 
+/*
+template <class... Args>
+struct variant_cast_proxy
+{
+    std::variant<Args...> v;
+
+    template <class... ToArgs>
+    operator std::variant<ToArgs...>() const
+    {
+        return std::visit([](auto&& arg) -> std::variant<ToArgs...> { return arg ; },
+                          v);
+    }
+};
+
+template <class... Args>
+auto variant_cast(const std::variant<Args...>& v) -> variant_cast_proxy<Args...>
+{
+    return {v};
+}
+*/
+
 };  // namespace utils
diff --git a/include/gantry/core/can_task.hpp b/include/gantry/core/can_task.hpp
index f78b7146e..3c18301fc 100644
--- a/include/gantry/core/can_task.hpp
+++ b/include/gantry/core/can_task.hpp
@@ -37,11 +37,7 @@ using MotionControllerDispatchTarget = can::dispatch::DispatchParseTarget<
     can::messages::SetMotionConstraints, can::messages::ReadLimitSwitchRequest,
     can::messages::MotorPositionRequest,
     can::messages::UpdateMotorPositionEstimationRequest,
-    can::messages::GetMotorUsageRequest,
-    can::messages::RouteMotorDriverInterrupt,
-    can::messages::MotorDriverErrorEncountered,
-    can::messages::ResetMotorDriverErrorHandling,
-    can::messages::DebounceMotorDriverError>;
+    can::messages::GetMotorUsageRequest>;
 using SystemDispatchTarget = can::dispatch::DispatchParseTarget<
     can::message_handlers::system::SystemMessageHandler<
         gantry::queues::QueueClient>,
diff --git a/include/gripper/core/can_task.hpp b/include/gripper/core/can_task.hpp
index a00c5c050..5c7d5a65b 100644
--- a/include/gripper/core/can_task.hpp
+++ b/include/gripper/core/can_task.hpp
@@ -33,11 +33,7 @@ using MotionControllerDispatchTarget = can::dispatch::DispatchParseTarget<
     can::messages::SetMotionConstraints, can::messages::ReadLimitSwitchRequest,
     can::messages::MotorPositionRequest,
     can::messages::UpdateMotorPositionEstimationRequest,
-    can::messages::GetMotorUsageRequest,
-    can::messages::RouteMotorDriverInterrupt,
-    can::messages::MotorDriverErrorEncountered,
-    can::messages::ResetMotorDriverErrorHandling,
-    can::messages::DebounceMotorDriverError>;
+    can::messages::GetMotorUsageRequest>;
 using SystemDispatchTarget = can::dispatch::DispatchParseTarget<
     can::message_handlers::system::SystemMessageHandler<
         gripper_tasks::QueueClient>,
diff --git a/include/motor-control/core/tasks/messages.hpp b/include/motor-control/core/tasks/messages.hpp
index 4e0d95586..bd16e28b8 100644
--- a/include/motor-control/core/tasks/messages.hpp
+++ b/include/motor-control/core/tasks/messages.hpp
@@ -6,6 +6,102 @@
 
 namespace motor_control_task_messages {
 
+/*enum class MessageId {
+    route_motor_driver_interrupt = 0x0,
+    motor_driver_error_encountered = 0x1,
+    reset_motor_driver_error_handling = 0x2,
+};
+
+template <MessageId MId>
+struct BaseMessage {
+    static const auto id = MId;
+    auto operator==(const BaseMessage& other) const -> bool = default;
+};
+
+template <MessageId MId>
+struct Empty : BaseMessage<MId> {
+    uint32_t message_index;
+    template <bit_utils::ByteIterator Input, typename Limit>
+    static auto parse(Input body, Limit limit) -> Empty {
+        uint32_t msg_ind = 0;
+        body = bit_utils::bytes_to_int(body, limit, msg_ind);
+        return Empty{.message_index = msg_ind};
+    }
+
+    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);
+        return iter - body;
+    }
+
+    auto operator==(const Empty&) const -> bool = default;
+};
+
+using RouteMotorDriverInterrupt = Empty<MessageId::route_motor_driver_interrupt>;
+
+using MotorDriverErrorEncountered = Empty<MessageId::motor_driver_error_encountered>;
+
+using ResetMotorDriverErrorHandling = Empty<MessageId::reset_motor_driver_error_handling>;
+
+struct RouteMotorDriverInterrupt {
+    uint32_t message_index;
+    template <bit_utils::ByteIterator Input, typename Limit>
+    static auto parse(Input body, Limit limit) -> RouteMotorDriverInterrupt {
+        uint32_t msg_ind = 0;
+        body = bit_utils::bytes_to_int(body, limit, msg_ind);
+        return RouteMotorDriverInterrupt{.message_index = msg_ind};
+    }
+
+    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);
+        return iter - body;
+    }
+
+    auto operator==(const RouteMotorDriverInterrupt&) const -> bool = default;
+};
+
+struct MotorDriverErrorEncountered {
+    uint32_t message_index;
+    template <bit_utils::ByteIterator Input, typename Limit>
+    static auto parse(Input body, Limit limit) -> MotorDriverErrorEncountered {
+        uint32_t msg_ind = 0;
+        body = bit_utils::bytes_to_int(body, limit, msg_ind);
+        return MotorDriverErrorEncountered{.message_index = msg_ind};
+    }
+
+    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);
+        return iter - body;
+    }
+
+    auto operator==(const MotorDriverErrorEncountered&) const -> bool = default;
+};
+
+struct ResetMotorDriverErrorHandling {
+    uint32_t message_index;
+    template <bit_utils::ByteIterator Input, typename Limit>
+    static auto parse(Input body, Limit limit) -> ResetMotorDriverErrorHandling {
+        uint32_t msg_ind = 0;
+        body = bit_utils::bytes_to_int(body, limit, msg_ind);
+        return ResetMotorDriverErrorHandling{.message_index = msg_ind};
+    }
+
+    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);
+        return iter - body;
+    }
+
+    auto operator==(const ResetMotorDriverErrorHandling&) const -> bool = default;
+};*/
+// needs a member 'id'. This seems to just be becoming a CAN msg!
+
+struct RouteMotorDriverInterrupt {
+    uint32_t message_index;
+};
+
 using MotionControlTaskMessage = std::variant<
     std::monostate, can::messages::AddLinearMoveRequest,
     can::messages::DisableMotorRequest, can::messages::EnableMotorRequest,
@@ -14,11 +110,7 @@ using MotionControlTaskMessage = std::variant<
     can::messages::MotorPositionRequest, can::messages::ReadLimitSwitchRequest,
     can::messages::HomeRequest,
     can::messages::UpdateMotorPositionEstimationRequest,
-    can::messages::GetMotorUsageRequest,
-    can::messages::RouteMotorDriverInterrupt,
-    can::messages::MotorDriverErrorEncountered,
-    can::messages::ResetMotorDriverErrorHandling,
-    can::messages::DebounceMotorDriverError>;
+    can::messages::GetMotorUsageRequest, RouteMotorDriverInterrupt>;
 
 using MotorDriverTaskMessage =
     std::variant<std::monostate, can::messages::ReadMotorDriverRegister,
diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp
index 129c1a29f..d853de95a 100644
--- a/include/motor-control/core/tasks/motion_controller_task.hpp
+++ b/include/motor-control/core/tasks/motion_controller_task.hpp
@@ -176,15 +176,26 @@ class MotionControllerMessageHandler {
         controller.send_usage_data(m.message_index, usage_client);
     }
 
-    // use this method, get rid of debounce message
-    void handle(const can::messages::RouteMotorDriverInterrupt& m) {
-        if (!diag0_debounced) {
-            motion_client.send_motion_controller_queue(can::messages::DebounceMotorDriverError{.message_index = m.message_index});
-            diag0_debounced = true;
+    // no debouncing needed in final product
+    // make messages non-CAN, should be trivial, everything takes std::monostate
+    /*
+    void handle(const motor_control_task_messages::RouteMotorDriverInterrupt& m) {
+        vTaskDelay(pdMS_TO_TICKS(100));
+        debounce_count++;
+        if (debounce_count > 9) {
+            if (controller.read_tmc_diag0()) {
+                motion_client.send_motion_controller_queue(motor_control_task_messages::MotorDriverErrorEncountered{.message_index = m.message_index});
+            } else {
+                motion_client.send_motion_controller_queue(motor_control_task_messages::ResetMotorDriverErrorHandling{.message_index = m.message_index});
+            }
+            diag0_debounced = false;
+            debounce_count = 0;
+        } else {
+            motion_client.send_motion_controller_queue(motor_control_task_messages::RouteMotorDriverInterrupt{.message_index = m.message_index});
         }
     }
 
-    void handle(const can::messages::MotorDriverErrorEncountered& m) {
+    void handle(const motor_control_task_messages::MotorDriverErrorEncountered& m) {
         controller.stop(can::ids::ErrorSeverity::unrecoverable,
                         can::ids::ErrorCode::motor_driver_error_detected);
         if (!controller.is_timer_interrupt_running()) {
@@ -201,7 +212,7 @@ class MotionControllerMessageHandler {
         }
     }
 
-    void handle(const can::messages::ResetMotorDriverErrorHandling& m) {
+    void handle(const motor_control_task_messages::ResetMotorDriverErrorHandling& m) {
         static_cast<void>(m);
         controller.clear_cancel_request();
         can_client.send_can_message(
@@ -212,22 +223,25 @@ class MotionControllerMessageHandler {
                 .error_code =
                     can::ids::ErrorCode::motor_driver_error_detected}); // delete
     }
+    */
 
-    // no debouncing needed in final product
-    // make messages non-CAN, should be trivial, everything takes std::monostate
-    void handle(const can::messages::DebounceMotorDriverError& m) {
-        vTaskDelay(pdMS_TO_TICKS(100));
-        debounce_count++;
-        if (debounce_count > 9) {
-            if (controller.read_tmc_diag0()) {
-                motion_client.send_motion_controller_queue(can::messages::MotorDriverErrorEncountered{.message_index = m.message_index});
-            } else {
-                motion_client.send_motion_controller_queue(can::messages::ResetMotorDriverErrorHandling{.message_index = m.message_index});
+    void handle(const motor_control_task_messages::RouteMotorDriverInterrupt& m) {
+        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::ReadMotorDriverErrorStatus{.message_index = m.message_index});
             }
-            diag0_debounced = false;
-            debounce_count = 0;
         } else {
-            motion_client.send_motion_controller_queue(can::messages::DebounceMotorDriverError{.message_index = m.message_index});
+            controller.clear_cancel_request();
         }
     }
 
@@ -285,17 +299,48 @@ class MotionControllerTask {
 
     [[nodiscard]] auto get_queue() const -> QueueType& { return queue; }
 
+    /*
     void run_diag0_interrupt() {
         if (!diag0_debounced) {
-            static_cast<void>(queue.try_write_isr(
-                can::messages::DebounceMotorDriverError{.message_index = 0}));
+            static_cast<void>(queue.try_write_isr(motor_control_task_messages::RouteMotorDriverInterrupt{.message_index = 0}));
             diag0_debounced = true;
         }
     }
 
+    void run_diag0_interrupt() {
+        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 = 0,
+                        .severity = can::ids::ErrorSeverity::unrecoverable,
+                        .error_code =
+                            can::ids::ErrorCode::motor_driver_error_detected});
+                driver_client->send_motor_driver_queue(
+                    can::messages::ReadMotorDriverErrorStatus{.message_index = 0});
+            }
+        } else {
+            controller->clear_cancel_request();
+        }
+    }
+    */
+
+    void run_diag0_interrupt() {
+        static_cast<void>(queue.try_write_isr(motor_control_task_messages::RouteMotorDriverInterrupt{.message_index = 0}));
+    }
+
   private:
     QueueType& queue;
     std::atomic<bool> diag0_debounced = false;
+    // static motion_controller::MotionController<lms::MotorMechanicalConfig>* controller = nullptr;
+    // static can::message_writer_task::TaskClient* can_client = nullptr;
+    // static tmc::tasks::TaskClient* driver_client = nullptr;
+    // static member object for motion controller (a nullable pointer)
+    // Can that much calling of motion controller be done in interrupt??
+    // do what impacts the least
 };
 
 }  // namespace motion_controller_task
diff --git a/include/pipettes/core/dispatch_builder.hpp b/include/pipettes/core/dispatch_builder.hpp
index be1695ba5..ab9585f88 100644
--- a/include/pipettes/core/dispatch_builder.hpp
+++ b/include/pipettes/core/dispatch_builder.hpp
@@ -65,11 +65,7 @@ using MotionControllerDispatchTarget = can::dispatch::DispatchParseTarget<
     can::messages::SetMotionConstraints, can::messages::ReadLimitSwitchRequest,
     can::messages::MotorPositionRequest,
     can::messages::UpdateMotorPositionEstimationRequest,
-    can::messages::GetMotorUsageRequest,
-    can::messages::RouteMotorDriverInterrupt,
-    can::messages::MotorDriverErrorEncountered,
-    can::messages::ResetMotorDriverErrorHandling,
-    can::messages::DebounceMotorDriverError>;
+    can::messages::GetMotorUsageRequest>;
 
 using GearMotionControllerDispatchTarget = can::dispatch::DispatchParseTarget<
     gear_motion_handler::GearMotorMotionHandler<gear_motor_tasks::QueueClient>,

From 68488ff614219bc7391aa8b04557deede08ab280 Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Mon, 11 Dec 2023 11:10:19 -0500
Subject: [PATCH 45/68] format

---
 gripper/core/tasks_z.cpp                      |  5 +-
 head/core/tasks_proto.cpp                     | 12 +--
 head/core/tasks_rev1.cpp                      | 12 +--
 include/can/core/message_handlers/motion.hpp  |  8 +-
 include/common/core/message_utils.hpp         |  4 +-
 include/motor-control/core/tasks/messages.hpp | 17 ++--
 .../core/tasks/motion_controller_task.hpp     | 80 ++++++++++---------
 .../core/tasks/motion_controller_task.hpp     | 12 +--
 8 files changed, 81 insertions(+), 69 deletions(-)

diff --git a/gripper/core/tasks_z.cpp b/gripper/core/tasks_z.cpp
index b367ede7a..7fb826448 100644
--- a/gripper/core/tasks_z.cpp
+++ b/gripper/core/tasks_z.cpp
@@ -41,8 +41,9 @@ void z_tasks::start_task(
     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, z_queues, z_queues);
+    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(
diff --git a/head/core/tasks_proto.cpp b/head/core/tasks_proto.cpp
index 199278f07..51ec208e5 100644
--- a/head/core/tasks_proto.cpp
+++ b/head/core/tasks_proto.cpp
@@ -140,9 +140,9 @@ void head_tasks::start_tasks(
     head_queues.eeprom_queue = &eeprom_task.get_queue();
 
     // Start the left motor tasks
-    auto& left_motion =
-        left_mc_task_builder.start(5, "left mc", left_motion_controller,
-                                   left_queues, left_queues, left_queues, left_queues);
+    auto& left_motion = left_mc_task_builder.start(
+        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,9 +170,9 @@ void head_tasks::start_tasks(
     left_queues.usage_storage_queue = &left_usage_storage_task.get_queue();
 
     // Start the right motor tasks
-    auto& right_motion =
-        right_mc_task_builder.start(5, "right mc", right_motion_controller,
-                                    right_queues, right_queues, right_queues, right_queues);
+    auto& right_motion = right_mc_task_builder.start(
+        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);
diff --git a/head/core/tasks_rev1.cpp b/head/core/tasks_rev1.cpp
index c0df9536e..dedb0aca3 100644
--- a/head/core/tasks_rev1.cpp
+++ b/head/core/tasks_rev1.cpp
@@ -145,9 +145,9 @@ void head_tasks::start_tasks(
     head_queues.eeprom_queue = &eeprom_task.get_queue();
 #endif
     // Start the left motor tasks
-    auto& left_motion =
-        left_mc_task_builder.start(5, "left mc", left_motion_controller,
-                                   left_queues, left_queues, left_queues, left_queues);
+    auto& left_motion = left_mc_task_builder.start(
+        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,9 +183,9 @@ void head_tasks::start_tasks(
 #endif
 
     // Start the right motor tasks
-    auto& right_motion =
-        right_mc_task_builder.start(5, "right mc", right_motion_controller,
-                                    right_queues, right_queues, right_queues, right_queues);
+    auto& right_motion = right_mc_task_builder.start(
+        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);
diff --git a/include/can/core/message_handlers/motion.hpp b/include/can/core/message_handlers/motion.hpp
index 0787cf138..e49c4e13a 100644
--- a/include/can/core/message_handlers/motion.hpp
+++ b/include/can/core/message_handlers/motion.hpp
@@ -17,7 +17,8 @@ class MotionHandler {
         std::variant<std::monostate, DisableMotorRequest, EnableMotorRequest,
                      GetMotionConstraintsRequest, SetMotionConstraints,
                      ReadLimitSwitchRequest, MotorPositionRequest,
-                     UpdateMotorPositionEstimationRequest, GetMotorUsageRequest>;
+                     UpdateMotorPositionEstimationRequest,
+                     GetMotorUsageRequest>;
 
     MotionHandler(MotionTaskClient &motion_client)
         : motion_client{motion_client} {}
@@ -36,10 +37,11 @@ class MotionHandler {
 
     /*
     void handle_message(const auto &m) {
-        motion_client.send_motion_controller_queue(utils::variant_cast(m)); // try to compile, talk with Seth
+        motion_client.send_motion_controller_queue(utils::variant_cast(m)); //
+    try to compile, talk with Seth
     }
     */
-    
+
     void handle_message(const auto &m) {
         motion_client.send_motion_controller_queue(m);
     }
diff --git a/include/common/core/message_utils.hpp b/include/common/core/message_utils.hpp
index 8eb431dc6..d5ea890a7 100644
--- a/include/common/core/message_utils.hpp
+++ b/include/common/core/message_utils.hpp
@@ -82,8 +82,8 @@ struct variant_cast_proxy
     template <class... ToArgs>
     operator std::variant<ToArgs...>() const
     {
-        return std::visit([](auto&& arg) -> std::variant<ToArgs...> { return arg ; },
-                          v);
+        return std::visit([](auto&& arg) -> std::variant<ToArgs...> { return arg
+; }, v);
     }
 };
 
diff --git a/include/motor-control/core/tasks/messages.hpp b/include/motor-control/core/tasks/messages.hpp
index bd16e28b8..394c8b25a 100644
--- a/include/motor-control/core/tasks/messages.hpp
+++ b/include/motor-control/core/tasks/messages.hpp
@@ -37,11 +37,14 @@ struct Empty : BaseMessage<MId> {
     auto operator==(const Empty&) const -> bool = default;
 };
 
-using RouteMotorDriverInterrupt = Empty<MessageId::route_motor_driver_interrupt>;
+using RouteMotorDriverInterrupt =
+Empty<MessageId::route_motor_driver_interrupt>;
 
-using MotorDriverErrorEncountered = Empty<MessageId::motor_driver_error_encountered>;
+using MotorDriverErrorEncountered =
+Empty<MessageId::motor_driver_error_encountered>;
 
-using ResetMotorDriverErrorHandling = Empty<MessageId::reset_motor_driver_error_handling>;
+using ResetMotorDriverErrorHandling =
+Empty<MessageId::reset_motor_driver_error_handling>;
 
 struct RouteMotorDriverInterrupt {
     uint32_t message_index;
@@ -82,9 +85,8 @@ struct MotorDriverErrorEncountered {
 struct ResetMotorDriverErrorHandling {
     uint32_t message_index;
     template <bit_utils::ByteIterator Input, typename Limit>
-    static auto parse(Input body, Limit limit) -> ResetMotorDriverErrorHandling {
-        uint32_t msg_ind = 0;
-        body = bit_utils::bytes_to_int(body, limit, msg_ind);
+    static auto parse(Input body, Limit limit) -> ResetMotorDriverErrorHandling
+{ uint32_t msg_ind = 0; body = bit_utils::bytes_to_int(body, limit, msg_ind);
         return ResetMotorDriverErrorHandling{.message_index = msg_ind};
     }
 
@@ -94,7 +96,8 @@ struct ResetMotorDriverErrorHandling {
         return iter - body;
     }
 
-    auto operator==(const ResetMotorDriverErrorHandling&) const -> bool = default;
+    auto operator==(const ResetMotorDriverErrorHandling&) const -> bool =
+default;
 };*/
 // needs a member 'id'. This seems to just be becoming a CAN msg!
 
diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp
index d853de95a..212d31734 100644
--- a/include/motor-control/core/tasks/motion_controller_task.hpp
+++ b/include/motor-control/core/tasks/motion_controller_task.hpp
@@ -29,8 +29,7 @@ concept TaskClient = requires(Client client, const TaskMessage& m) {
 template <lms::MotorMechanicalConfig MEConfig,
           can::message_writer_task::TaskClient CanClient,
           usage_storage_task::TaskClient UsageClient,
-          tmc::tasks::TaskClient DriverClient,
-          TaskClient MotionClient>
+          tmc::tasks::TaskClient DriverClient, TaskClient MotionClient>
 class MotionControllerMessageHandler {
   public:
     using MotorControllerType = motion_controller::MotionController<MEConfig>;
@@ -71,9 +70,9 @@ class MotionControllerMessageHandler {
     void handle(const can::messages::EnableMotorRequest& m) {
         LOG("Received enable motor request");
         if (controller.read_tmc_diag0()) {
-            can_client.send_can_message(
-                can::ids::NodeId::host,
-                can::messages::MotorDriverInErrorState{.message_index = m.message_index});
+            can_client.send_can_message(can::ids::NodeId::host,
+                                        can::messages::MotorDriverInErrorState{
+                                            .message_index = m.message_index});
         } else {
             controller.enable_motor();
             can_client.send_can_message(can::ids::NodeId::host,
@@ -117,9 +116,9 @@ class MotionControllerMessageHandler {
             m.velocity, m.acceleration, m.group_id, m.seq_id, m.duration,
             m.request_stop_condition);
         if (controller.read_tmc_diag0()) {
-            can_client.send_can_message(
-                can::ids::NodeId::host,
-                can::messages::MotorDriverInErrorState{.message_index = m.message_index});
+            can_client.send_can_message(can::ids::NodeId::host,
+                                        can::messages::MotorDriverInErrorState{
+                                            .message_index = m.message_index});
         } else {
             controller.move(m);
         }
@@ -130,9 +129,9 @@ class MotionControllerMessageHandler {
             "groupid=%d, seqid=%d\n",
             m.velocity, m.group_id, m.seq_id);
         if (controller.read_tmc_diag0()) {
-            can_client.send_can_message(
-                can::ids::NodeId::host,
-                can::messages::MotorDriverInErrorState{.message_index = m.message_index});
+            can_client.send_can_message(can::ids::NodeId::host,
+                                        can::messages::MotorDriverInErrorState{
+                                            .message_index = m.message_index});
         } else {
             controller.move(m);
         }
@@ -179,24 +178,24 @@ class MotionControllerMessageHandler {
     // no debouncing needed in final product
     // make messages non-CAN, should be trivial, everything takes std::monostate
     /*
-    void handle(const motor_control_task_messages::RouteMotorDriverInterrupt& m) {
-        vTaskDelay(pdMS_TO_TICKS(100));
-        debounce_count++;
-        if (debounce_count > 9) {
-            if (controller.read_tmc_diag0()) {
-                motion_client.send_motion_controller_queue(motor_control_task_messages::MotorDriverErrorEncountered{.message_index = m.message_index});
-            } else {
-                motion_client.send_motion_controller_queue(motor_control_task_messages::ResetMotorDriverErrorHandling{.message_index = m.message_index});
+    void handle(const motor_control_task_messages::RouteMotorDriverInterrupt& m)
+    { vTaskDelay(pdMS_TO_TICKS(100)); debounce_count++; if (debounce_count > 9)
+    { if (controller.read_tmc_diag0()) {
+                motion_client.send_motion_controller_queue(motor_control_task_messages::MotorDriverErrorEncountered{.message_index
+    = m.message_index}); } else {
+                motion_client.send_motion_controller_queue(motor_control_task_messages::ResetMotorDriverErrorHandling{.message_index
+    = m.message_index});
             }
             diag0_debounced = false;
             debounce_count = 0;
         } else {
-            motion_client.send_motion_controller_queue(motor_control_task_messages::RouteMotorDriverInterrupt{.message_index = m.message_index});
+            motion_client.send_motion_controller_queue(motor_control_task_messages::RouteMotorDriverInterrupt{.message_index
+    = m.message_index});
         }
     }
 
-    void handle(const motor_control_task_messages::MotorDriverErrorEncountered& m) {
-        controller.stop(can::ids::ErrorSeverity::unrecoverable,
+    void handle(const motor_control_task_messages::MotorDriverErrorEncountered&
+    m) { controller.stop(can::ids::ErrorSeverity::unrecoverable,
                         can::ids::ErrorCode::motor_driver_error_detected);
         if (!controller.is_timer_interrupt_running()) {
             can_client.send_can_message(
@@ -212,7 +211,8 @@ class MotionControllerMessageHandler {
         }
     }
 
-    void handle(const motor_control_task_messages::ResetMotorDriverErrorHandling& m) {
+    void handle(const
+    motor_control_task_messages::ResetMotorDriverErrorHandling& m) {
         static_cast<void>(m);
         controller.clear_cancel_request();
         can_client.send_can_message(
@@ -221,11 +221,13 @@ class MotionControllerMessageHandler {
                 .message_index = m.message_index,
                 .severity = can::ids::ErrorSeverity::none,
                 .error_code =
-                    can::ids::ErrorCode::motor_driver_error_detected}); // delete
+                    can::ids::ErrorCode::motor_driver_error_detected}); //
+    delete
     }
     */
 
-    void handle(const motor_control_task_messages::RouteMotorDriverInterrupt& m) {
+    void handle(
+        const motor_control_task_messages::RouteMotorDriverInterrupt& m) {
         if (controller.read_tmc_diag0()) {
             controller.stop(can::ids::ErrorSeverity::unrecoverable,
                             can::ids::ErrorCode::motor_driver_error_detected);
@@ -238,7 +240,8 @@ class MotionControllerMessageHandler {
                         .error_code =
                             can::ids::ErrorCode::motor_driver_error_detected});
                 driver_client.send_motor_driver_queue(
-                    can::messages::ReadMotorDriverErrorStatus{.message_index = m.message_index});
+                    can::messages::ReadMotorDriverErrorStatus{
+                        .message_index = m.message_index});
             }
         } else {
             controller.clear_cancel_request();
@@ -276,14 +279,14 @@ class MotionControllerTask {
     template <lms::MotorMechanicalConfig MEConfig,
               can::message_writer_task::TaskClient CanClient,
               usage_storage_task::TaskClient UsageClient,
-              tmc::tasks::TaskClient DriverClient,
-              TaskClient MotionClient>
+              tmc::tasks::TaskClient DriverClient, TaskClient MotionClient>
     [[noreturn]] void operator()(
         motion_controller::MotionController<MEConfig>* controller,
         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};
+            *controller,    *can_client,    *usage_client,
+            *driver_client, *motion_client, diag0_debounced};
         TaskMessage message{};
         bool first_run = true;
         for (;;) {
@@ -302,8 +305,8 @@ class MotionControllerTask {
     /*
     void run_diag0_interrupt() {
         if (!diag0_debounced) {
-            static_cast<void>(queue.try_write_isr(motor_control_task_messages::RouteMotorDriverInterrupt{.message_index = 0}));
-            diag0_debounced = true;
+            static_cast<void>(queue.try_write_isr(motor_control_task_messages::RouteMotorDriverInterrupt{.message_index
+    = 0})); diag0_debounced = true;
         }
     }
 
@@ -320,7 +323,8 @@ class MotionControllerTask {
                         .error_code =
                             can::ids::ErrorCode::motor_driver_error_detected});
                 driver_client->send_motor_driver_queue(
-                    can::messages::ReadMotorDriverErrorStatus{.message_index = 0});
+                    can::messages::ReadMotorDriverErrorStatus{.message_index =
+    0});
             }
         } else {
             controller->clear_cancel_request();
@@ -329,16 +333,18 @@ class MotionControllerTask {
     */
 
     void run_diag0_interrupt() {
-        static_cast<void>(queue.try_write_isr(motor_control_task_messages::RouteMotorDriverInterrupt{.message_index = 0}));
+        static_cast<void>(queue.try_write_isr(
+            motor_control_task_messages::RouteMotorDriverInterrupt{
+                .message_index = 0}));
     }
 
   private:
     QueueType& queue;
     std::atomic<bool> diag0_debounced = false;
-    // static motion_controller::MotionController<lms::MotorMechanicalConfig>* controller = nullptr;
-    // static can::message_writer_task::TaskClient* can_client = nullptr;
-    // static tmc::tasks::TaskClient* driver_client = nullptr;
-    // static member object for motion controller (a nullable pointer)
+    // static motion_controller::MotionController<lms::MotorMechanicalConfig>*
+    // controller = nullptr; static can::message_writer_task::TaskClient*
+    // can_client = nullptr; static tmc::tasks::TaskClient* driver_client =
+    // nullptr; static member object for motion controller (a nullable pointer)
     // Can that much calling of motion controller be done in interrupt??
     // do what impacts the least
 };
diff --git a/include/pipettes/core/tasks/motion_controller_task.hpp b/include/pipettes/core/tasks/motion_controller_task.hpp
index 39d93ca82..ceade960d 100644
--- a/include/pipettes/core/tasks/motion_controller_task.hpp
+++ b/include/pipettes/core/tasks/motion_controller_task.hpp
@@ -62,9 +62,9 @@ class MotionControllerMessageHandler {
         // TODO only toggle the enable pin once since all motors share
         // a single enable pin line.
         if (controller.read_tmc_diag0()) {
-            can_client.send_can_message(
-                can::ids::NodeId::host,
-                can::messages::MotorDriverInErrorState{.message_index = m.message_index});
+            can_client.send_can_message(can::ids::NodeId::host,
+                                        can::messages::MotorDriverInErrorState{
+                                            .message_index = m.message_index});
         } else {
             controller.enable_motor();
             can_client.send_can_message(can::ids::NodeId::host,
@@ -109,9 +109,9 @@ class MotionControllerMessageHandler {
             "acceleration=%d, groupid=%d, seqid=%d\n",
             m.velocity, m.acceleration, m.group_id, m.seq_id);
         if (controller.read_tmc_diag0()) {
-            can_client.send_can_message(
-                can::ids::NodeId::host,
-                can::messages::MotorDriverInErrorState{.message_index = m.message_index});
+            can_client.send_can_message(can::ids::NodeId::host,
+                                        can::messages::MotorDriverInErrorState{
+                                            .message_index = m.message_index});
         } else {
             controller.move(m);
         }

From c6d92c72fcce4508b33658869b1f73b15ba84180 Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Wed, 13 Dec 2023 22:15:10 -0500
Subject: [PATCH 46/68] set up motor driver error surfacing on z-axis board

---
 gantry/firmware/interfaces_rev1.cpp            | 12 ++++++++----
 gripper/core/tasks.cpp                         |  9 ++++++---
 gripper/core/tasks_z.cpp                       | 12 ++++++++++--
 gripper/firmware/interfaces_z_motor.cpp        | 13 ++++++++++---
 gripper/firmware/main_proto.cpp                |  6 ++++--
 gripper/firmware/main_rev1.cpp                 |  6 ++++--
 gripper/firmware/motor_hardware.h              |  2 ++
 gripper/firmware/motor_hardware_shared.c       | 18 ++++++++++++++++++
 gripper/firmware/stm32g4xx_it.c                |  4 ++++
 gripper/firmware/utility_gpio.c                |  7 +++++++
 include/gripper/core/interfaces.hpp            |  4 +++-
 include/gripper/core/tasks.hpp                 | 12 ++++++++----
 include/gripper/firmware/utility_gpio.h        |  1 +
 .../core/stepper_motor/motion_controller.hpp   |  6 ++++++
 14 files changed, 91 insertions(+), 21 deletions(-)

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(); }

From c7af96ea15b4e6589f5cb94c3e98c4312f58ba6c Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Wed, 13 Dec 2023 22:22:47 -0500
Subject: [PATCH 47/68] formatted

---
 gripper/core/tasks.cpp                  |  4 ++--
 gripper/firmware/interfaces_z_motor.cpp | 14 ++++++++------
 include/gripper/core/tasks.hpp          |  2 +-
 3 files changed, 11 insertions(+), 9 deletions(-)

diff --git a/gripper/core/tasks.cpp b/gripper/core/tasks.cpp
index b58b68b44..00a250ace 100644
--- a/gripper/core/tasks.cpp
+++ b/gripper/core/tasks.cpp
@@ -123,8 +123,8 @@ auto gripper_tasks::start_tasks(
     queues.capacitive_sensor_queue_rear =
         &capacitive_sensor_task_rear.get_queue();
 
-    auto diag0_handler = 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);
 
diff --git a/gripper/firmware/interfaces_z_motor.cpp b/gripper/firmware/interfaces_z_motor.cpp
index 36d25611d..66fffe068 100644
--- a/gripper/firmware/interfaces_z_motor.cpp
+++ b/gripper/firmware/interfaces_z_motor.cpp
@@ -103,11 +103,12 @@ struct motion_controller::HardwareConfig motor_pins {
             .port = ESTOP_IN_PORT,
             .pin = ESTOP_IN_PIN,
             .active_setting = GPIO_PIN_RESET},
-    .diag0 = {
-        // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
-        .port = GPIOB,
-        .pin = GPIO_PIN_2,
-        .active_setting = GPIO_PIN_RESET},
+    .diag0 =
+        {
+            // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
+            .port = GPIOB,
+            .pin = GPIO_PIN_2,
+            .active_setting = GPIO_PIN_RESET},
     .ebrake = ebrake
 };
 
@@ -217,7 +218,8 @@ void z_motor_iface::initialize(diag0_handler* call_diag0_handler) {
         Error_Handler();
     }
     initialize_hardware_z();
-    set_z_motor_timer_callback(call_motor_handler, call_diag0_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/include/gripper/core/tasks.hpp b/include/gripper/core/tasks.hpp
index da61ddeca..64233697f 100644
--- a/include/gripper/core/tasks.hpp
+++ b/include/gripper/core/tasks.hpp
@@ -47,7 +47,7 @@ auto start_tasks(can::bus::CanBus& can_bus,
                  eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface,
                  motor_hardware_task::MotorHardwareTask& zmh_tsk,
                  motor_hardware_task::MotorHardwareTask& gmh_tsk)
-                 -> z_motor_iface::diag0_handler;
+    -> z_motor_iface::diag0_handler;
 
 /**
  * Access to all the message queues in the system.

From 19ace4117a9f249515f56814e7e4604f7e84e88e Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Mon, 18 Dec 2023 13:53:37 -0500
Subject: [PATCH 48/68] updated message naming

---
 gripper/simulator/interfaces.cpp               |  3 ++-
 gripper/simulator/main.cpp                     |  6 ++++--
 head/core/can_task.cpp                         |  2 +-
 include/can/core/ids.hpp                       |  4 ++--
 include/can/core/messages.hpp                  | 18 +++++++++---------
 include/gantry/core/can_task.hpp               |  2 +-
 include/gripper/core/can_task.hpp              |  2 +-
 include/gripper/simulation/sim_interfaces.hpp  |  2 ++
 .../stepper_motor/motor_interrupt_handler.hpp  |  2 +-
 .../tasks/gear_tmc2160_motor_driver_task.hpp   |  2 +-
 include/motor-control/core/tasks/messages.hpp  |  2 +-
 .../core/tasks/motion_controller_task.hpp      |  2 +-
 .../core/tasks/tmc2130_motor_driver_task.hpp   |  4 ++--
 .../core/tasks/tmc2160_motor_driver_task.hpp   |  4 ++--
 .../core/tasks/tmc_motor_driver_common.hpp     |  4 ++--
 include/pipettes/core/dispatch_builder.hpp     |  6 +++---
 16 files changed, 35 insertions(+), 30 deletions(-)

diff --git a/gripper/simulator/interfaces.cpp b/gripper/simulator/interfaces.cpp
index dd419450e..d2e354e15 100644
--- a/gripper/simulator/interfaces.cpp
+++ b/gripper/simulator/interfaces.cpp
@@ -135,7 +135,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 831f2673d..c3f235244 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);
@@ -111,9 +113,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 d2f4e7380..b757b1dc6 100644
--- a/head/core/can_task.cpp
+++ b/head/core/can_task.cpp
@@ -29,7 +29,7 @@ using MotorDispatchTarget = can::dispatch::DispatchParseTarget<
     can::messages::ReadMotorDriverRegister,
     can::messages::WriteMotorDriverRegister,
     can::messages::WriteMotorCurrentRequest,
-    can::messages::ReadMotorDriverErrorStatus>;
+    can::messages::ReadMotorDriverErrorStatusRequest>;
 using MoveGroupDispatchTarget = can::dispatch::DispatchParseTarget<
     can::message_handlers::move_group::MoveGroupHandler<
         head_tasks::MotorQueueClient>,
diff --git a/include/can/core/ids.hpp b/include/can/core/ids.hpp
index 17c60ff6f..cc954a1f2 100644
--- a/include/can/core/ids.hpp
+++ b/include/can/core/ids.hpp
@@ -61,8 +61,8 @@ enum class MessageId {
     write_motor_current_request = 0x33,
     read_motor_current_request = 0x34,
     read_motor_current_response = 0x35,
-    read_motor_driver_error_status = 0x36,
-    read_motor_driver_error_register_response = 0x37,
+    read_motor_driver_error_status_request = 0x36,
+    read_motor_driver_error_status_response = 0x37,
     motor_driver_in_error_state = 0x38,
     set_brushed_motor_vref_request = 0x40,
     set_brushed_motor_pwm_request = 0x41,
diff --git a/include/can/core/messages.hpp b/include/can/core/messages.hpp
index 6781b228a..f26f0094f 100644
--- a/include/can/core/messages.hpp
+++ b/include/can/core/messages.hpp
@@ -618,24 +618,24 @@ struct ReadMotorDriverRegisterResponse
         -> bool = default;
 };
 
-struct ReadMotorDriverErrorStatus
-    : BaseMessage<MessageId::read_motor_driver_error_status> {
+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) -> ReadMotorDriverErrorStatus {
+    static auto parse(Input body, Limit limit) -> ReadMotorDriverErrorStatusRequest {
         uint32_t msg_ind = 0;
 
         body = bit_utils::bytes_to_int(body, limit, msg_ind);
-        return ReadMotorDriverErrorStatus{.message_index = msg_ind};
+        return ReadMotorDriverErrorStatusRequest{.message_index = msg_ind};
     }
 
-    auto operator==(const ReadMotorDriverErrorStatus& other) const
+    auto operator==(const ReadMotorDriverErrorStatusRequest& other) const
         -> bool = default;
 };
 
-struct ReadMotorDriverErrorRegisterResponse
-    : BaseMessage<MessageId::read_motor_driver_error_register_response> {
+struct ReadMotorDriverErrorStatusResponse
+    : BaseMessage<MessageId::read_motor_driver_error_status_response> {
     uint32_t message_index;
     uint8_t reg_address;
     uint32_t data;
@@ -648,7 +648,7 @@ struct ReadMotorDriverErrorRegisterResponse
         return iter - body;
     }
 
-    auto operator==(const ReadMotorDriverErrorRegisterResponse& other) const
+    auto operator==(const ReadMotorDriverErrorStatusResponse& other) const
         -> bool = default;
 };
 
@@ -1573,7 +1573,7 @@ using ResponseMessageType = std::variant<
     PeripheralStatusResponse, BrushedMotorConfResponse,
     UpdateMotorPositionEstimationResponse, BaselineSensorResponse,
     PushTipPresenceNotification, GetMotorUsageResponse,
-    ReadMotorDriverErrorRegisterResponse, GripperJawStateResponse,
+    ReadMotorDriverErrorStatusResponse, GripperJawStateResponse,
     MotorDriverInErrorState>;
 
 }  // namespace can::messages
diff --git a/include/gantry/core/can_task.hpp b/include/gantry/core/can_task.hpp
index 3c18301fc..efdb9c61d 100644
--- a/include/gantry/core/can_task.hpp
+++ b/include/gantry/core/can_task.hpp
@@ -22,7 +22,7 @@ using MotorDispatchTarget = can::dispatch::DispatchParseTarget<
     can::messages::ReadMotorDriverRegister,
     can::messages::WriteMotorDriverRegister,
     can::messages::WriteMotorCurrentRequest,
-    can::messages::ReadMotorDriverErrorStatus>;
+    can::messages::ReadMotorDriverErrorStatusRequest>;
 using MoveGroupDispatchTarget = can::dispatch::DispatchParseTarget<
     can::message_handlers::move_group::MoveGroupHandler<
         gantry::queues::QueueClient>,
diff --git a/include/gripper/core/can_task.hpp b/include/gripper/core/can_task.hpp
index 5c7d5a65b..7798a347f 100644
--- a/include/gripper/core/can_task.hpp
+++ b/include/gripper/core/can_task.hpp
@@ -19,7 +19,7 @@ using MotorDispatchTarget = can::dispatch::DispatchParseTarget<
     can::messages::ReadMotorDriverRegister,
     can::messages::WriteMotorDriverRegister,
     can::messages::WriteMotorCurrentRequest,
-    can::messages::ReadMotorDriverErrorStatus>;
+    can::messages::ReadMotorDriverErrorStatusRequest>;
 using MoveGroupDispatchTarget = can::dispatch::DispatchParseTarget<
     can::message_handlers::move_group::MoveGroupHandler<z_tasks::QueueClient>,
     can::messages::AddLinearMoveRequest,
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/motor-control/core/stepper_motor/motor_interrupt_handler.hpp b/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp
index 12f15900d..82e0fef95 100644
--- a/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp
+++ b/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp
@@ -433,7 +433,7 @@ class MotorInterruptHandler {
                                         .error_code = err_code});
         if (err_code == can::ids::ErrorCode::motor_driver_error_detected) {
             driver_client.send_motor_driver_queue(
-                can::messages::ReadMotorDriverErrorStatus{.message_index =
+                can::messages::ReadMotorDriverErrorStatusRequest{.message_index =
                                                               message_index});
         }
         if (err_code == can::ids::ErrorCode::collision_detected) {
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 1760a93c4..517f1e577 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
@@ -87,7 +87,7 @@ class MotorDriverMessageHandler {
         }
     }
 
-    void handle(const can::messages::ReadMotorDriverErrorStatus& m) {
+    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};
diff --git a/include/motor-control/core/tasks/messages.hpp b/include/motor-control/core/tasks/messages.hpp
index 394c8b25a..df012b5df 100644
--- a/include/motor-control/core/tasks/messages.hpp
+++ b/include/motor-control/core/tasks/messages.hpp
@@ -119,7 +119,7 @@ using MotorDriverTaskMessage =
     std::variant<std::monostate, can::messages::ReadMotorDriverRegister,
                  can::messages::WriteMotorDriverRegister,
                  can::messages::WriteMotorCurrentRequest,
-                 can::messages::ReadMotorDriverErrorStatus>;
+                 can::messages::ReadMotorDriverErrorStatusRequest>;
 
 using MoveGroupTaskMessage =
     std::variant<std::monostate, can::messages::AddLinearMoveRequest,
diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp
index 212d31734..c985c3264 100644
--- a/include/motor-control/core/tasks/motion_controller_task.hpp
+++ b/include/motor-control/core/tasks/motion_controller_task.hpp
@@ -240,7 +240,7 @@ class MotionControllerMessageHandler {
                         .error_code =
                             can::ids::ErrorCode::motor_driver_error_detected});
                 driver_client.send_motor_driver_queue(
-                    can::messages::ReadMotorDriverErrorStatus{
+                    can::messages::ReadMotorDriverErrorStatusRequest{
                         .message_index = m.message_index});
             }
         } else {
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 85eb32cf6..3ec884cf2 100644
--- a/include/motor-control/core/tasks/tmc2130_motor_driver_task.hpp
+++ b/include/motor-control/core/tasks/tmc2130_motor_driver_task.hpp
@@ -60,7 +60,7 @@ class MotorDriverMessageHandler {
                 m.rxBuffer);
             if (spi::utils::tag_in_token(
                     m.id.token, spi::utils::ResponseTag::IS_ERROR_RESPONSE)) {
-                can::messages::ReadMotorDriverErrorRegisterResponse
+                can::messages::ReadMotorDriverErrorStatusResponse
                     response_msg{
                         .message_index = m.id.message_index,
                         .reg_address = static_cast<uint8_t>(m.id.token),
@@ -99,7 +99,7 @@ class MotorDriverMessageHandler {
         }
     }
 
-    void handle(const can::messages::ReadMotorDriverErrorStatus& m) {
+    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};
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 568c7d4c6..6ee39198d 100644
--- a/include/motor-control/core/tasks/tmc2160_motor_driver_task.hpp
+++ b/include/motor-control/core/tasks/tmc2160_motor_driver_task.hpp
@@ -60,7 +60,7 @@ class MotorDriverMessageHandler {
                 m.rxBuffer);
             if (spi::utils::tag_in_token(
                     m.id.token, spi::utils::ResponseTag::IS_ERROR_RESPONSE)) {
-                can::messages::ReadMotorDriverErrorRegisterResponse
+                can::messages::ReadMotorDriverErrorStatusResponse
                     response_msg{
                         .message_index = m.id.message_index,
                         .reg_address = static_cast<uint8_t>(m.id.token),
@@ -99,7 +99,7 @@ class MotorDriverMessageHandler {
         }
     }
 
-    void handle(const can::messages::ReadMotorDriverErrorStatus& m) {
+    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};
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 0a132dc9a..f70fef7fe 100644
--- a/include/motor-control/core/tasks/tmc_motor_driver_common.hpp
+++ b/include/motor-control/core/tasks/tmc_motor_driver_common.hpp
@@ -11,10 +11,10 @@ using SpiResponseMessage = std::tuple<spi::messages::TransactResponse>;
 using CanMessageTuple = std::tuple<can::messages::ReadMotorDriverRegister,
                                    can::messages::WriteMotorDriverRegister,
                                    can::messages::WriteMotorCurrentRequest,
-                                   can::messages::ReadMotorDriverErrorStatus>;
+                                   can::messages::ReadMotorDriverErrorStatusRequest>;
 using GearCanMessageTuple =
     std::tuple<can::messages::GearReadMotorDriverRegister,
-               can::messages::ReadMotorDriverErrorStatus,
+               can::messages::ReadMotorDriverErrorStatusRequest,
                can::messages::GearWriteMotorDriverRegister,
                can::messages::GearWriteMotorCurrentRequest>;
 using CanMessage =
diff --git a/include/pipettes/core/dispatch_builder.hpp b/include/pipettes/core/dispatch_builder.hpp
index ab9585f88..43d1ee0da 100644
--- a/include/pipettes/core/dispatch_builder.hpp
+++ b/include/pipettes/core/dispatch_builder.hpp
@@ -24,7 +24,7 @@ using TMC2130MotorDispatchTarget = can::dispatch::DispatchParseTarget<
     can::messages::ReadMotorDriverRegister,
     can::messages::WriteMotorDriverRegister,
     can::messages::WriteMotorCurrentRequest,
-    can::messages::ReadMotorDriverErrorStatus>;
+    can::messages::ReadMotorDriverErrorStatusRequest>;
 
 using TMC2160MotorDispatchTarget = can::dispatch::DispatchParseTarget<
     can::message_handlers::motor::MotorHandler<
@@ -32,13 +32,13 @@ using TMC2160MotorDispatchTarget = can::dispatch::DispatchParseTarget<
     can::messages::ReadMotorDriverRegister,
     can::messages::WriteMotorDriverRegister,
     can::messages::WriteMotorCurrentRequest,
-    can::messages::ReadMotorDriverErrorStatus>;
+    can::messages::ReadMotorDriverErrorStatusRequest>;
 
 using GearMotorDispatchTarget = can::dispatch::DispatchParseTarget<
     can::message_handlers::motor::GearMotorHandler<
         gear_motor_tasks::QueueClient>,
     can::messages::GearReadMotorDriverRegister,
-    can::messages::ReadMotorDriverErrorStatus,
+    can::messages::ReadMotorDriverErrorStatusRequest,
     can::messages::GearWriteMotorDriverRegister,
     can::messages::GearWriteMotorCurrentRequest>;
 

From cd19068aaddbb8362a6e43aeafaff17a468195b3 Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Wed, 3 Jan 2024 11:35:16 -0500
Subject: [PATCH 49/68] implemented motor driver error surfacing for head board

---
 head/core/tasks_proto.cpp             | 19 ++++++++++--
 head/core/tasks_rev1.cpp              | 19 ++++++++++--
 head/firmware/main_proto.cpp          | 33 +++++++++++++++-----
 head/firmware/main_rev1.cpp           | 28 ++++++++++++++---
 head/firmware/motor_hardware.h        |  5 +++-
 head/firmware/motor_hardware_common.c | 43 ++++++++++++++++++++++++++-
 head/firmware/stm32g4xx_it.c          | 11 +++++++
 include/head/core/tasks_proto.hpp     |  7 +++--
 include/head/core/tasks_rev1.hpp      |  7 +++--
 9 files changed, 149 insertions(+), 23 deletions(-)

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.

From 4359c14eb6a7827af68ee0494d56749fb4c62a91 Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Thu, 4 Jan 2024 15:01:08 -0500
Subject: [PATCH 50/68] added debounce

---
 .../core/tasks/motion_controller_task.hpp     | 45 +++++++++++--------
 1 file changed, 27 insertions(+), 18 deletions(-)

diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp
index c985c3264..3491a4612 100644
--- a/include/motor-control/core/tasks/motion_controller_task.hpp
+++ b/include/motor-control/core/tasks/motion_controller_task.hpp
@@ -228,23 +228,31 @@ class MotionControllerMessageHandler {
 
     void handle(
         const motor_control_task_messages::RouteMotorDriverInterrupt& m) {
-        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});
+        vTaskDelay(pdMS_TO_TICKS(100));
+        debounce_count++;
+        if (debounce_count > 9) {
+            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});
+                }
+            } else {
+                controller.clear_cancel_request();
             }
+            diag0_debounced = false;
+            debounce_count = 0;
         } else {
-            controller.clear_cancel_request();
+            motion_client.send_motion_controller_queue(motor_control_task_messages::RouteMotorDriverInterrupt{.message_index = m.message_index});
         }
     }
 
@@ -333,9 +341,10 @@ class MotionControllerTask {
     */
 
     void run_diag0_interrupt() {
-        static_cast<void>(queue.try_write_isr(
-            motor_control_task_messages::RouteMotorDriverInterrupt{
-                .message_index = 0}));
+        if (!diag0_debounced){
+            static_cast<void>(queue.try_write_isr(motor_control_task_messages::RouteMotorDriverInterrupt{.message_index = 0}));
+            diag0_debounced = true;
+        }
     }
 
   private:

From 772ff3c9b0935e9bda672d0b93e4468faccb5a80 Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Thu, 4 Jan 2024 15:37:44 -0500
Subject: [PATCH 51/68] cleaned up logic

---
 include/motor-control/core/tasks/motion_controller_task.hpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp
index 3491a4612..beb7d2cc7 100644
--- a/include/motor-control/core/tasks/motion_controller_task.hpp
+++ b/include/motor-control/core/tasks/motion_controller_task.hpp
@@ -228,8 +228,6 @@ class MotionControllerMessageHandler {
 
     void handle(
         const motor_control_task_messages::RouteMotorDriverInterrupt& m) {
-        vTaskDelay(pdMS_TO_TICKS(100));
-        debounce_count++;
         if (debounce_count > 9) {
             if (controller.read_tmc_diag0()) {
                 controller.stop(can::ids::ErrorSeverity::unrecoverable,
@@ -252,7 +250,9 @@ class MotionControllerMessageHandler {
             diag0_debounced = false;
             debounce_count = 0;
         } else {
+            debounce_count++;
             motion_client.send_motion_controller_queue(motor_control_task_messages::RouteMotorDriverInterrupt{.message_index = m.message_index});
+            vTaskDelay(pdMS_TO_TICKS(100));
         }
     }
 

From 9652cb5afd67efe9a6d8728ab26fb834c7d426a3 Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Tue, 16 Jan 2024 13:07:47 -0500
Subject: [PATCH 52/68] moved delay

---
 include/motor-control/core/tasks/motion_controller_task.hpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp
index beb7d2cc7..d24f58657 100644
--- a/include/motor-control/core/tasks/motion_controller_task.hpp
+++ b/include/motor-control/core/tasks/motion_controller_task.hpp
@@ -251,8 +251,8 @@ class MotionControllerMessageHandler {
             debounce_count = 0;
         } else {
             debounce_count++;
-            motion_client.send_motion_controller_queue(motor_control_task_messages::RouteMotorDriverInterrupt{.message_index = m.message_index});
             vTaskDelay(pdMS_TO_TICKS(100));
+            motion_client.send_motion_controller_queue(motor_control_task_messages::RouteMotorDriverInterrupt{.message_index = m.message_index});
         }
     }
 

From 3ea1c8079bfd4da9b5d90cc810123205d154cda2 Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Wed, 24 Jan 2024 17:21:37 -0500
Subject: [PATCH 53/68] added send_from_isr method

---
 gantry/core/tasks_proto.cpp                               | 5 +++++
 gantry/core/tasks_rev1.cpp                                | 5 +++++
 gripper/core/tasks_z.cpp                                  | 5 +++++
 head/core/tasks_proto.cpp                                 | 5 +++++
 head/core/tasks_rev1.cpp                                  | 5 +++++
 include/gantry/core/queues.hpp                            | 2 ++
 include/gripper/core/tasks.hpp                            | 2 ++
 include/head/core/queues.hpp                              | 2 ++
 .../core/stepper_motor/motor_interrupt_handler.hpp        | 2 +-
 .../core/tasks/tmc2130_motor_driver_task.hpp              | 1 +
 .../core/tasks/tmc2160_motor_driver_task.hpp              | 1 +
 .../motor-control/core/tasks/tmc_motor_driver_common.hpp  | 1 +
 include/pipettes/core/gear_motor_tasks.hpp                | 2 ++
 include/pipettes/core/linear_motor_tasks.hpp              | 8 ++++++++
 include/pipettes/core/tasks.hpp                           | 2 ++
 pipettes/core/gear_motor_tasks.cpp                        | 5 +++++
 16 files changed, 52 insertions(+), 1 deletion(-)

diff --git a/gantry/core/tasks_proto.cpp b/gantry/core/tasks_proto.cpp
index c7577a431..965adbd0f 100644
--- a/gantry/core/tasks_proto.cpp
+++ b/gantry/core/tasks_proto.cpp
@@ -142,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 f376b1e39..4ba6f156c 100644
--- a/gantry/core/tasks_rev1.cpp
+++ b/gantry/core/tasks_rev1.cpp
@@ -141,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/gripper/core/tasks_z.cpp b/gripper/core/tasks_z.cpp
index bdeb75d3b..ce5f9b306 100644
--- a/gripper/core/tasks_z.cpp
+++ b/gripper/core/tasks_z.cpp
@@ -101,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/head/core/tasks_proto.cpp b/head/core/tasks_proto.cpp
index 9567d6011..54c772575 100644
--- a/head/core/tasks_proto.cpp
+++ b/head/core/tasks_proto.cpp
@@ -248,6 +248,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 3b618f4e7..7d3ead190 100644
--- a/head/core/tasks_rev1.cpp
+++ b/head/core/tasks_rev1.cpp
@@ -273,6 +273,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/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/gripper/core/tasks.hpp b/include/gripper/core/tasks.hpp
index 64233697f..4f8641442 100644
--- a/include/gripper/core/tasks.hpp
+++ b/include/gripper/core/tasks.hpp
@@ -191,6 +191,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/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/motor-control/core/stepper_motor/motor_interrupt_handler.hpp b/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp
index 82e0fef95..43a9d957b 100644
--- a/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp
+++ b/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp
@@ -432,7 +432,7 @@ class MotorInterruptHandler {
                                         .severity = severity,
                                         .error_code = err_code});
         if (err_code == can::ids::ErrorCode::motor_driver_error_detected) {
-            driver_client.send_motor_driver_queue(
+            driver_client.send_motor_driver_queue_isr(
                 can::messages::ReadMotorDriverErrorStatusRequest{.message_index =
                                                               message_index});
         }
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 3ec884cf2..9983bd344 100644
--- a/include/motor-control/core/tasks/tmc2130_motor_driver_task.hpp
+++ b/include/motor-control/core/tasks/tmc2130_motor_driver_task.hpp
@@ -176,6 +176,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 6ee39198d..728652258 100644
--- a/include/motor-control/core/tasks/tmc2160_motor_driver_task.hpp
+++ b/include/motor-control/core/tasks/tmc2160_motor_driver_task.hpp
@@ -176,6 +176,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/tmc_motor_driver_common.hpp b/include/motor-control/core/tasks/tmc_motor_driver_common.hpp
index f70fef7fe..5398de1dc 100644
--- a/include/motor-control/core/tasks/tmc_motor_driver_common.hpp
+++ b/include/motor-control/core/tasks/tmc_motor_driver_common.hpp
@@ -39,6 +39,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)};
 };
 
 /**
diff --git a/include/pipettes/core/gear_motor_tasks.hpp b/include/pipettes/core/gear_motor_tasks.hpp
index 292f73b4d..65330b1f1 100644
--- a/include/pipettes/core/gear_motor_tasks.hpp
+++ b/include/pipettes/core/gear_motor_tasks.hpp
@@ -73,6 +73,8 @@ 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/linear_motor_tasks.hpp b/include/pipettes/core/linear_motor_tasks.hpp
index ae20e52d8..3f622d257 100644
--- a/include/pipettes/core/linear_motor_tasks.hpp
+++ b/include/pipettes/core/linear_motor_tasks.hpp
@@ -134,6 +134,10 @@ 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 +177,10 @@ 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/pipettes/core/gear_motor_tasks.cpp b/pipettes/core/gear_motor_tasks.cpp
index c5caabc5d..5355bf2d3 100644
--- a/pipettes/core/gear_motor_tasks.cpp
+++ b/pipettes/core/gear_motor_tasks.cpp
@@ -133,6 +133,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);

From 11214b0afcde20b9fade4794c3579540873f0fa2 Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Wed, 24 Jan 2024 17:25:03 -0500
Subject: [PATCH 54/68] formatted

---
 gripper/simulator/interfaces.cpp              |  2 +-
 head/core/tasks_proto.cpp                     | 14 +++--
 head/core/tasks_rev1.cpp                      | 12 ++--
 head/firmware/main_proto.cpp                  | 58 +++++++++----------
 head/firmware/main_rev1.cpp                   | 14 +++--
 include/can/core/messages.hpp                 |  3 +-
 include/head/core/tasks_proto.hpp             |  3 +-
 include/head/core/tasks_rev1.hpp              |  3 +-
 .../stepper_motor/motor_interrupt_handler.hpp |  4 +-
 .../core/tasks/motion_controller_task.hpp     | 19 +++---
 .../core/tasks/tmc2130_motor_driver_task.hpp  | 11 ++--
 .../core/tasks/tmc2160_motor_driver_task.hpp  | 11 ++--
 .../core/tasks/tmc_motor_driver_common.hpp    |  9 +--
 include/pipettes/core/gear_motor_tasks.hpp    |  3 +-
 include/pipettes/core/linear_motor_tasks.hpp  |  6 +-
 15 files changed, 95 insertions(+), 77 deletions(-)

diff --git a/gripper/simulator/interfaces.cpp b/gripper/simulator/interfaces.cpp
index e7039733c..bdc4ce326 100644
--- a/gripper/simulator/interfaces.cpp
+++ b/gripper/simulator/interfaces.cpp
@@ -136,7 +136,7 @@ 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(diag0_handler *call_diag0_handler) {
+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/head/core/tasks_proto.cpp b/head/core/tasks_proto.cpp
index 54c772575..c3c09d968 100644
--- a/head/core/tasks_proto.cpp
+++ b/head/core/tasks_proto.cpp
@@ -81,13 +81,15 @@ 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();
+        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();
+        return head_tasks::get_right_tasks()
+            .motion_controller->run_diag0_interrupt();
     }
 }
 
@@ -108,7 +110,8 @@ auto 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) -> std::tuple<diag0_handler, diag0_handler> {
+    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);
@@ -214,8 +217,9 @@ auto 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);
+
+    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 7d3ead190..c86699d71 100644
--- a/head/core/tasks_rev1.cpp
+++ b/head/core/tasks_rev1.cpp
@@ -86,13 +86,15 @@ static auto eeprom_data_rev_update_builder =
 
 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();
+        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();
+        return head_tasks::get_right_tasks()
+            .motion_controller->run_diag0_interrupt();
     }
 }
 
@@ -113,7 +115,8 @@ auto 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) -> std::tuple<diag0_handler, diag0_handler> {
+    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);
@@ -236,7 +239,8 @@ auto head_tasks::start_tasks(
     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);
+    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 5d4a44cfa..2dce99539 100644
--- a/head/firmware/main_proto.cpp
+++ b/head/firmware/main_proto.cpp
@@ -146,17 +146,17 @@ struct motor_hardware::HardwareConfig pin_configurations_left {
             .port = GPIOA,
             .pin = GPIO_PIN_8,
             .active_setting = GPIO_PIN_RESET},
-    .estop_in = {
-        // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
-        .port = GPIOB,
-        .pin = GPIO_PIN_4,
-        .active_setting = GPIO_PIN_RESET},
-    .diag0 =
+    .estop_in =
         {
             // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
-            .port = GPIOC,
-            .pin = GPIO_PIN_13,
-            .active_setting = GPIO_PIN_RESET}
+            .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}
 };
 
 struct motor_hardware::HardwareConfig pin_configurations_right {
@@ -196,26 +196,24 @@ struct motor_hardware::HardwareConfig pin_configurations_right {
             .port = GPIOA,
             .pin = GPIO_PIN_8,
             .active_setting = GPIO_PIN_RESET},
-    .estop_in = {
-        // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
-        .port = GPIOB,
-        .pin = GPIO_PIN_4,
-        .active_setting = GPIO_PIN_RESET},
-    .diag0 =
+    .estop_in =
         {
             // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
-            .port = GPIOC,
-            .pin = GPIO_PIN_15,
-            .active_setting = GPIO_PIN_RESET}
+            .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}
 };
 
 // TODO clean up the head main file by using interfaces.
 static tmc2130::configs::TMC2130DriverConfig motor_driver_configs_right{
     .registers =
         {
-            .gconfig = {.en_pwm_mode = 1,
-                        .diag0_error = 1,
-                        .diag0_otpw = 1},
+            .gconfig = {.en_pwm_mode = 1, .diag0_error = 1, .diag0_otpw = 1},
             .ihold_irun = {.hold_current = 0xB,
                            .run_current = 0x19,
                            .hold_current_delay = 0x7},
@@ -242,9 +240,7 @@ static tmc2130::configs::TMC2130DriverConfig motor_driver_configs_right{
 static tmc2130::configs::TMC2130DriverConfig motor_driver_configs_left{
     .registers =
         {
-            .gconfig = {.en_pwm_mode = 1,
-                        .diag0_error = 1,
-                        .diag0_otpw = 1},
+            .gconfig = {.en_pwm_mode = 1, .diag0_error = 1, .diag0_otpw = 1},
             .ihold_irun = {.hold_current = 0xB,
                            .run_current = 0x19,
                            .hold_current_delay = 0x7},
@@ -415,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, &call_diag0_z_handler, &call_diag0_a_handler);
+                     right_enc_overflow_callback_glue, &call_diag0_z_handler,
+                     &call_diag0_a_handler);
 
     if (initialize_spi(&hspi2) != HAL_OK) {
         Error_Handler();
@@ -429,11 +426,12 @@ auto main() -> int {
 
     i2c_setup(&i2c_handles);
     i2c_comms3.set_handle(i2c_handles.i2c3);
-    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);
+    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 dfa4317e6..801298f00 100644
--- a/head/firmware/main_rev1.cpp
+++ b/head/firmware/main_rev1.cpp
@@ -440,7 +440,8 @@ auto main() -> int {
     app_update_clear_flags();
 
     initialize_timer(motor_callback_glue, left_enc_overflow_callback_glue,
-                     right_enc_overflow_callback_glue, &call_diag0_z_handler, &call_diag0_a_handler);
+                     right_enc_overflow_callback_glue, &call_diag0_z_handler,
+                     &call_diag0_a_handler);
 
     i2c_setup(&i2c_handles);
     i2c_comms3.set_handle(i2c_handles.i2c3);
@@ -454,11 +455,12 @@ auto main() -> int {
 
     utility_gpio_init();
     can_bus_1.start(can_bit_timings);
-    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);
+    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/include/can/core/messages.hpp b/include/can/core/messages.hpp
index 0fde7b9de..273fbeb22 100644
--- a/include/can/core/messages.hpp
+++ b/include/can/core/messages.hpp
@@ -623,7 +623,8 @@ struct ReadMotorDriverErrorStatusRequest
     uint32_t message_index;
 
     template <bit_utils::ByteIterator Input, typename Limit>
-    static auto parse(Input body, Limit limit) -> ReadMotorDriverErrorStatusRequest {
+    static auto parse(Input body, Limit limit)
+        -> ReadMotorDriverErrorStatusRequest {
         uint32_t msg_ind = 0;
 
         body = bit_utils::bytes_to_int(body, limit, msg_ind);
diff --git a/include/head/core/tasks_proto.hpp b/include/head/core/tasks_proto.hpp
index decacd3ab..8be17643e 100644
--- a/include/head/core/tasks_proto.hpp
+++ b/include/head/core/tasks_proto.hpp
@@ -43,7 +43,8 @@ auto 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) -> std::tuple<diag0_handler, diag0_handler>;
+    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 9f6d0f84e..430c89cf4 100644
--- a/include/head/core/tasks_rev1.hpp
+++ b/include/head/core/tasks_rev1.hpp
@@ -43,7 +43,8 @@ auto 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) -> std::tuple<diag0_handler, diag0_handler>;
+    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/stepper_motor/motor_interrupt_handler.hpp b/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp
index 43a9d957b..3715aa491 100644
--- a/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp
+++ b/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp
@@ -433,8 +433,8 @@ class MotorInterruptHandler {
                                         .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});
+                can::messages::ReadMotorDriverErrorStatusRequest{
+                    .message_index = message_index});
         }
         if (err_code == can::ids::ErrorCode::collision_detected) {
             build_and_send_ack(AckMessageId::position_error);
diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp
index d24f58657..9cc020263 100644
--- a/include/motor-control/core/tasks/motion_controller_task.hpp
+++ b/include/motor-control/core/tasks/motion_controller_task.hpp
@@ -230,16 +230,17 @@ class MotionControllerMessageHandler {
         const motor_control_task_messages::RouteMotorDriverInterrupt& m) {
         if (debounce_count > 9) {
             if (controller.read_tmc_diag0()) {
-                controller.stop(can::ids::ErrorSeverity::unrecoverable,
-                                can::ids::ErrorCode::motor_driver_error_detected);
+                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});
+                            .error_code = can::ids::ErrorCode::
+                                motor_driver_error_detected});
                     driver_client.send_motor_driver_queue(
                         can::messages::ReadMotorDriverErrorStatusRequest{
                             .message_index = m.message_index});
@@ -252,7 +253,9 @@ class MotionControllerMessageHandler {
         } else {
             debounce_count++;
             vTaskDelay(pdMS_TO_TICKS(100));
-            motion_client.send_motion_controller_queue(motor_control_task_messages::RouteMotorDriverInterrupt{.message_index = m.message_index});
+            motion_client.send_motion_controller_queue(
+                motor_control_task_messages::RouteMotorDriverInterrupt{
+                    .message_index = m.message_index});
         }
     }
 
@@ -341,8 +344,10 @@ class MotionControllerTask {
     */
 
     void run_diag0_interrupt() {
-        if (!diag0_debounced){
-            static_cast<void>(queue.try_write_isr(motor_control_task_messages::RouteMotorDriverInterrupt{.message_index = 0}));
+        if (!diag0_debounced) {
+            static_cast<void>(queue.try_write_isr(
+                motor_control_task_messages::RouteMotorDriverInterrupt{
+                    .message_index = 0}));
             diag0_debounced = true;
         }
     }
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 9983bd344..526b0ebaf 100644
--- a/include/motor-control/core/tasks/tmc2130_motor_driver_task.hpp
+++ b/include/motor-control/core/tasks/tmc2130_motor_driver_task.hpp
@@ -60,12 +60,11 @@ class MotorDriverMessageHandler {
                 m.rxBuffer);
             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::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 {
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 728652258..aec315ea7 100644
--- a/include/motor-control/core/tasks/tmc2160_motor_driver_task.hpp
+++ b/include/motor-control/core/tasks/tmc2160_motor_driver_task.hpp
@@ -60,12 +60,11 @@ class MotorDriverMessageHandler {
                 m.rxBuffer);
             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::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 {
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 5398de1dc..53b0be14b 100644
--- a/include/motor-control/core/tasks/tmc_motor_driver_common.hpp
+++ b/include/motor-control/core/tasks/tmc_motor_driver_common.hpp
@@ -8,10 +8,11 @@ namespace tmc {
 namespace tasks {
 
 using SpiResponseMessage = std::tuple<spi::messages::TransactResponse>;
-using CanMessageTuple = std::tuple<can::messages::ReadMotorDriverRegister,
-                                   can::messages::WriteMotorDriverRegister,
-                                   can::messages::WriteMotorCurrentRequest,
-                                   can::messages::ReadMotorDriverErrorStatusRequest>;
+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,
diff --git a/include/pipettes/core/gear_motor_tasks.hpp b/include/pipettes/core/gear_motor_tasks.hpp
index 65330b1f1..c1d693302 100644
--- a/include/pipettes/core/gear_motor_tasks.hpp
+++ b/include/pipettes/core/gear_motor_tasks.hpp
@@ -73,7 +73,8 @@ 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_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/linear_motor_tasks.hpp b/include/pipettes/core/linear_motor_tasks.hpp
index 3f622d257..854b7b1f6 100644
--- a/include/pipettes/core/linear_motor_tasks.hpp
+++ b/include/pipettes/core/linear_motor_tasks.hpp
@@ -134,7 +134,8 @@ struct QueueClient : can::message_writer::MessageWriter {
         driver_queue->try_write(m);
     }
 
-    void send_motor_driver_queue_isr(const tmc2130::tasks::TaskMessage& m) const {
+    void send_motor_driver_queue_isr(
+        const tmc2130::tasks::TaskMessage& m) const {
         static_cast<void>(driver_queue->try_write_isr(m));
     }
 
@@ -177,7 +178,8 @@ struct QueueClient : can::message_writer::MessageWriter {
         driver_queue->try_write(m);
     }
 
-    void send_motor_driver_queue_isr(const tmc2160::tasks::TaskMessage& m) const {
+    void send_motor_driver_queue_isr(
+        const tmc2160::tasks::TaskMessage& m) const {
         static_cast<void>(driver_queue->try_write_isr(m));
     }
 

From 7a8dbfeed10132df303f88824a97dad3c1c0d1a9 Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Thu, 25 Jan 2024 16:49:25 -0500
Subject: [PATCH 55/68] fixed tests and HAL_GPIO_EXTI_Callback

---
 gripper/firmware/motor_hardware_shared.c                 | 6 +-----
 include/motor-control/tests/mock_motor_driver_client.hpp | 4 ++++
 2 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/gripper/firmware/motor_hardware_shared.c b/gripper/firmware/motor_hardware_shared.c
index 2017fd2e3..f6118af8b 100644
--- a/gripper/firmware/motor_hardware_shared.c
+++ b/gripper/firmware/motor_hardware_shared.c
@@ -228,11 +228,7 @@ void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
                 (*diag0_callback)();
             }
         }
-    }
-}
-
-void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
-    if (GPIO_Pin == ESTOP_IN_PIN) {
+    } 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/include/motor-control/tests/mock_motor_driver_client.hpp b/include/motor-control/tests/mock_motor_driver_client.hpp
index ac760bc14..d9651bb69 100644
--- a/include/motor-control/tests/mock_motor_driver_client.hpp
+++ b/include/motor-control/tests/mock_motor_driver_client.hpp
@@ -11,6 +11,10 @@ struct MockMotorDriverClient {
         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{};
 };
 

From c8aa62c218f6d6292818cb1d72f07ee84c0641aa Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Tue, 30 Jan 2024 14:23:28 -0500
Subject: [PATCH 56/68] pipette motor driver error surfacing

---
 head/firmware/stm32g4xx_it.c                 |  6 +--
 include/pipettes/core/linear_motor_tasks.hpp | 19 ++++----
 include/pipettes/firmware/interfaces_g4.hpp  | 12 ++---
 pipettes/core/linear_motor_tasks.cpp         | 26 ++++++----
 pipettes/firmware/hardware_config.c          | 50 +++++++++++++-------
 pipettes/firmware/hardware_config.h          |  2 +-
 pipettes/firmware/main.cpp                   | 12 ++++-
 pipettes/firmware/motor_configurations.cpp   | 17 ++++++-
 pipettes/firmware/motor_hardware.c           | 17 +++++--
 pipettes/firmware/stm32g4xx_it.c             |  4 ++
 pipettes/firmware/utility_gpio.c             | 10 ++++
 11 files changed, 117 insertions(+), 58 deletions(-)

diff --git a/head/firmware/stm32g4xx_it.c b/head/firmware/stm32g4xx_it.c
index 60100c6f2..3a9591e0e 100644
--- a/head/firmware/stm32g4xx_it.c
+++ b/head/firmware/stm32g4xx_it.c
@@ -179,11 +179,9 @@ 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)) {
+    if (__HAL_GPIO_EXTI_GET_IT(GPIO_PIN_13)) {
         HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_13);
-    } else if (HAL_EXTI_GetPending(&hexti_15, 0)) {
+    } else if (__HAL_GPIO_EXTI_GET_IT(GPIO_PIN_15)) {
         HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_15);
     }
 }
diff --git a/include/pipettes/core/linear_motor_tasks.hpp b/include/pipettes/core/linear_motor_tasks.hpp
index 854b7b1f6..d1f30e72f 100644
--- a/include/pipettes/core/linear_motor_tasks.hpp
+++ b/include/pipettes/core/linear_motor_tasks.hpp
@@ -13,6 +13,7 @@
 #include "motor-control/core/tasks/tmc2130_motor_driver_task.hpp"
 #include "motor-control/core/tasks/tmc2160_motor_driver_task.hpp"
 #include "pipettes/core/sensor_tasks.hpp"
+#include "pipettes/firmware/interfaces_g4.hpp"
 #include "spi/core/writer.hpp"
 
 /**
@@ -31,26 +32,24 @@ 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,
+    motion_controller::MotionController<lms::LeadScrewConfig>& motion_controller,
     SPIWriterClient& spi_writer,
     tmc2130::configs::TMC2130DriverConfig& linear_driver_configs,
     can::ids::NodeId, motor_hardware_task::MotorHardwareTask& lmh_tsk,
-    eeprom::dev_data::DevDataTailAccessor<sensor_tasks::QueueClient>&
-        tail_accessor);
+    eeprom::dev_data::DevDataTailAccessor<sensor_tasks::QueueClient>& tail_accessor) -> interfaces::linear_motor::diag0_handler;
 
 // 96/384 linear motor tasks
-void start_tasks(
+auto start_tasks(
     CanWriterTask& can_writer,
-    motion_controller::MotionController<lms::LeadScrewConfig>&
-        motion_controller,
+    motion_controller::MotionController<lms::LeadScrewConfig>& motion_controller,
     SPIWriterClient& spi_writer,
     tmc2160::configs::TMC2160DriverConfig& linear_driver_configs,
     can::ids::NodeId, motor_hardware_task::MotorHardwareTask& lmh_tsk,
-    eeprom::dev_data::DevDataTailAccessor<sensor_tasks::QueueClient>&
-        tail_accessor);
+    eeprom::dev_data::DevDataTailAccessor<sensor_tasks::QueueClient>& tail_accessor) -> interfaces::linear_motor::diag0_handler;
+
+void call_run_diag0_interrupt()
 
 /**
  * Access to all the linear motion task queues on the pipette.
diff --git a/include/pipettes/firmware/interfaces_g4.hpp b/include/pipettes/firmware/interfaces_g4.hpp
index 63d63b183..83cdc6600 100644
--- a/include/pipettes/firmware/interfaces_g4.hpp
+++ b/include/pipettes/firmware/interfaces_g4.hpp
@@ -73,18 +73,14 @@ auto get_interrupt_queues<PipetteType::THREE_EIGHTY_FOUR_CHANNEL>()
 
 namespace linear_motor {
 
+extern "C" using diag0_handler = void(*)();
+
 auto get_interrupt(motor_hardware::MotorHardware& hw,
                    LowThroughputInterruptQueues& queues,
-                   stall_check::StallCheck& stall)
-    -> MotorInterruptHandlerType<
-        linear_motor_tasks::QueueClient,
-        linear_motor_tasks::tmc2130_driver::QueueClient>;
+                   stall_check::StallCheck& stall) -> 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,
-        linear_motor_tasks::tmc2160_driver::QueueClient>;
+                   stall_check::StallCheck& stall) -> 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/pipettes/core/linear_motor_tasks.cpp b/pipettes/core/linear_motor_tasks.cpp
index a0fa09a1d..1f239be0d 100644
--- a/pipettes/core/linear_motor_tasks.cpp
+++ b/pipettes/core/linear_motor_tasks.cpp
@@ -31,15 +31,13 @@ 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,
+    motion_controller::MotionController<lms::LeadScrewConfig>& motion_controller,
     linear_motor_tasks::SPIWriterClient& spi_writer,
     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) {
+    eeprom::dev_data::DevDataTailAccessor<sensor_tasks::QueueClient>& tail_accessor) -> interfaces::linear_motor::diag0_handler {
     tmc2130_queue_client.set_node_id(id);
     motion_queue_client.set_node_id(id);
 
@@ -81,17 +79,17 @@ 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,
+    motion_controller::MotionController<lms::LeadScrewConfig>& motion_controller,
     linear_motor_tasks::SPIWriterClient& spi_writer,
     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) {
+    eeprom::dev_data::DevDataTailAccessor<sensor_tasks::QueueClient>& tail_accessor) -> interfaces::linear_motor::diag0_handler {
     tmc2160_queue_client.set_node_id(id);
     motion_queue_client.set_node_id(id);
 
@@ -135,6 +133,14 @@ 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..22b790d2d 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
@@ -140,7 +146,7 @@ static uint16_t get_spi_pins_lt(GPIO_TypeDef* for_handle) {
     }
 }
 
-static uint16_t get_motor_driver_pins_ht(GPIO_TypeDef* for_handle) {
+static uint16_t get_motor_driver_pins_ht(GPIO_TypeDef* for_handle, bool for_diag0) {
     /*
      * Dir Pins
      * plunger -> PA6
@@ -156,29 +162,41 @@ static uint16_t get_motor_driver_pins_ht(GPIO_TypeDef* for_handle) {
      * Motor Enable
      * pickup enable -> PA10
      * plunger enable -> PB4
+     * 
+     * Diag0 Pin
+     * PB6
      */
-    switch((uint32_t)for_handle) {
-        case (uint32_t)GPIOA: return GPIO_PIN_6 | GPIO_PIN_7 | GPIO_PIN_10;
-        case (uint32_t)GPIOB: return GPIO_PIN_0 | GPIO_PIN_1 | GPIO_PIN_7 | GPIO_PIN_4;
-        case (uint32_t)GPIOC: return GPIO_PIN_2;
-        default: return 0;
+    if (!for_diag0) {
+        switch((uint32_t)for_handle) {
+            case (uint32_t)GPIOA: return GPIO_PIN_6 | GPIO_PIN_7 | GPIO_PIN_10;
+            case (uint32_t)GPIOB: return GPIO_PIN_0 | GPIO_PIN_1 | GPIO_PIN_7 | GPIO_PIN_4;
+            case (uint32_t)GPIOC: return GPIO_PIN_2;
+            default: return 0;
+        }
+    } else {
+        return GPIO_PIN_6;
     }
 }
 
-static uint16_t get_motor_driver_pins_lt(GPIO_TypeDef* for_handle) {
+static uint16_t get_motor_driver_pins_lt(GPIO_TypeDef* for_handle, bool for_diag0) {
     /*
      * Dir Pin -> PC6
      *
      * Step Pin -> PC7
      * Enable Pin -> PA10
      *
-     * VREF (TMC2130)
-     * PA5
+     * VREF (TMC2130) -> PA5
+     * 
+     * DIAG0 Pin -> PC11
      */
-    switch((uint32_t)for_handle) {
-        case (uint32_t)GPIOA: return GPIO_PIN_10;
-        case (uint32_t)GPIOC: return GPIO_PIN_6 | GPIO_PIN_7;
-        default: return 0;
+    if (!for_diag0) {
+        switch((uint32_t)for_handle) {
+            case (uint32_t)GPIOA: return GPIO_PIN_10;
+            case (uint32_t)GPIOC: return GPIO_PIN_6 | GPIO_PIN_7;
+            default: return 0;
+        }
+    } else {
+        return GPIO_PIN_11;
     }
 }
 
@@ -208,14 +226,14 @@ uint16_t pipette_hardware_spi_pins(const PipetteType pipette_type, GPIO_TypeDef*
     }
 }
 
-uint16_t pipette_hardware_motor_driver_pins(const PipetteType pipette_type, GPIO_TypeDef* for_handle) {
+uint16_t pipette_hardware_motor_driver_pins(const PipetteType pipette_type, GPIO_TypeDef* for_handle, bool for_diag0) {
     switch (pipette_type) {
         case NINETY_SIX_CHANNEL:
         case THREE_EIGHTY_FOUR_CHANNEL:
-            return get_motor_driver_pins_ht(for_handle);
+            return get_motor_driver_pins_ht(for_handle, for_diag0);
         case SINGLE_CHANNEL:
         case EIGHT_CHANNEL:
         default:
-            return get_motor_driver_pins_lt(for_handle);
+            return get_motor_driver_pins_lt(for_handle, for_diag0);
     }
 }
diff --git a/pipettes/firmware/hardware_config.h b/pipettes/firmware/hardware_config.h
index d203ddb49..0f499d9c0 100644
--- a/pipettes/firmware/hardware_config.h
+++ b/pipettes/firmware/hardware_config.h
@@ -24,6 +24,6 @@ typedef enum {
 }GPIOInterruptBlock;
 
 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_pins(const PipetteType pipette_type, GPIO_TypeDef* for_handle, bool for_diag0);
 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/main.cpp b/pipettes/firmware/main.cpp
index 7baaeb2e4..53ac46ec0 100644
--- a/pipettes/firmware/main.cpp
+++ b/pipettes/firmware/main.cpp
@@ -45,6 +45,8 @@ constexpr auto PIPETTE_TYPE = get_pipette_type();
 
 static auto iWatchdog = iwdg::IndependentWatchDog{};
 
+static interfaces::linear_motor::diag0_handler call_diag0_handler = nullptr;
+
 static auto can_bus_1 = can::hal::bus::HalCanBus(
     can_get_device_handle(), utility_configs::led_gpio(PIPETTE_TYPE));
 
@@ -144,6 +146,12 @@ 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.diag0.pin) {
+        if (call_diag0_handler != NULL) {
+            if (*call_diag0_handler != NULL) {
+                (*call_diag0_handler)();
+            }
+        }
     }
 }
 
@@ -188,7 +196,7 @@ 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_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);
@@ -223,7 +231,7 @@ auto initialize_motor_tasks(
 
     initialize_linear_timer(plunger_callback);
     initialize_enc_timer(encoder_callback);
-    linear_motor_tasks::start_tasks(
+    call_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..a074c3ac4 100644
--- a/pipettes/firmware/motor_configurations.cpp
+++ b/pipettes/firmware/motor_configurations.cpp
@@ -64,7 +64,8 @@ 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 +80,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 +101,8 @@ 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},
             };
     }
 }
@@ -265,6 +273,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..9a3815b2e 100644
--- a/pipettes/firmware/motor_hardware.c
+++ b/pipettes/firmware/motor_hardware.c
@@ -83,7 +83,7 @@ void motor_driver_gpio_init() {
     GPIO_InitTypeDef GPIO_InitStruct = {0};
     PipetteType pipette_type = get_pipette_type();
 
-    GPIO_InitStruct.Pin = pipette_hardware_motor_driver_pins(pipette_type, GPIOC);
+    GPIO_InitStruct.Pin = pipette_hardware_motor_driver_pins(pipette_type, GPIOC, false);
     GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
     GPIO_InitStruct.Pull = GPIO_NOPULL;
     GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
@@ -92,16 +92,23 @@ void motor_driver_gpio_init() {
     if (pipette_type != NINETY_SIX_CHANNEL) {
         // Driver Clock Pin.
         // Enable Dir/Step pin
-        GPIO_InitStruct.Pin = pipette_hardware_motor_driver_pins(pipette_type, GPIOA);
+        GPIO_InitStruct.Pin = pipette_hardware_motor_driver_pins(pipette_type, GPIOA, false);
         HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
+        // Diag0 pin
+        GPIO_InitStruct.Pin = pipette_hardware_motor_driver_pins(pipette_type, GPIOC, true);
+        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);
+        GPIO_InitStruct.Pin = pipette_hardware_motor_driver_pins(pipette_type, GPIOA, false);
         HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
         // Enable/Dir/Step pin
-        GPIO_InitStruct.Pin = pipette_hardware_motor_driver_pins(pipette_type, GPIOB);
+        GPIO_InitStruct.Pin = pipette_hardware_motor_driver_pins(pipette_type, GPIOB, false);
+        HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
+        // Diag0 pin
+        GPIO_InitStruct.Pin = pipette_hardware_motor_driver_pins(pipette_type, GPIOB, true);
+        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);
     }
 }
 

From 62a09c81e9747177798903861897601253f28e67 Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Tue, 30 Jan 2024 16:09:13 -0500
Subject: [PATCH 57/68] pipette subsystem fixes

---
 .../firmware/stepper_motor/motor_hardware.hpp    |  2 ++
 include/pipettes/core/interfaces.hpp             |  6 ++++++
 include/pipettes/core/linear_motor_tasks.hpp     | 16 ++++++++++------
 include/pipettes/firmware/interfaces_g4.hpp      | 12 ++++++++----
 pipettes/core/linear_motor_tasks.cpp             | 15 ++++++++++-----
 pipettes/firmware/hardware_config.h              |  1 +
 pipettes/firmware/main.cpp                       |  2 +-
 pipettes/firmware/motor_configurations.cpp       |  6 ++----
 8 files changed, 40 insertions(+), 20 deletions(-)

diff --git a/include/motor-control/firmware/stepper_motor/motor_hardware.hpp b/include/motor-control/firmware/stepper_motor/motor_hardware.hpp
index 157e0b90e..ca7be4974 100644
--- a/include/motor-control/firmware/stepper_motor/motor_hardware.hpp
+++ b/include/motor-control/firmware/stepper_motor/motor_hardware.hpp
@@ -80,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{};
diff --git a/include/pipettes/core/interfaces.hpp b/include/pipettes/core/interfaces.hpp
index 91a6cefbb..18671e880 100644
--- a/include/pipettes/core/interfaces.hpp
+++ b/include/pipettes/core/interfaces.hpp
@@ -32,6 +32,12 @@ struct HighThroughputInterruptQueues {
     UpdatePositionQueue left_update_queue;
 };
 
+namespace linear_motor {
+
+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 d1f30e72f..8316523c0 100644
--- a/include/pipettes/core/linear_motor_tasks.hpp
+++ b/include/pipettes/core/linear_motor_tasks.hpp
@@ -12,8 +12,8 @@
 #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 "pipettes/firmware/interfaces_g4.hpp"
 #include "spi/core/writer.hpp"
 
 /**
@@ -34,22 +34,26 @@ using SPIWriterClient =
 // single channel/8 channel linear motor tasks
 auto start_tasks(
     CanWriterTask& can_writer,
-    motion_controller::MotionController<lms::LeadScrewConfig>& motion_controller,
+    motion_controller::MotionController<lms::LeadScrewConfig>&
+        motion_controller,
     SPIWriterClient& spi_writer,
     tmc2130::configs::TMC2130DriverConfig& linear_driver_configs,
     can::ids::NodeId, motor_hardware_task::MotorHardwareTask& lmh_tsk,
-    eeprom::dev_data::DevDataTailAccessor<sensor_tasks::QueueClient>& tail_accessor) -> interfaces::linear_motor::diag0_handler;
+    eeprom::dev_data::DevDataTailAccessor<sensor_tasks::QueueClient>&
+        tail_accessor) -> interfaces::linear_motor::diag0_handler;
 
 // 96/384 linear motor tasks
 auto start_tasks(
     CanWriterTask& can_writer,
-    motion_controller::MotionController<lms::LeadScrewConfig>& motion_controller,
+    motion_controller::MotionController<lms::LeadScrewConfig>&
+        motion_controller,
     SPIWriterClient& spi_writer,
     tmc2160::configs::TMC2160DriverConfig& linear_driver_configs,
     can::ids::NodeId, motor_hardware_task::MotorHardwareTask& lmh_tsk,
-    eeprom::dev_data::DevDataTailAccessor<sensor_tasks::QueueClient>& tail_accessor) -> interfaces::linear_motor::diag0_handler;
+    eeprom::dev_data::DevDataTailAccessor<sensor_tasks::QueueClient>&
+        tail_accessor) -> interfaces::linear_motor::diag0_handler;
 
-void call_run_diag0_interrupt()
+void call_run_diag0_interrupt();
 
 /**
  * Access to all the linear motion task queues on the pipette.
diff --git a/include/pipettes/firmware/interfaces_g4.hpp b/include/pipettes/firmware/interfaces_g4.hpp
index 83cdc6600..63d63b183 100644
--- a/include/pipettes/firmware/interfaces_g4.hpp
+++ b/include/pipettes/firmware/interfaces_g4.hpp
@@ -73,14 +73,18 @@ auto get_interrupt_queues<PipetteType::THREE_EIGHTY_FOUR_CHANNEL>()
 
 namespace linear_motor {
 
-extern "C" using diag0_handler = void(*)();
-
 auto get_interrupt(motor_hardware::MotorHardware& hw,
                    LowThroughputInterruptQueues& queues,
-                   stall_check::StallCheck& stall) -> MotorInterruptHandlerType<linear_motor_tasks::QueueClient, linear_motor_tasks::tmc2130_driver::QueueClient>;
+                   stall_check::StallCheck& stall)
+    -> 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, linear_motor_tasks::tmc2160_driver::QueueClient>;
+                   stall_check::StallCheck& stall)
+    -> 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/pipettes/core/linear_motor_tasks.cpp b/pipettes/core/linear_motor_tasks.cpp
index 1f239be0d..334e3b550 100644
--- a/pipettes/core/linear_motor_tasks.cpp
+++ b/pipettes/core/linear_motor_tasks.cpp
@@ -33,11 +33,13 @@ static auto eeprom_data_rev_update_builder =
 
 auto linear_motor_tasks::start_tasks(
     linear_motor_tasks::CanWriterTask& can_writer,
-    motion_controller::MotionController<lms::LeadScrewConfig>& motion_controller,
+    motion_controller::MotionController<lms::LeadScrewConfig>&
+        motion_controller,
     linear_motor_tasks::SPIWriterClient& spi_writer,
     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) -> interfaces::linear_motor::diag0_handler {
+    eeprom::dev_data::DevDataTailAccessor<sensor_tasks::QueueClient>&
+        tail_accessor) -> interfaces::linear_motor::diag0_handler {
     tmc2130_queue_client.set_node_id(id);
     motion_queue_client.set_node_id(id);
 
@@ -85,11 +87,13 @@ auto linear_motor_tasks::start_tasks(
 
 auto linear_motor_tasks::start_tasks(
     linear_motor_tasks::CanWriterTask& can_writer,
-    motion_controller::MotionController<lms::LeadScrewConfig>& motion_controller,
+    motion_controller::MotionController<lms::LeadScrewConfig>&
+        motion_controller,
     linear_motor_tasks::SPIWriterClient& spi_writer,
     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) -> interfaces::linear_motor::diag0_handler {
+    eeprom::dev_data::DevDataTailAccessor<sensor_tasks::QueueClient>&
+        tail_accessor) -> interfaces::linear_motor::diag0_handler {
     tmc2160_queue_client.set_node_id(id);
     motion_queue_client.set_node_id(id);
 
@@ -139,7 +143,8 @@ auto linear_motor_tasks::start_tasks(
 
 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();
+        return linear_motor_tasks::get_tasks()
+            .motion_controller->run_diag0_interrupt();
     }
 }
 
diff --git a/pipettes/firmware/hardware_config.h b/pipettes/firmware/hardware_config.h
index 0f499d9c0..4a0ec859c 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"
 
diff --git a/pipettes/firmware/main.cpp b/pipettes/firmware/main.cpp
index 53ac46ec0..2d30ed5ce 100644
--- a/pipettes/firmware/main.cpp
+++ b/pipettes/firmware/main.cpp
@@ -146,7 +146,7 @@ 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.diag0.pin) {
+    } else if (GPIO_Pin == linear_motor_hardware.get_pins().diag0.pin) {
         if (call_diag0_handler != NULL) {
             if (*call_diag0_handler != NULL) {
                 (*call_diag0_handler)();
diff --git a/pipettes/firmware/motor_configurations.cpp b/pipettes/firmware/motor_configurations.cpp
index a074c3ac4..c6f85e37b 100644
--- a/pipettes/firmware/motor_configurations.cpp
+++ b/pipettes/firmware/motor_configurations.cpp
@@ -64,8 +64,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,
-                                          .diag0_error = 1},
+                .registers = {.gconfig = {.en_pwm_mode = 0, .diag0_error = 1},
                               .ihold_irun = {.hold_current = 16,
                                              .run_current = 31,
                                              .hold_current_delay = 0x7},
@@ -101,8 +100,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,
-                                          .diag0_error = 1},
+                .registers = {.gconfig = {.en_pwm_mode = 1, .diag0_error = 1},
                               .ihold_irun = {.hold_current = 0x2,
                                              .run_current = 0x10,
                                              .hold_current_delay = 0x7},

From 62e582547238ac15269c00da9dce1970141fe43d Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Mon, 12 Feb 2024 15:46:00 -0500
Subject: [PATCH 58/68] refactored debounce mechanism, changed
 existing-error-present message, cleaned up motion controller code

---
 include/can/core/ids.hpp                      |   1 -
 include/can/core/messages.hpp                 |   5 +-
 include/motor-control/core/tasks/messages.hpp |  96 +-------------
 .../core/tasks/motion_controller_task.hpp     | 118 ++----------------
 .../core/tasks/motion_controller_task.hpp     |   8 +-
 pipettes/firmware/main.cpp                    |   4 +-
 6 files changed, 18 insertions(+), 214 deletions(-)

diff --git a/include/can/core/ids.hpp b/include/can/core/ids.hpp
index b915e2853..621e9f3ee 100644
--- a/include/can/core/ids.hpp
+++ b/include/can/core/ids.hpp
@@ -63,7 +63,6 @@ enum class MessageId {
     read_motor_current_response = 0x35,
     read_motor_driver_error_status_request = 0x36,
     read_motor_driver_error_status_response = 0x37,
-    motor_driver_in_error_state = 0x38,
     set_brushed_motor_vref_request = 0x40,
     set_brushed_motor_pwm_request = 0x41,
     gripper_grip_request = 0x42,
diff --git a/include/can/core/messages.hpp b/include/can/core/messages.hpp
index 8ac097aeb..0f1346c8f 100644
--- a/include/can/core/messages.hpp
+++ b/include/can/core/messages.hpp
@@ -198,8 +198,6 @@ using MotorPositionRequest = Empty<MessageId::motor_position_request>;
 using UpdateMotorPositionEstimationRequest =
     Empty<MessageId::update_motor_position_estimation_request>;
 
-using MotorDriverInErrorState = Empty<MessageId::motor_driver_in_error_state>;
-
 struct WriteToEEPromRequest : BaseMessage<MessageId::write_eeprom> {
     uint32_t message_index;
     eeprom::types::address address;
@@ -1614,7 +1612,6 @@ using ResponseMessageType = std::variant<
     PeripheralStatusResponse, BrushedMotorConfResponse,
     UpdateMotorPositionEstimationResponse, BaselineSensorResponse,
     PushTipPresenceNotification, GetMotorUsageResponse, GripperJawStateResponse,
-    GripperJawHoldoffResponse, ReadMotorDriverErrorStatusResponse,
-    MotorDriverInErrorState>;
+    GripperJawHoldoffResponse, ReadMotorDriverErrorStatusResponse>;
 
 }  // namespace can::messages
diff --git a/include/motor-control/core/tasks/messages.hpp b/include/motor-control/core/tasks/messages.hpp
index bc55c0d60..1614c6037 100644
--- a/include/motor-control/core/tasks/messages.hpp
+++ b/include/motor-control/core/tasks/messages.hpp
@@ -6,103 +6,9 @@
 
 namespace motor_control_task_messages {
 
-/*enum class MessageId {
-    route_motor_driver_interrupt = 0x0,
-    motor_driver_error_encountered = 0x1,
-    reset_motor_driver_error_handling = 0x2,
-};
-
-template <MessageId MId>
-struct BaseMessage {
-    static const auto id = MId;
-    auto operator==(const BaseMessage& other) const -> bool = default;
-};
-
-template <MessageId MId>
-struct Empty : BaseMessage<MId> {
-    uint32_t message_index;
-    template <bit_utils::ByteIterator Input, typename Limit>
-    static auto parse(Input body, Limit limit) -> Empty {
-        uint32_t msg_ind = 0;
-        body = bit_utils::bytes_to_int(body, limit, msg_ind);
-        return Empty{.message_index = msg_ind};
-    }
-
-    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);
-        return iter - body;
-    }
-
-    auto operator==(const Empty&) const -> bool = default;
-};
-
-using RouteMotorDriverInterrupt =
-Empty<MessageId::route_motor_driver_interrupt>;
-
-using MotorDriverErrorEncountered =
-Empty<MessageId::motor_driver_error_encountered>;
-
-using ResetMotorDriverErrorHandling =
-Empty<MessageId::reset_motor_driver_error_handling>;
-
-struct RouteMotorDriverInterrupt {
-    uint32_t message_index;
-    template <bit_utils::ByteIterator Input, typename Limit>
-    static auto parse(Input body, Limit limit) -> RouteMotorDriverInterrupt {
-        uint32_t msg_ind = 0;
-        body = bit_utils::bytes_to_int(body, limit, msg_ind);
-        return RouteMotorDriverInterrupt{.message_index = msg_ind};
-    }
-
-    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);
-        return iter - body;
-    }
-
-    auto operator==(const RouteMotorDriverInterrupt&) const -> bool = default;
-};
-
-struct MotorDriverErrorEncountered {
-    uint32_t message_index;
-    template <bit_utils::ByteIterator Input, typename Limit>
-    static auto parse(Input body, Limit limit) -> MotorDriverErrorEncountered {
-        uint32_t msg_ind = 0;
-        body = bit_utils::bytes_to_int(body, limit, msg_ind);
-        return MotorDriverErrorEncountered{.message_index = msg_ind};
-    }
-
-    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);
-        return iter - body;
-    }
-
-    auto operator==(const MotorDriverErrorEncountered&) const -> bool = default;
-};
-
-struct ResetMotorDriverErrorHandling {
-    uint32_t message_index;
-    template <bit_utils::ByteIterator Input, typename Limit>
-    static auto parse(Input body, Limit limit) -> ResetMotorDriverErrorHandling
-{ uint32_t msg_ind = 0; body = bit_utils::bytes_to_int(body, limit, msg_ind);
-        return ResetMotorDriverErrorHandling{.message_index = msg_ind};
-    }
-
-    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);
-        return iter - body;
-    }
-
-    auto operator==(const ResetMotorDriverErrorHandling&) const -> bool =
-default;
-};*/
-// needs a member 'id'. This seems to just be becoming a CAN msg!
-
 struct RouteMotorDriverInterrupt {
     uint32_t message_index;
+    uint8_t debounce_count;
 };
 
 using MotionControlTaskMessage = std::variant<
diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp
index 9cc020263..d77f07c10 100644
--- a/include/motor-control/core/tasks/motion_controller_task.hpp
+++ b/include/motor-control/core/tasks/motion_controller_task.hpp
@@ -70,9 +70,7 @@ class MotionControllerMessageHandler {
     void handle(const can::messages::EnableMotorRequest& m) {
         LOG("Received enable motor request");
         if (controller.read_tmc_diag0()) {
-            can_client.send_can_message(can::ids::NodeId::host,
-                                        can::messages::MotorDriverInErrorState{
-                                            .message_index = m.message_index});
+            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,
@@ -116,9 +114,7 @@ class MotionControllerMessageHandler {
             m.velocity, m.acceleration, m.group_id, m.seq_id, m.duration,
             m.request_stop_condition);
         if (controller.read_tmc_diag0()) {
-            can_client.send_can_message(can::ids::NodeId::host,
-                                        can::messages::MotorDriverInErrorState{
-                                            .message_index = m.message_index});
+            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);
         }
@@ -129,9 +125,7 @@ class MotionControllerMessageHandler {
             "groupid=%d, seqid=%d\n",
             m.velocity, m.group_id, m.seq_id);
         if (controller.read_tmc_diag0()) {
-            can_client.send_can_message(can::ids::NodeId::host,
-                                        can::messages::MotorDriverInErrorState{
-                                            .message_index = m.message_index});
+            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);
         }
@@ -171,64 +165,13 @@ 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);
     }
 
-    // no debouncing needed in final product
-    // make messages non-CAN, should be trivial, everything takes std::monostate
-    /*
-    void handle(const motor_control_task_messages::RouteMotorDriverInterrupt& m)
-    { vTaskDelay(pdMS_TO_TICKS(100)); debounce_count++; if (debounce_count > 9)
-    { if (controller.read_tmc_diag0()) {
-                motion_client.send_motion_controller_queue(motor_control_task_messages::MotorDriverErrorEncountered{.message_index
-    = m.message_index}); } else {
-                motion_client.send_motion_controller_queue(motor_control_task_messages::ResetMotorDriverErrorHandling{.message_index
-    = m.message_index});
-            }
-            diag0_debounced = false;
-            debounce_count = 0;
-        } else {
-            motion_client.send_motion_controller_queue(motor_control_task_messages::RouteMotorDriverInterrupt{.message_index
-    = m.message_index});
-        }
-    }
-
-    void handle(const motor_control_task_messages::MotorDriverErrorEncountered&
-    m) { 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::ReadMotorDriverErrorStatus{.message_index =
-                                                              m.message_index});
-        }
-    }
-
-    void handle(const
-    motor_control_task_messages::ResetMotorDriverErrorHandling& m) {
-        static_cast<void>(m);
-        controller.clear_cancel_request();
-        can_client.send_can_message(
-            can::ids::NodeId::host,
-            can::messages::ErrorMessage{
-                .message_index = m.message_index,
-                .severity = can::ids::ErrorSeverity::none,
-                .error_code =
-                    can::ids::ErrorCode::motor_driver_error_detected}); //
-    delete
-    }
-    */
-
-    void handle(
-        const motor_control_task_messages::RouteMotorDriverInterrupt& m) {
-        if (debounce_count > 9) {
+    void handle(const motor_control_task_messages::RouteMotorDriverInterrupt& m) {
+        if (m.debounce_count > 9) {
             if (controller.read_tmc_diag0()) {
                 controller.stop(
                     can::ids::ErrorSeverity::unrecoverable,
@@ -249,23 +192,22 @@ class MotionControllerMessageHandler {
                 controller.clear_cancel_request();
             }
             diag0_debounced = false;
-            debounce_count = 0;
         } else {
-            debounce_count++;
             vTaskDelay(pdMS_TO_TICKS(100));
-            motion_client.send_motion_controller_queue(
-                motor_control_task_messages::RouteMotorDriverInterrupt{
-                    .message_index = m.message_index});
+            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)};
+    }
+
     MotorControllerType& controller;
     CanClient& can_client;
     UsageClient& usage_client;
     DriverClient& driver_client;
     MotionClient& motion_client;
     std::atomic<bool>& diag0_debounced;
-    std::atomic<uint8_t> debounce_count = 0;
 };
 
 /**
@@ -313,41 +255,11 @@ 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})); diag0_debounced = true;
-        }
-    }
-
-    void run_diag0_interrupt() {
-        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 = 0,
-                        .severity = can::ids::ErrorSeverity::unrecoverable,
-                        .error_code =
-                            can::ids::ErrorCode::motor_driver_error_detected});
-                driver_client->send_motor_driver_queue(
-                    can::messages::ReadMotorDriverErrorStatus{.message_index =
-    0});
-            }
-        } else {
-            controller->clear_cancel_request();
-        }
-    }
-    */
-
     void run_diag0_interrupt() {
         if (!diag0_debounced) {
             static_cast<void>(queue.try_write_isr(
                 motor_control_task_messages::RouteMotorDriverInterrupt{
-                    .message_index = 0}));
+                    .message_index = 0, .debounce_count = 0}));
             diag0_debounced = true;
         }
     }
@@ -355,12 +267,6 @@ class MotionControllerTask {
   private:
     QueueType& queue;
     std::atomic<bool> diag0_debounced = false;
-    // static motion_controller::MotionController<lms::MotorMechanicalConfig>*
-    // controller = nullptr; static can::message_writer_task::TaskClient*
-    // can_client = nullptr; static tmc::tasks::TaskClient* driver_client =
-    // nullptr; static member object for motion controller (a nullable pointer)
-    // Can that much calling of motion controller be done in interrupt??
-    // do what impacts the least
 };
 
 }  // namespace motion_controller_task
diff --git a/include/pipettes/core/tasks/motion_controller_task.hpp b/include/pipettes/core/tasks/motion_controller_task.hpp
index ceade960d..8685bf66f 100644
--- a/include/pipettes/core/tasks/motion_controller_task.hpp
+++ b/include/pipettes/core/tasks/motion_controller_task.hpp
@@ -62,9 +62,7 @@ class MotionControllerMessageHandler {
         // TODO only toggle the enable pin once since all motors share
         // a single enable pin line.
         if (controller.read_tmc_diag0()) {
-            can_client.send_can_message(can::ids::NodeId::host,
-                                        can::messages::MotorDriverInErrorState{
-                                            .message_index = m.message_index});
+            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,
@@ -109,9 +107,7 @@ class MotionControllerMessageHandler {
             "acceleration=%d, groupid=%d, seqid=%d\n",
             m.velocity, m.acceleration, m.group_id, m.seq_id);
         if (controller.read_tmc_diag0()) {
-            can_client.send_can_message(can::ids::NodeId::host,
-                                        can::messages::MotorDriverInErrorState{
-                                            .message_index = m.message_index});
+            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);
         }
diff --git a/pipettes/firmware/main.cpp b/pipettes/firmware/main.cpp
index 2d30ed5ce..5073dbd9d 100644
--- a/pipettes/firmware/main.cpp
+++ b/pipettes/firmware/main.cpp
@@ -147,8 +147,8 @@ extern "C" void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
                     sensors::tip_presence::TipStatusChangeDetected{}));
         }
     } else if (GPIO_Pin == linear_motor_hardware.get_pins().diag0.pin) {
-        if (call_diag0_handler != NULL) {
-            if (*call_diag0_handler != NULL) {
+        if (call_diag0_handler != nullptr) {
+            if (*call_diag0_handler != nullptr) {
                 (*call_diag0_handler)();
             }
         }

From 46aed14ea8126ed30ecb916c60cb1ef2e81cbc6f Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Wed, 14 Feb 2024 14:03:49 -0500
Subject: [PATCH 59/68] finished pipette subsystem implementation for testing

---
 .../core/tasks/tmc_motor_driver_common.hpp    |  1 +
 include/pipettes/core/gear_motor_tasks.hpp    |  6 +-
 include/pipettes/core/interfaces.hpp          |  4 -
 include/pipettes/core/linear_motor_tasks.hpp  |  4 +-
 include/pipettes/core/tasks/messages.hpp      |  8 +-
 .../core/tasks/motion_controller_task.hpp     | 85 +++++++++++++++----
 pipettes/core/gear_motor_tasks.cpp            | 25 +++++-
 pipettes/core/linear_motor_tasks.cpp          |  4 +-
 pipettes/firmware/main.cpp                    | 26 ++++--
 9 files changed, 127 insertions(+), 36 deletions(-)

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 53b0be14b..0b0839348 100644
--- a/include/motor-control/core/tasks/tmc_motor_driver_common.hpp
+++ b/include/motor-control/core/tasks/tmc_motor_driver_common.hpp
@@ -50,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/pipettes/core/gear_motor_tasks.hpp b/include/pipettes/core/gear_motor_tasks.hpp
index c1d693302..36d36ccad 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,9 @@ 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.
diff --git a/include/pipettes/core/interfaces.hpp b/include/pipettes/core/interfaces.hpp
index 18671e880..4a3a9c116 100644
--- a/include/pipettes/core/interfaces.hpp
+++ b/include/pipettes/core/interfaces.hpp
@@ -32,12 +32,8 @@ struct HighThroughputInterruptQueues {
     UpdatePositionQueue left_update_queue;
 };
 
-namespace linear_motor {
-
 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 8316523c0..c4511567a 100644
--- a/include/pipettes/core/linear_motor_tasks.hpp
+++ b/include/pipettes/core/linear_motor_tasks.hpp
@@ -40,7 +40,7 @@ auto 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) -> interfaces::linear_motor::diag0_handler;
+        tail_accessor) -> interfaces::diag0_handler;
 
 // 96/384 linear motor tasks
 auto start_tasks(
@@ -51,7 +51,7 @@ auto 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) -> interfaces::linear_motor::diag0_handler;
+        tail_accessor) -> interfaces::diag0_handler;
 
 void call_run_diag0_interrupt();
 
diff --git a/include/pipettes/core/tasks/messages.hpp b/include/pipettes/core/tasks/messages.hpp
index 074629f01..e93a27252 100644
--- a/include/pipettes/core/tasks/messages.hpp
+++ b/include/pipettes/core/tasks/messages.hpp
@@ -10,13 +10,19 @@ 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,
     can::messages::GearEnableMotorRequest,
     can::messages::GetMotionConstraintsRequest,
     can::messages::SetMotionConstraints, can::messages::StopRequest,
-    can::messages::ReadLimitSwitchRequest, can::messages::GetMotorUsageRequest>;
+    can::messages::ReadLimitSwitchRequest, can::messages::GetMotorUsageRequest,
+    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 8685bf66f..3a7e98ec7 100644
--- a/include/pipettes/core/tasks/motion_controller_task.hpp
+++ b/include/pipettes/core/tasks/motion_controller_task.hpp
@@ -8,6 +8,7 @@
 #include "motor-control/core/linear_motion_system.hpp"
 #include "motor-control/core/stepper_motor/motion_controller.hpp"
 #include "motor-control/core/tasks/usage_storage_task.hpp"
+#include "motor-control/core/tasks/tmc_motor_driver_common.hpp"
 #include "pipettes/core/tasks/messages.hpp"
 
 namespace pipettes {
@@ -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) =
@@ -125,9 +142,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 > 9) {
+            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});
+                }
+            } else {
+                controller.clear_cancel_request();
+            }
+            diag0_debounced = false;
+        } else {
+            vTaskDelay(pdMS_TO_TICKS(100));
+            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)};
+    }
+
     MotorControllerType& controller;
     CanClient& can_client;
     UsageClient& usage_client;
+    DriverClient& driver_client;
+    MotionClient& motion_client;
+    std::atomic<bool>& diag0_debounced;
 };
 
 /**
@@ -151,13 +203,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::GearTaskClient DriverClient, TaskClient MotionClient>
     [[noreturn]] void operator()(
         pipette_motion_controller::PipetteMotionController<MEConfig>*
             controller,
-        CanClient* can_client, UsageClient* usage_client) {
+        CanClient* can_client, UsageClient* usage_client,
+        DriverClient* driver_client, MotionClient* motion_client) {
         auto handler = MotionControllerMessageHandler{*controller, *can_client,
-                                                      *usage_client};
+                                                      *usage_client, *driver_client, *motion_client, diag0_debounced};
         TaskMessage message{};
         for (;;) {
             if (queue.try_read(&message, queue.max_delay)) {
@@ -168,17 +222,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(
+                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/pipettes/core/gear_motor_tasks.cpp b/pipettes/core/gear_motor_tasks.cpp
index 5355bf2d3..6527782cd 100644
--- a/pipettes/core/gear_motor_tasks.cpp
+++ b/pipettes/core/gear_motor_tasks.cpp
@@ -37,7 +37,22 @@ 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 +60,7 @@ 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);
 
@@ -57,6 +72,7 @@ void gear_motor_tasks::start_tasks(
     // Left Gear Motor Tasks
     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,
@@ -87,7 +103,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 +132,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()
diff --git a/pipettes/core/linear_motor_tasks.cpp b/pipettes/core/linear_motor_tasks.cpp
index 334e3b550..86bd526e3 100644
--- a/pipettes/core/linear_motor_tasks.cpp
+++ b/pipettes/core/linear_motor_tasks.cpp
@@ -39,7 +39,7 @@ auto 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) -> interfaces::linear_motor::diag0_handler {
+        tail_accessor) -> interfaces::diag0_handler {
     tmc2130_queue_client.set_node_id(id);
     motion_queue_client.set_node_id(id);
 
@@ -93,7 +93,7 @@ auto 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) -> interfaces::linear_motor::diag0_handler {
+        tail_accessor) -> interfaces::diag0_handler {
     tmc2160_queue_client.set_node_id(id);
     motion_queue_client.set_node_id(id);
 
diff --git a/pipettes/firmware/main.cpp b/pipettes/firmware/main.cpp
index 5073dbd9d..30e7fbf96 100644
--- a/pipettes/firmware/main.cpp
+++ b/pipettes/firmware/main.cpp
@@ -45,7 +45,9 @@ constexpr auto PIPETTE_TYPE = get_pipette_type();
 
 static auto iWatchdog = iwdg::IndependentWatchDog{};
 
-static interfaces::linear_motor::diag0_handler call_diag0_handler = nullptr;
+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));
@@ -147,9 +149,19 @@ extern "C" void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
                     sensors::tip_presence::TipStatusChangeDetected{}));
         }
     } else if (GPIO_Pin == linear_motor_hardware.get_pins().diag0.pin) {
-        if (call_diag0_handler != nullptr) {
-            if (*call_diag0_handler != nullptr) {
-                (*call_diag0_handler)();
+        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,11 +208,11 @@ auto initialize_motor_tasks(
     initialize_linear_timer(plunger_callback);
     initialize_gear_timer(gear_callback_wrapper);
     initialize_enc_timer(encoder_callback);
-    call_diag0_handler = 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(
+    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);
 }
@@ -231,7 +243,7 @@ auto initialize_motor_tasks(
 
     initialize_linear_timer(plunger_callback);
     initialize_enc_timer(encoder_callback);
-    call_diag0_handler = 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);

From 12c37d16e48e8b6c69e246466a931dbe9881e974 Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Thu, 15 Feb 2024 13:20:46 -0500
Subject: [PATCH 60/68] updated overtemp settings for gantry testing

---
 gripper/firmware/interfaces_z_motor.cpp    | 1 -
 head/firmware/main_proto.cpp               | 4 ++--
 head/firmware/main_rev1.cpp                | 6 ++----
 pipettes/firmware/motor_configurations.cpp | 8 ++++++--
 4 files changed, 10 insertions(+), 9 deletions(-)

diff --git a/gripper/firmware/interfaces_z_motor.cpp b/gripper/firmware/interfaces_z_motor.cpp
index 66fffe068..cb6d1c3c0 100644
--- a/gripper/firmware/interfaces_z_motor.cpp
+++ b/gripper/firmware/interfaces_z_motor.cpp
@@ -126,7 +126,6 @@ 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
diff --git a/head/firmware/main_proto.cpp b/head/firmware/main_proto.cpp
index 2dce99539..614af1142 100644
--- a/head/firmware/main_proto.cpp
+++ b/head/firmware/main_proto.cpp
@@ -213,7 +213,7 @@ struct motor_hardware::HardwareConfig pin_configurations_right {
 static tmc2130::configs::TMC2130DriverConfig motor_driver_configs_right{
     .registers =
         {
-            .gconfig = {.en_pwm_mode = 1, .diag0_error = 1, .diag0_otpw = 1},
+            .gconfig = {.en_pwm_mode = 1, .diag0_error = 1},
             .ihold_irun = {.hold_current = 0xB,
                            .run_current = 0x19,
                            .hold_current_delay = 0x7},
@@ -240,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, .diag0_error = 1, .diag0_otpw = 1},
+            .gconfig = {.en_pwm_mode = 1, .diag0_error = 1},
             .ihold_irun = {.hold_current = 0xB,
                            .run_current = 0x19,
                            .hold_current_delay = 0x7},
diff --git a/head/firmware/main_rev1.cpp b/head/firmware/main_rev1.cpp
index 801298f00..a306d1460 100644
--- a/head/firmware/main_rev1.cpp
+++ b/head/firmware/main_rev1.cpp
@@ -223,8 +223,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,
-                              .diag0_error = 1,
-                              .diag0_otpw = 1},
+                              .diag0_error = 1},
                   .ihold_irun = {.hold_current = 16,
                                  .run_current = 31,
                                  .hold_current_delay = 0x7},
@@ -251,8 +250,7 @@ static tmc2160::configs::TMC2160DriverConfig motor_driver_configs_right{
 
 static tmc2160::configs::TMC2160DriverConfig motor_driver_configs_left{
     .registers = {.gconfig = {.en_pwm_mode = 0,
-                              .diag0_error = 1,
-                              .diag0_otpw = 1},
+                              .diag0_error = 1},
                   .ihold_irun = {.hold_current = 16,
                                  .run_current = 31,
                                  .hold_current_delay = 0x7},
diff --git a/pipettes/firmware/motor_configurations.cpp b/pipettes/firmware/motor_configurations.cpp
index c6f85e37b..5a1741afd 100644
--- a/pipettes/firmware/motor_configurations.cpp
+++ b/pipettes/firmware/motor_configurations.cpp
@@ -11,7 +11,8 @@ 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 +25,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 +39,8 @@ 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 +53,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 =
                     {

From 629d53117cc4d168525cf7ef9e61c20eac41d308 Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Thu, 15 Feb 2024 15:01:25 -0500
Subject: [PATCH 61/68] finalized driver configs

---
 gantry/firmware/interfaces_rev1.cpp | 6 ++----
 1 file changed, 2 insertions(+), 4 deletions(-)

diff --git a/gantry/firmware/interfaces_rev1.cpp b/gantry/firmware/interfaces_rev1.cpp
index 687ca2d75..92aec2ea9 100644
--- a/gantry/firmware/interfaces_rev1.cpp
+++ b/gantry/firmware/interfaces_rev1.cpp
@@ -146,8 +146,7 @@ struct motion_controller::HardwareConfig motor_pins_y {
 
 static tmc2160::configs::TMC2160DriverConfig motor_driver_config_x{
     .registers = {.gconfig = {.en_pwm_mode = 0,
-                              .diag0_error = 1,
-                              .diag0_otpw = 1},
+                              .diag0_error = 1},
                   .ihold_irun = {.hold_current = 16,
                                  .run_current = 31,
                                  .hold_current_delay = 0x7},
@@ -188,8 +187,7 @@ 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,
-                              .diag0_otpw = 1},
+                              .diag0_error = 1},
                   .ihold_irun = {.hold_current = 16,
                                  .run_current = 31,
                                  .hold_current_delay = 0x7},

From 36af4ba1cfcd8a92abaa553bd30893f2cfe65087 Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Thu, 15 Feb 2024 18:03:40 -0500
Subject: [PATCH 62/68] formatted

---
 gantry/firmware/interfaces_rev1.cpp           |  6 +--
 head/firmware/main_rev1.cpp                   |  6 +--
 head/firmware/stm32g4xx_it.c                  |  1 -
 include/can/core/message_handlers/motion.hpp  |  7 ---
 include/common/core/message_utils.hpp         | 21 ---------
 .../stepper_motor/motor_interrupt_handler.hpp |  4 +-
 .../core/tasks/motion_controller_task.hpp     | 38 +++++++++++++---
 .../tests/mock_motor_driver_client.hpp        |  2 +-
 include/pipettes/core/gear_motor_tasks.hpp    |  3 +-
 .../core/tasks/motion_controller_task.hpp     | 44 ++++++++++++++-----
 include/spi/core/utils.hpp                    |  2 +-
 pipettes/core/gear_motor_tasks.cpp            | 11 +++--
 pipettes/firmware/main.cpp                    |  8 ++--
 pipettes/firmware/motor_configurations.cpp    |  6 +--
 14 files changed, 85 insertions(+), 74 deletions(-)

diff --git a/gantry/firmware/interfaces_rev1.cpp b/gantry/firmware/interfaces_rev1.cpp
index 92aec2ea9..d34b82c10 100644
--- a/gantry/firmware/interfaces_rev1.cpp
+++ b/gantry/firmware/interfaces_rev1.cpp
@@ -145,8 +145,7 @@ 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},
                   .ihold_irun = {.hold_current = 16,
                                  .run_current = 31,
                                  .hold_current_delay = 0x7},
@@ -186,8 +185,7 @@ 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},
                   .ihold_irun = {.hold_current = 16,
                                  .run_current = 31,
                                  .hold_current_delay = 0x7},
diff --git a/head/firmware/main_rev1.cpp b/head/firmware/main_rev1.cpp
index a306d1460..e6d17776f 100644
--- a/head/firmware/main_rev1.cpp
+++ b/head/firmware/main_rev1.cpp
@@ -222,8 +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,
-                              .diag0_error = 1},
+    .registers = {.gconfig = {.en_pwm_mode = 0, .diag0_error = 1},
                   .ihold_irun = {.hold_current = 16,
                                  .run_current = 31,
                                  .hold_current_delay = 0x7},
@@ -249,8 +248,7 @@ static tmc2160::configs::TMC2160DriverConfig motor_driver_configs_right{
     }};
 
 static tmc2160::configs::TMC2160DriverConfig motor_driver_configs_left{
-    .registers = {.gconfig = {.en_pwm_mode = 0,
-                              .diag0_error = 1},
+    .registers = {.gconfig = {.en_pwm_mode = 0, .diag0_error = 1},
                   .ihold_irun = {.hold_current = 16,
                                  .run_current = 31,
                                  .hold_current_delay = 0x7},
diff --git a/head/firmware/stm32g4xx_it.c b/head/firmware/stm32g4xx_it.c
index 3a9591e0e..929586b89 100644
--- a/head/firmware/stm32g4xx_it.c
+++ b/head/firmware/stm32g4xx_it.c
@@ -178,7 +178,6 @@ 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!
     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)) {
diff --git a/include/can/core/message_handlers/motion.hpp b/include/can/core/message_handlers/motion.hpp
index 64400598f..203407ab7 100644
--- a/include/can/core/message_handlers/motion.hpp
+++ b/include/can/core/message_handlers/motion.hpp
@@ -35,13 +35,6 @@ class MotionHandler {
   private:
     void handle_message(std::monostate &m) { static_cast<void>(m); }
 
-    /*
-    void handle_message(const auto &m) {
-        motion_client.send_motion_controller_queue(utils::variant_cast(m)); //
-    try to compile, talk with Seth
-    }
-    */
-
     void handle_message(const auto &m) {
         motion_client.send_motion_controller_queue(m);
     }
diff --git a/include/common/core/message_utils.hpp b/include/common/core/message_utils.hpp
index d5ea890a7..e1fcee583 100644
--- a/include/common/core/message_utils.hpp
+++ b/include/common/core/message_utils.hpp
@@ -73,25 +73,4 @@ struct VariantCat<std::variant<VariantAContents...>,
     using type = std::variant<VariantAContents..., VariantBContents...>;
 };
 
-/*
-template <class... Args>
-struct variant_cast_proxy
-{
-    std::variant<Args...> v;
-
-    template <class... ToArgs>
-    operator std::variant<ToArgs...>() const
-    {
-        return std::visit([](auto&& arg) -> std::variant<ToArgs...> { return arg
-; }, v);
-    }
-};
-
-template <class... Args>
-auto variant_cast(const std::variant<Args...>& v) -> variant_cast_proxy<Args...>
-{
-    return {v};
-}
-*/
-
 };  // namespace utils
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 3715aa491..a6cb34bb4 100644
--- a/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp
+++ b/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp
@@ -208,9 +208,7 @@ class MotorInterruptHandler {
             cancel_and_clear_moves(can::ids::ErrorCode::estop_detected);
             in_estop = true;
         } else if (static_cast<can::ids::ErrorCode>(cancel_request.code) !=
-                   can::ids::ErrorCode::ok) {  // is this correct? Should be
-                                               // zero-initialized. Check,
-                                               // confirm!
+                   can::ids::ErrorCode::ok) {
             cancel_and_clear_moves(
                 static_cast<can::ids::ErrorCode>(cancel_request.code),
                 static_cast<can::ids::ErrorSeverity>(cancel_request.severity));
diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp
index d77f07c10..e5bbe7f61 100644
--- a/include/motor-control/core/tasks/motion_controller_task.hpp
+++ b/include/motor-control/core/tasks/motion_controller_task.hpp
@@ -70,7 +70,13 @@ class MotionControllerMessageHandler {
     void handle(const can::messages::EnableMotorRequest& m) {
         LOG("Received enable motor request");
         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});
+            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,
@@ -114,7 +120,13 @@ class MotionControllerMessageHandler {
             m.velocity, m.acceleration, m.group_id, m.seq_id, m.duration,
             m.request_stop_condition);
         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});
+            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);
         }
@@ -125,7 +137,13 @@ class MotionControllerMessageHandler {
             "groupid=%d, seqid=%d\n",
             m.velocity, m.group_id, m.seq_id);
         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});
+            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);
         }
@@ -170,7 +188,8 @@ class MotionControllerMessageHandler {
         controller.send_usage_data(m.message_index, usage_client);
     }
 
-    void handle(const motor_control_task_messages::RouteMotorDriverInterrupt& m) {
+    void handle(
+        const motor_control_task_messages::RouteMotorDriverInterrupt& m) {
         if (m.debounce_count > 9) {
             if (controller.read_tmc_diag0()) {
                 controller.stop(
@@ -194,12 +213,17 @@ class MotionControllerMessageHandler {
             diag0_debounced = false;
         } else {
             vTaskDelay(pdMS_TO_TICKS(100));
-            motion_client.send_motion_controller_queue(increment_message_debounce_count(m));
+            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)};
+    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)};
     }
 
     MotorControllerType& controller;
diff --git a/include/motor-control/tests/mock_motor_driver_client.hpp b/include/motor-control/tests/mock_motor_driver_client.hpp
index d9651bb69..b4fb715ca 100644
--- a/include/motor-control/tests/mock_motor_driver_client.hpp
+++ b/include/motor-control/tests/mock_motor_driver_client.hpp
@@ -18,4 +18,4 @@ struct MockMotorDriverClient {
     std::vector<tmc::tasks::TaskMessage> messages{};
 };
 
-}  // namespace test_mocks
\ No newline at end of file
+}  // namespace test_mocks
diff --git a/include/pipettes/core/gear_motor_tasks.hpp b/include/pipettes/core/gear_motor_tasks.hpp
index 36d36ccad..6f4edc169 100644
--- a/include/pipettes/core/gear_motor_tasks.hpp
+++ b/include/pipettes/core/gear_motor_tasks.hpp
@@ -39,7 +39,8 @@ auto start_tasks(
     can::ids::NodeId id,
     interfaces::gear_motor::GearMotorHardwareTasks& gmh_tsks,
     eeprom::dev_data::DevDataTailAccessor<sensor_tasks::QueueClient>&
-        tail_accessor) -> std::tuple<interfaces::diag0_handler, interfaces::diag0_handler>;
+        tail_accessor)
+    -> std::tuple<interfaces::diag0_handler, interfaces::diag0_handler>;
 
 void call_run_diag0_interrupt();
 
diff --git a/include/pipettes/core/tasks/motion_controller_task.hpp b/include/pipettes/core/tasks/motion_controller_task.hpp
index 3a7e98ec7..081d8d5e8 100644
--- a/include/pipettes/core/tasks/motion_controller_task.hpp
+++ b/include/pipettes/core/tasks/motion_controller_task.hpp
@@ -7,8 +7,8 @@
 #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/usage_storage_task.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"
 
 namespace pipettes {
@@ -79,7 +79,13 @@ class MotionControllerMessageHandler {
         // TODO only toggle the enable pin once since all motors share
         // a single enable pin line.
         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});
+            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,
@@ -124,7 +130,13 @@ class MotionControllerMessageHandler {
             "acceleration=%d, groupid=%d, seqid=%d\n",
             m.velocity, m.acceleration, m.group_id, m.seq_id);
         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});
+            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);
         }
@@ -142,7 +154,8 @@ class MotionControllerMessageHandler {
         controller.send_usage_data(m.message_index, usage_client);
     }
 
-    void handle(const pipettes::task_messages::motor_control_task_messages::RouteMotorDriverInterrupt& m) {
+    void handle(const pipettes::task_messages::motor_control_task_messages::
+                    RouteMotorDriverInterrupt& m) {
         if (m.debounce_count > 9) {
             if (controller.read_tmc_diag0()) {
                 controller.stop(
@@ -166,12 +179,19 @@ class MotionControllerMessageHandler {
             diag0_debounced = false;
         } else {
             vTaskDelay(pdMS_TO_TICKS(100));
-            motion_client.send_motion_controller_queue(increment_message_debounce_count(m));
+            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)};
+    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)};
     }
 
     MotorControllerType& controller;
@@ -210,8 +230,9 @@ class MotionControllerTask {
             controller,
         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};
+        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)) {
@@ -225,8 +246,9 @@ class MotionControllerTask {
     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}));
+                pipettes::task_messages::motor_control_task_messages::
+                    RouteMotorDriverInterrupt{.message_index = 0,
+                                              .debounce_count = 0}));
             diag0_debounced = true;
         }
     }
diff --git a/include/spi/core/utils.hpp b/include/spi/core/utils.hpp
index 360d1acab..0f28e89fa 100644
--- a/include/spi/core/utils.hpp
+++ b/include/spi/core/utils.hpp
@@ -51,4 +51,4 @@ template <typename RegType>
 }
 }  // namespace utils
 
-}  // namespace spi
\ No newline at end of file
+}  // namespace spi
diff --git a/pipettes/core/gear_motor_tasks.cpp b/pipettes/core/gear_motor_tasks.cpp
index 6527782cd..43e4d5585 100644
--- a/pipettes/core/gear_motor_tasks.cpp
+++ b/pipettes/core/gear_motor_tasks.cpp
@@ -37,7 +37,6 @@ 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 call_run_diag0_left_interrupt() {
     if (gear_motor_tasks::get_left_gear_tasks().motion_controller) {
         return gear_motor_tasks::get_left_gear_tasks()
@@ -60,7 +59,8 @@ auto gear_motor_tasks::start_tasks(
     can::ids::NodeId id,
     interfaces::gear_motor::GearMotorHardwareTasks& gmh_tsks,
     eeprom::dev_data::DevDataTailAccessor<sensor_tasks::QueueClient>&
-        tail_accessor) -> std::tuple<interfaces::diag0_handler, interfaces::diag0_handler> {
+        tail_accessor)
+    -> std::tuple<interfaces::diag0_handler, interfaces::diag0_handler> {
     left_queue_client.set_node_id(id);
     right_queue_client.set_node_id(id);
 
@@ -70,10 +70,9 @@ auto 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,
-                                                   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);
diff --git a/pipettes/firmware/main.cpp b/pipettes/firmware/main.cpp
index 30e7fbf96..0e2561c9b 100644
--- a/pipettes/firmware/main.cpp
+++ b/pipettes/firmware/main.cpp
@@ -212,9 +212,11 @@ auto initialize_motor_tasks(
         *central_tasks::get_tasks().can_writer, linear_motion_control,
         peripheral_tasks::get_spi_client(), conf.linear_motor, id, lmh_tsk,
         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);
+    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,
diff --git a/pipettes/firmware/motor_configurations.cpp b/pipettes/firmware/motor_configurations.cpp
index 5a1741afd..2a43d0f1f 100644
--- a/pipettes/firmware/motor_configurations.cpp
+++ b/pipettes/firmware/motor_configurations.cpp
@@ -11,8 +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,
-                                          .diag0_error = 1},
+                .registers = {.gconfig = {.en_pwm_mode = 1, .diag0_error = 1},
                               .ihold_irun = {.hold_current = 16,
                                              .run_current = 31,
                                              .hold_current_delay = 0x7},
@@ -39,8 +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,
-                                          .diag0_error = 1},
+                .registers = {.gconfig = {.en_pwm_mode = 1, .diag0_error = 1},
                               .ihold_irun = {.hold_current = 16,
                                              .run_current = 31,
                                              .hold_current_delay = 0x7},

From 986661d3301a0956e29dd42f12be1237d10490cb Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Tue, 20 Feb 2024 20:42:29 -0500
Subject: [PATCH 63/68] PR review comment fixes

---
 .../tasks/gear_tmc2160_motor_driver_task.hpp  | 24 ++++++---
 .../core/tasks/motion_controller_task.hpp     |  9 ++--
 .../core/tasks/motion_controller_task.hpp     |  2 -
 pipettes/firmware/hardware_config.c           | 53 +++++++++----------
 pipettes/firmware/hardware_config.h           |  3 +-
 pipettes/firmware/motor_hardware.c            | 12 ++---
 6 files changed, 57 insertions(+), 46 deletions(-)

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 517f1e577..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);
+            }
         }
     }
 
diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp
index e5bbe7f61..cc53aba6c 100644
--- a/include/motor-control/core/tasks/motion_controller_task.hpp
+++ b/include/motor-control/core/tasks/motion_controller_task.hpp
@@ -10,6 +10,9 @@
 #include "motor-control/core/tasks/messages.hpp"
 #include "motor-control/core/tasks/tmc_motor_driver_common.hpp"
 
+constexpr uint32_t DIAG0_DEBOUNCE_REPS = 9;
+constexpr uint32_t DIAG0_DEBOUNCE_DELAY = 100;
+
 namespace motion_controller_task {
 
 using TaskMessage = motor_control_task_messages::MotionControlTaskMessage;
@@ -190,7 +193,7 @@ class MotionControllerMessageHandler {
 
     void handle(
         const motor_control_task_messages::RouteMotorDriverInterrupt& m) {
-        if (m.debounce_count > 9) {
+        if (m.debounce_count > DIAG0_DEBOUNCE_REPS) {
             if (controller.read_tmc_diag0()) {
                 controller.stop(
                     can::ids::ErrorSeverity::unrecoverable,
@@ -207,12 +210,10 @@ class MotionControllerMessageHandler {
                         can::messages::ReadMotorDriverErrorStatusRequest{
                             .message_index = m.message_index});
                 }
-            } else {
-                controller.clear_cancel_request();
             }
             diag0_debounced = false;
         } else {
-            vTaskDelay(pdMS_TO_TICKS(100));
+            vTaskDelay(pdMS_TO_TICKS(DIAG0_DEBOUNCE_DELAY));
             motion_client.send_motion_controller_queue(
                 increment_message_debounce_count(m));
         }
diff --git a/include/pipettes/core/tasks/motion_controller_task.hpp b/include/pipettes/core/tasks/motion_controller_task.hpp
index 081d8d5e8..5022263f6 100644
--- a/include/pipettes/core/tasks/motion_controller_task.hpp
+++ b/include/pipettes/core/tasks/motion_controller_task.hpp
@@ -173,8 +173,6 @@ class MotionControllerMessageHandler {
                         can::messages::ReadMotorDriverErrorStatusRequest{
                             .message_index = m.message_index});
                 }
-            } else {
-                controller.clear_cancel_request();
             }
             diag0_debounced = false;
         } else {
diff --git a/pipettes/firmware/hardware_config.c b/pipettes/firmware/hardware_config.c
index 22b790d2d..b4ada4b98 100644
--- a/pipettes/firmware/hardware_config.c
+++ b/pipettes/firmware/hardware_config.c
@@ -146,7 +146,7 @@ static uint16_t get_spi_pins_lt(GPIO_TypeDef* for_handle) {
     }
 }
 
-static uint16_t get_motor_driver_pins_ht(GPIO_TypeDef* for_handle, bool for_diag0) {
+static uint16_t get_motor_driver_pins_ht(GPIO_TypeDef* for_handle) {
     /*
      * Dir Pins
      * plunger -> PA6
@@ -162,23 +162,16 @@ static uint16_t get_motor_driver_pins_ht(GPIO_TypeDef* for_handle, bool for_diag
      * Motor Enable
      * pickup enable -> PA10
      * plunger enable -> PB4
-     * 
-     * Diag0 Pin
-     * PB6
      */
-    if (!for_diag0) {
-        switch((uint32_t)for_handle) {
-            case (uint32_t)GPIOA: return GPIO_PIN_6 | GPIO_PIN_7 | GPIO_PIN_10;
-            case (uint32_t)GPIOB: return GPIO_PIN_0 | GPIO_PIN_1 | GPIO_PIN_7 | GPIO_PIN_4;
-            case (uint32_t)GPIOC: return GPIO_PIN_2;
-            default: return 0;
-        }
-    } else {
-        return GPIO_PIN_6;
+    switch((uint32_t)for_handle) {
+        case (uint32_t)GPIOA: return GPIO_PIN_6 | GPIO_PIN_7 | GPIO_PIN_10;
+        case (uint32_t)GPIOB: return GPIO_PIN_0 | GPIO_PIN_1 | GPIO_PIN_7 | GPIO_PIN_4;
+        case (uint32_t)GPIOC: return GPIO_PIN_2;
+        default: return 0;
     }
 }
 
-static uint16_t get_motor_driver_pins_lt(GPIO_TypeDef* for_handle, bool for_diag0) {
+static uint16_t get_motor_driver_pins_lt(GPIO_TypeDef* for_handle) {
     /*
      * Dir Pin -> PC6
      *
@@ -186,17 +179,11 @@ static uint16_t get_motor_driver_pins_lt(GPIO_TypeDef* for_handle, bool for_diag
      * Enable Pin -> PA10
      *
      * VREF (TMC2130) -> PA5
-     * 
-     * DIAG0 Pin -> PC11
      */
-    if (!for_diag0) {
-        switch((uint32_t)for_handle) {
-            case (uint32_t)GPIOA: return GPIO_PIN_10;
-            case (uint32_t)GPIOC: return GPIO_PIN_6 | GPIO_PIN_7;
-            default: return 0;
-        }
-    } else {
-        return GPIO_PIN_11;
+    switch((uint32_t)for_handle) {
+        case (uint32_t)GPIOA: return GPIO_PIN_10;
+        case (uint32_t)GPIOC: return GPIO_PIN_6 | GPIO_PIN_7;
+        default: return 0;
     }
 }
 
@@ -226,14 +213,26 @@ uint16_t pipette_hardware_spi_pins(const PipetteType pipette_type, GPIO_TypeDef*
     }
 }
 
-uint16_t pipette_hardware_motor_driver_pins(const PipetteType pipette_type, GPIO_TypeDef* for_handle, bool for_diag0) {
+uint16_t pipette_hardware_motor_driver_pins(const PipetteType pipette_type, GPIO_TypeDef* for_handle) {
+    switch (pipette_type) {
+        case NINETY_SIX_CHANNEL:
+        case THREE_EIGHTY_FOUR_CHANNEL:
+            return get_motor_driver_pins_ht(for_handle);
+        case SINGLE_CHANNEL:
+        case EIGHT_CHANNEL:
+        default:
+            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 get_motor_driver_pins_ht(for_handle, for_diag0);
+            return GPIO_PIN_6;
         case SINGLE_CHANNEL:
         case EIGHT_CHANNEL:
         default:
-            return get_motor_driver_pins_lt(for_handle, for_diag0);
+            return GPIO_PIN_11;
     }
 }
diff --git a/pipettes/firmware/hardware_config.h b/pipettes/firmware/hardware_config.h
index 4a0ec859c..730cd48e2 100644
--- a/pipettes/firmware/hardware_config.h
+++ b/pipettes/firmware/hardware_config.h
@@ -25,6 +25,7 @@ typedef enum {
 }GPIOInterruptBlock;
 
 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, bool for_diag0);
+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/motor_hardware.c b/pipettes/firmware/motor_hardware.c
index 9a3815b2e..867a7d4fd 100644
--- a/pipettes/firmware/motor_hardware.c
+++ b/pipettes/firmware/motor_hardware.c
@@ -83,7 +83,7 @@ void motor_driver_gpio_init() {
     GPIO_InitTypeDef GPIO_InitStruct = {0};
     PipetteType pipette_type = get_pipette_type();
 
-    GPIO_InitStruct.Pin = pipette_hardware_motor_driver_pins(pipette_type, GPIOC, false);
+    GPIO_InitStruct.Pin = pipette_hardware_motor_driver_pins(pipette_type, GPIOC);
     GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
     GPIO_InitStruct.Pull = GPIO_NOPULL;
     GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
@@ -92,21 +92,21 @@ void motor_driver_gpio_init() {
     if (pipette_type != NINETY_SIX_CHANNEL) {
         // Driver Clock Pin.
         // Enable Dir/Step pin
-        GPIO_InitStruct.Pin = pipette_hardware_motor_driver_pins(pipette_type, GPIOA, false);
+        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_pins(pipette_type, GPIOC, true);
+        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, false);
+        GPIO_InitStruct.Pin = pipette_hardware_motor_driver_pins(pipette_type, GPIOA);
         HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
         // Enable/Dir/Step pin
-        GPIO_InitStruct.Pin = pipette_hardware_motor_driver_pins(pipette_type, GPIOB, false);
+        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_pins(pipette_type, GPIOB, true);
+        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);
     }

From 4aec9d300549d9e76deab465930df98d62df82a7 Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Tue, 20 Feb 2024 21:03:29 -0500
Subject: [PATCH 64/68] removed comments

---
 include/motor-control/core/stepper_motor/tmc2160_driver.hpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/include/motor-control/core/stepper_motor/tmc2160_driver.hpp b/include/motor-control/core/stepper_motor/tmc2160_driver.hpp
index 5fde77f07..4c4763a24 100644
--- a/include/motor-control/core/stepper_motor/tmc2160_driver.hpp
+++ b/include/motor-control/core/stepper_motor/tmc2160_driver.hpp
@@ -315,9 +315,9 @@ class TMC2160 {
      * be read, so this function gets it from the actual device.
      */
     auto update_glob_scaler(uint32_t data) -> void {
-        auto ret = read_register<GConfig>(data);  // should be GlobalScalar
+        auto ret = read_register<GConfig>(data);
         if (ret.has_value()) {
-            _registers.gconfig = ret.value();  // should be glob_scale
+            _registers.gconfig = ret.value();
         }
     }
 

From 2b75f6f6d3e635c804bca9dbbdffcd9ac56069dc Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Mon, 25 Mar 2024 13:01:06 -0400
Subject: [PATCH 65/68] update pipette motor_interrupt_handler

---
 include/pipettes/core/motor_interrupt_handler.hpp | 9 +++++----
 1 file changed, 5 insertions(+), 4 deletions(-)

diff --git a/include/pipettes/core/motor_interrupt_handler.hpp b/include/pipettes/core/motor_interrupt_handler.hpp
index 94e54ef07..fbfb35a28 100644
--- a/include/pipettes/core/motor_interrupt_handler.hpp
+++ b/include/pipettes/core/motor_interrupt_handler.hpp
@@ -5,14 +5,14 @@
 
 namespace pipettes {
 using namespace motor_messages;
-template <template <class> class QueueImpl, class StatusClient,
+template <template <class> class QueueImpl, class StatusClient, class DriverClient,
           typename MotorMoveMessage, typename MotorHardware, class SensorClient>
 requires MessageQueue<QueueImpl<MotorMoveMessage>, MotorMoveMessage> &&
     std::is_base_of_v<motor_hardware::MotorHardwareIface, MotorHardware>
 class PipetteMotorInterruptHandler
     : public motor_handler::MotorInterruptHandler<
           freertos_message_queue::FreeRTOSMessageQueue, StatusClient,
-          MotorMoveMessage, MotorHardware> {
+          DriverClient, MotorMoveMessage, MotorHardware> {
   public:
     using MoveQueue = QueueImpl<MotorMoveMessage>;
     using UpdatePositionQueue =
@@ -20,13 +20,14 @@ class PipetteMotorInterruptHandler
     PipetteMotorInterruptHandler() = delete;
     PipetteMotorInterruptHandler(
         MoveQueue& incoming_move_queue, StatusClient& outgoing_queue,
+        DriverClient& driver_queue,
         MotorHardware& hardware_iface, stall_check::StallCheck& stall,
         UpdatePositionQueue& incoming_update_position_queue,
         SensorClient& sensor_queue_client)
         : motor_handler::MotorInterruptHandler<
               freertos_message_queue::FreeRTOSMessageQueue, StatusClient,
-              MotorMoveMessage, MotorHardware>(
-              incoming_move_queue, outgoing_queue, hardware_iface, stall,
+              DriverClient, MotorMoveMessage, MotorHardware>(
+              incoming_move_queue, outgoing_queue, driver_queue, hardware_iface, stall,
               incoming_update_position_queue),
           sensor_client(sensor_queue_client) {}
 

From 960085824fda9eabbb7a08f6183a1404683c1629 Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Mon, 25 Mar 2024 13:16:55 -0400
Subject: [PATCH 66/68] fixed cross-compile issue

---
 include/pipettes/firmware/interfaces_g4.hpp | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/include/pipettes/firmware/interfaces_g4.hpp b/include/pipettes/firmware/interfaces_g4.hpp
index a451d356b..7d4ab2346 100644
--- a/include/pipettes/firmware/interfaces_g4.hpp
+++ b/include/pipettes/firmware/interfaces_g4.hpp
@@ -29,21 +29,21 @@
 namespace interfaces {
 
 #ifdef USE_PRESSURE_MOVE
-template <typename Client, DriverClient>
+template <typename Client, typename DriverClient>
 using MotorInterruptHandlerType = motor_handler::MotorInterruptHandler<
     freertos_message_queue::FreeRTOSMessageQueue, Client, DriverClient,
     motor_messages::SensorSyncMove, motor_hardware::MotorHardware>;
-template <typename Client, DriverClient>
+template <typename Client, typename DriverClient>
 using PipetteMotorInterruptHandlerType = pipettes::PipetteMotorInterruptHandler<
     freertos_message_queue::FreeRTOSMessageQueue, Client, DriverClient,
     motor_messages::SensorSyncMove, motor_hardware::MotorHardware,
     sensor_tasks::QueueClient>;
 #else
-template <typename Client, DriverClient>
+template <typename Client, typename DriverClient>
 using MotorInterruptHandlerType = motor_handler::MotorInterruptHandler<
     freertos_message_queue::FreeRTOSMessageQueue, Client, DriverClient,
     motor_messages::Move, motor_hardware::MotorHardware>;
-template <typename Client, DriverClient>
+template <typename Client, typename DriverClient>
 using PipetteMotorInterruptHandlerType = pipettes::PipetteMotorInterruptHandler<
     freertos_message_queue::FreeRTOSMessageQueue, Client, DriverClient,
     motor_messages::Move, motor_hardware::MotorHardware,

From c8ac8dc0511300c21e1150c049c786bf7d7bb80c Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Mon, 25 Mar 2024 13:20:04 -0400
Subject: [PATCH 67/68] formatted

---
 .../core/tasks/motion_controller_task.hpp          |  2 +-
 include/pipettes/core/motor_interrupt_handler.hpp  | 13 +++++++------
 .../pipettes/core/tasks/motion_controller_task.hpp |  2 +-
 include/pipettes/firmware/interfaces_g4.hpp        |  8 ++++++--
 pipettes/firmware/interfaces.cpp                   | 14 ++++++++++----
 pipettes/simulator/interfaces.cpp                  |  6 ++++--
 6 files changed, 29 insertions(+), 16 deletions(-)

diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp
index fd05589d4..239377ef5 100644
--- a/include/motor-control/core/tasks/motion_controller_task.hpp
+++ b/include/motor-control/core/tasks/motion_controller_task.hpp
@@ -236,7 +236,7 @@ class MotionControllerMessageHandler {
             .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,
diff --git a/include/pipettes/core/motor_interrupt_handler.hpp b/include/pipettes/core/motor_interrupt_handler.hpp
index fbfb35a28..f625f1a13 100644
--- a/include/pipettes/core/motor_interrupt_handler.hpp
+++ b/include/pipettes/core/motor_interrupt_handler.hpp
@@ -5,8 +5,9 @@
 
 namespace pipettes {
 using namespace motor_messages;
-template <template <class> class QueueImpl, class StatusClient, class DriverClient,
-          typename MotorMoveMessage, typename MotorHardware, class SensorClient>
+template <template <class> class QueueImpl, class StatusClient,
+          class DriverClient, typename MotorMoveMessage, typename MotorHardware,
+          class SensorClient>
 requires MessageQueue<QueueImpl<MotorMoveMessage>, MotorMoveMessage> &&
     std::is_base_of_v<motor_hardware::MotorHardwareIface, MotorHardware>
 class PipetteMotorInterruptHandler
@@ -20,15 +21,15 @@ class PipetteMotorInterruptHandler
     PipetteMotorInterruptHandler() = delete;
     PipetteMotorInterruptHandler(
         MoveQueue& incoming_move_queue, StatusClient& outgoing_queue,
-        DriverClient& driver_queue,
-        MotorHardware& hardware_iface, stall_check::StallCheck& stall,
+        DriverClient& driver_queue, MotorHardware& hardware_iface,
+        stall_check::StallCheck& stall,
         UpdatePositionQueue& incoming_update_position_queue,
         SensorClient& sensor_queue_client)
         : motor_handler::MotorInterruptHandler<
               freertos_message_queue::FreeRTOSMessageQueue, StatusClient,
               DriverClient, MotorMoveMessage, MotorHardware>(
-              incoming_move_queue, outgoing_queue, driver_queue, hardware_iface, stall,
-              incoming_update_position_queue),
+              incoming_move_queue, outgoing_queue, driver_queue, hardware_iface,
+              stall, incoming_update_position_queue),
           sensor_client(sensor_queue_client) {}
 
     ~PipetteMotorInterruptHandler() = default;
diff --git a/include/pipettes/core/tasks/motion_controller_task.hpp b/include/pipettes/core/tasks/motion_controller_task.hpp
index 0f65aca9e..0a8de42ec 100644
--- a/include/pipettes/core/tasks/motion_controller_task.hpp
+++ b/include/pipettes/core/tasks/motion_controller_task.hpp
@@ -191,7 +191,7 @@ class MotionControllerMessageHandler {
                 .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{
diff --git a/include/pipettes/firmware/interfaces_g4.hpp b/include/pipettes/firmware/interfaces_g4.hpp
index 7d4ab2346..ffe123b92 100644
--- a/include/pipettes/firmware/interfaces_g4.hpp
+++ b/include/pipettes/firmware/interfaces_g4.hpp
@@ -95,12 +95,16 @@ auto get_interrupt(motor_hardware::MotorHardware& hw,
                    LowThroughputInterruptQueues& queues,
                    stall_check::StallCheck& stall,
                    sensor_tasks::QueueClient& sensor_client)
-    -> PipetteMotorInterruptHandlerType<linear_motor_tasks::QueueClient, linear_motor_tasks::tmc2130_driver::QueueClient>;
+    -> PipetteMotorInterruptHandlerType<
+        linear_motor_tasks::QueueClient,
+        linear_motor_tasks::tmc2130_driver::QueueClient>;
 auto get_interrupt(motor_hardware::MotorHardware& hw,
                    HighThroughputInterruptQueues& queues,
                    stall_check::StallCheck& stall,
                    sensor_tasks::QueueClient& sensor_client)
-    -> PipetteMotorInterruptHandlerType<linear_motor_tasks::QueueClient, linear_motor_tasks::tmc2160_driver::QueueClient>;
+    -> PipetteMotorInterruptHandlerType<
+        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/pipettes/firmware/interfaces.cpp b/pipettes/firmware/interfaces.cpp
index fb1f92a04..5a73204b1 100644
--- a/pipettes/firmware/interfaces.cpp
+++ b/pipettes/firmware/interfaces.cpp
@@ -59,9 +59,12 @@ auto linear_motor::get_interrupt(motor_hardware::MotorHardware& hw,
                                  LowThroughputInterruptQueues& queues,
                                  stall_check::StallCheck& stall,
                                  sensor_tasks::QueueClient& sensor_client)
-    -> PipetteMotorInterruptHandlerType<linear_motor_tasks::QueueClient, linear_motor_tasks::tmc2130_driver::QueueClient> {
+    -> PipetteMotorInterruptHandlerType<
+        linear_motor_tasks::QueueClient,
+        linear_motor_tasks::tmc2130_driver::QueueClient> {
     return pipettes::PipetteMotorInterruptHandler(
-        queues.plunger_queue, linear_motor_tasks::get_queues(), linear_motor_tasks::tmc2130_driver::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, sensor_client);
 }
 
@@ -69,9 +72,12 @@ auto linear_motor::get_interrupt(motor_hardware::MotorHardware& hw,
                                  HighThroughputInterruptQueues& queues,
                                  stall_check::StallCheck& stall,
                                  sensor_tasks::QueueClient& sensor_client)
-    -> PipetteMotorInterruptHandlerType<linear_motor_tasks::QueueClient, linear_motor_tasks::tmc2160_driver::QueueClient> {
+    -> PipetteMotorInterruptHandlerType<
+        linear_motor_tasks::QueueClient,
+        linear_motor_tasks::tmc2160_driver::QueueClient> {
     return pipettes::PipetteMotorInterruptHandler(
-        queues.plunger_queue, linear_motor_tasks::get_queues(), linear_motor_tasks::tmc2160_driver::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, sensor_client);
 }
 
diff --git a/pipettes/simulator/interfaces.cpp b/pipettes/simulator/interfaces.cpp
index 3369ef67d..b231eda5f 100644
--- a/pipettes/simulator/interfaces.cpp
+++ b/pipettes/simulator/interfaces.cpp
@@ -77,7 +77,8 @@ auto linear_motor::get_interrupt_driver(
 #ifdef USE_PRESSURE_MOVE
     -> motor_interrupt_driver::MotorInterruptDriver<
         linear_motor_tasks::QueueClient,
-        linear_motor_tasks::tmc2130_driver::QueueClient, motor_messages::SensorSyncMove,
+        linear_motor_tasks::tmc2130_driver::QueueClient,
+        motor_messages::SensorSyncMove,
         sim_motor_hardware_iface::SimMotorHardwareIface> {
 #else
     -> motor_interrupt_driver::MotorInterruptDriver<
@@ -98,7 +99,8 @@ auto linear_motor::get_interrupt_driver(
 #ifdef USE_PRESSURE_MOVE
     -> motor_interrupt_driver::MotorInterruptDriver<
         linear_motor_tasks::QueueClient,
-        linear_motor_tasks::tmc2160_driver::QueueClient, motor_messages::SensorSyncMove,
+        linear_motor_tasks::tmc2160_driver::QueueClient,
+        motor_messages::SensorSyncMove,
         sim_motor_hardware_iface::SimMotorHardwareIface> {
 #else
     -> motor_interrupt_driver::MotorInterruptDriver<

From 827a8be125a7e7ca2507384e2fe11cbd45d6b007 Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Mon, 25 Mar 2024 14:49:19 -0400
Subject: [PATCH 68/68] increased build-and-test timeout to 20 min

---
 .github/workflows/build-tests.yaml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

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