debug changes

Opentrons · Nov 14, 2023 · e1627b2 · e1627b2
1 parent 110b08a
commit e1627b2
Show file tree

Hide file tree

Showing 15 changed files with 67 additions and 43 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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 =