working prototype

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 =

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 =

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 =

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 =

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 =

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,

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,

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;

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

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++;

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

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

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;

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 {

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 {

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",