feat(motor-control): use motor status message to get motor enabled (#754

) * use motor status message to get motor enabled * add gear status reponse to tell pipette motors apart
Opentrons · Mar 6, 2024 · f4ada86 · f4ada86
1 parent f775c63
commit f4ada86
Show file tree

Hide file tree

Showing 16 changed files with 75 additions and 15 deletions.
diff --git a/head/core/can_task.cpp b/head/core/can_task.cpp
@@ -43,7 +43,7 @@ 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::MotorStatusRequest>;
 using SystemDispatchTarget = can::dispatch::DispatchParseTarget<
     can::message_handlers::system::SystemMessageHandler<
         head_tasks::HeadQueueClient>,

diff --git a/include/bootloader/core/ids.h b/include/bootloader/core/ids.h
@@ -33,6 +33,7 @@ typedef enum {
     can_messageid_stop_request = 0x0,
     can_messageid_error_message = 0x2,
     can_messageid_get_status_request = 0x1,
+    can_messageid_get_gear_status_response = 0x4,
     can_messageid_get_status_response = 0x5,
     can_messageid_enable_motor_request = 0x6,
     can_messageid_disable_motor_request = 0x7,

diff --git a/include/can/core/ids.hpp b/include/can/core/ids.hpp
@@ -35,6 +35,7 @@ enum class MessageId {
     stop_request = 0x0,
     error_message = 0x2,
     get_status_request = 0x1,
+    get_gear_status_response = 0x4,
     get_status_response = 0x5,
     enable_motor_request = 0x6,
     disable_motor_request = 0x7,

diff --git 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>;
+                     UpdateMotorPositionEstimationRequest, GetMotorUsageRequest,
+                     MotorStatusRequest>;
 
     MotionHandler(MotionTaskClient &motion_client)
         : motion_client{motion_client} {}
@@ -50,7 +50,7 @@ class BrushedMotionHandler {
                      ReadLimitSwitchRequest, MotorPositionRequest,
                      SetGripperErrorToleranceRequest, GetMotorUsageRequest,
                      GripperJawStateRequest, SetGripperJawHoldoffRequest,
-                     GripperJawHoldoffRequest>;
+                     GripperJawHoldoffRequest, MotorStatusRequest>;
 
     BrushedMotionHandler(BrushedMotionTaskClient &motion_client)
         : motion_client{motion_client} {}

diff --git a/include/can/core/messages.hpp b/include/can/core/messages.hpp
@@ -110,6 +110,36 @@ using HeartbeatRequest = Empty<MessageId::heartbeat_request>;
 
 using HeartbeatResponse = Empty<MessageId::heartbeat_response>;
 
+using MotorStatusRequest = Empty<MessageId::get_status_request>;
+
+struct MotorStatusResponse : BaseMessage<MessageId::get_status_response> {
+    uint32_t message_index;
+    uint8_t enabled;
+
+    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(enabled, iter, limit);
+        return iter - body;
+    }
+    auto operator==(const MotorStatusResponse& other) const -> bool = default;
+};
+
+struct GearMotorStatusResponse
+    : BaseMessage<MessageId::get_gear_status_response> {
+    uint32_t message_index;
+    uint8_t enabled;
+
+    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(enabled, iter, limit);
+        return iter - body;
+    }
+    auto operator==(const GearMotorStatusResponse& other) const
+        -> bool = default;
+};
+
 using DeviceInfoRequest = Empty<MessageId::device_info_request>;
 
 struct DeviceInfoResponse : BaseMessage<MessageId::device_info_response> {
@@ -1689,6 +1719,6 @@ using ResponseMessageType = std::variant<
     UpdateMotorPositionEstimationResponse, BaselineSensorResponse,
     PushTipPresenceNotification, GetMotorUsageResponse, GripperJawStateResponse,
     GripperJawHoldoffResponse, HepaUVInfoResponse, GetHepaFanStateResponse,
-    GetHepaUVStateResponse>;
+    GetHepaUVStateResponse, MotorStatusResponse, GearMotorStatusResponse>;
 
 }  // namespace can::messages
diff --git a/include/gantry/core/can_task.hpp b/include/gantry/core/can_task.hpp
@@ -36,7 +36,7 @@ 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::MotorStatusRequest>;
 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
@@ -32,7 +32,7 @@ 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::MotorStatusRequest>;
 using SystemDispatchTarget = can::dispatch::DispatchParseTarget<
     can::message_handlers::system::SystemMessageHandler<
         gripper_tasks::QueueClient>,
@@ -50,7 +50,7 @@ using BrushedMotionDispatchTarget = can::dispatch::DispatchParseTarget<
     can::messages::SetGripperErrorToleranceRequest,
     can::messages::GetMotorUsageRequest, can::messages::GripperJawStateRequest,
     can::messages::SetGripperJawHoldoffRequest,
-    can::messages::GripperJawHoldoffRequest>;
+    can::messages::GripperJawHoldoffRequest, can::messages::MotorStatusRequest>;
 using BrushedMoveGroupDispatchTarget = can::dispatch::DispatchParseTarget<
     can::message_handlers::move_group::BrushedMoveGroupHandler<
         g_tasks::QueueClient>,

diff --git a/include/motor-control/core/brushed_motor/brushed_motion_controller.hpp b/include/motor-control/core/brushed_motor/brushed_motion_controller.hpp
@@ -154,6 +154,8 @@ class MotionController {
         return hardware.get_motor_state();
     }
 
+    [[nodiscard]] auto is_motor_enabled() const -> bool { return enabled; }
+
   private:
     lms::LinearMotionSystemConfig<MEConfig> linear_motion_sys_config;
     BrushedMotorHardwareIface& hardware;

diff --git a/include/motor-control/core/stepper_motor/motion_controller.hpp b/include/motor-control/core/stepper_motor/motion_controller.hpp
@@ -166,6 +166,8 @@ class MotionController {
         usage_client.send_usage_storage_queue(req);
     }
 
+    [[nodiscard]] auto is_motor_enabled() const -> bool { return enabled; }
+
   private:
     lms::LinearMotionSystemConfig<MEConfig> linear_motion_sys_config;
     StepperMotorHardwareIface& hardware;
@@ -314,16 +316,18 @@ class PipetteMotionController {
         usage_client.send_usage_storage_queue(req);
     }
 
+    [[nodiscard]] auto is_motor_enabled() const -> bool { return enabled; }
+
   private:
     lms::LinearMotionSystemConfig<MEConfig> linear_motion_sys_config;
     StepperMotorHardwareIface& hardware;
     MotionConstraints motion_constraints;
     GenericQueue& queue;
 
+    bool enabled = false;
     sq31_31 steps_per_um{0};
     sq31_31 steps_per_mm{0};
     sq31_31 um_per_step{0};
-    bool enabled = false;
     can::ids::GearMotorId gear_motor_id;
 };
 

diff --git a/include/motor-control/core/tasks/brushed_motion_controller_task.hpp b/include/motor-control/core/tasks/brushed_motion_controller_task.hpp
@@ -137,6 +137,13 @@ class MotionControllerMessageHandler {
         can_client.send_can_message(can::ids::NodeId::host, msg);
     }
 
+    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,
+                                               .enabled = response};
+        can_client.send_can_message(can::ids::NodeId::host, msg);
+    }
+
     brushed_motion_controller::MotionController<MEConfig>& controller;
     CanClient& can_client;
     UsageClient& usage_client;

diff --git a/include/motor-control/core/tasks/messages.hpp b/include/motor-control/core/tasks/messages.hpp
@@ -14,7 +14,7 @@ using MotionControlTaskMessage = std::variant<
     can::messages::MotorPositionRequest, can::messages::ReadLimitSwitchRequest,
     can::messages::HomeRequest,
     can::messages::UpdateMotorPositionEstimationRequest,
-    can::messages::GetMotorUsageRequest>;
+    can::messages::GetMotorUsageRequest, can::messages::MotorStatusRequest>;
 
 using MotorDriverTaskMessage =
     std::variant<std::monostate, can::messages::ReadMotorDriverRegister,
@@ -47,7 +47,7 @@ using BrushedMotionControllerTaskMessage = std::variant<
     can::messages::SetGripperErrorToleranceRequest,
     can::messages::GetMotorUsageRequest, can::messages::GripperJawStateRequest,
     can::messages::SetGripperJawHoldoffRequest,
-    can::messages::GripperJawHoldoffRequest>;
+    can::messages::GripperJawHoldoffRequest, can::messages::MotorStatusRequest>;
 
 using BrushedMoveGroupTaskMessage = std::variant<
     std::monostate, can::messages::ClearAllMoveGroupsRequest,

diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp
@@ -140,6 +140,13 @@ class MotionControllerMessageHandler {
         controller.send_usage_data(m.message_index, usage_client);
     }
 
+    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,
+                                               .enabled = response};
+        can_client.send_can_message(can::ids::NodeId::host, msg);
+    }
+
     MotorControllerType& controller;
     CanClient& can_client;
     UsageClient& usage_client;

diff --git a/include/pipettes/core/dispatch_builder.hpp b/include/pipettes/core/dispatch_builder.hpp
@@ -62,15 +62,15 @@ 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::MotorStatusRequest>;
 
 using GearMotionControllerDispatchTarget = can::dispatch::DispatchParseTarget<
     gear_motion_handler::GearMotorMotionHandler<gear_motor_tasks::QueueClient>,
     can::messages::GearDisableMotorRequest,
     can::messages::GearEnableMotorRequest,
     can::messages::GetMotionConstraintsRequest,
     can::messages::SetMotionConstraints, can::messages::ReadLimitSwitchRequest,
-    can::messages::GetMotorUsageRequest>;
+    can::messages::GetMotorUsageRequest, can::messages::MotorStatusRequest>;
 
 using SystemDispatchTarget = can::dispatch::DispatchParseTarget<
     can::message_handlers::system::SystemMessageHandler<

diff --git a/include/pipettes/core/tasks/message_handlers/motion.hpp b/include/pipettes/core/tasks/message_handlers/motion.hpp
@@ -18,7 +18,7 @@ class GearMotorMotionHandler {
         std::variant<std::monostate, GearDisableMotorRequest,
                      GearEnableMotorRequest, GetMotionConstraintsRequest,
                      SetMotionConstraints, ReadLimitSwitchRequest,
-                     GetMotorUsageRequest>;
+                     GetMotorUsageRequest, MotorStatusRequest>;
 
     GearMotorMotionHandler(GearMotionTaskClient &motion_client)
         : motion_client{motion_client} {}

diff --git a/include/pipettes/core/tasks/messages.hpp b/include/pipettes/core/tasks/messages.hpp
@@ -16,7 +16,8 @@ using MotionControlTaskMessage = std::variant<
     can::messages::GearEnableMotorRequest,
     can::messages::GetMotionConstraintsRequest,
     can::messages::SetMotionConstraints, can::messages::StopRequest,
-    can::messages::ReadLimitSwitchRequest, can::messages::GetMotorUsageRequest>;
+    can::messages::ReadLimitSwitchRequest, can::messages::GetMotorUsageRequest,
+    can::messages::MotorStatusRequest>;
 
 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
@@ -117,6 +117,13 @@ class MotionControllerMessageHandler {
         controller.send_usage_data(m.message_index, usage_client);
     }
 
+    void handle(const can::messages::MotorStatusRequest& m) {
+        auto response = static_cast<uint8_t>(controller.is_motor_enabled());
+        can::messages::GearMotorStatusResponse msg{
+            .message_index = m.message_index, .enabled = response};
+        can_client.send_can_message(can::ids::NodeId::host, msg);
+    }
+
     MotorControllerType& controller;
     CanClient& can_client;
     UsageClient& usage_client;