diff --git a/head/core/can_task.cpp b/head/core/can_task.cpp index 765e749b2..212248669 100644 --- 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 index 7c65b6c73..7f3e4b737 100644 --- 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 index fbffd73c2..5c0fd62b9 100644 --- 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 index 203407ab7..bda9dddbf 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; + 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 index 024de1908..6528275a5 100644 --- a/include/can/core/messages.hpp +++ b/include/can/core/messages.hpp @@ -110,6 +110,36 @@ using HeartbeatRequest = Empty; using HeartbeatResponse = Empty; +using MotorStatusRequest = Empty; + +struct MotorStatusResponse : BaseMessage { + uint32_t message_index; + uint8_t enabled; + + template + 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 { + uint32_t message_index; + uint8_t enabled; + + template + 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; struct DeviceInfoResponse : BaseMessage { @@ -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 index 8f72fc7ad..2c4918fff 100644 --- 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 index e04affca3..222f7e5aa 100644 --- 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 index 91610651c..00ec0dddf 100644 --- 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 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 index b9709df82..4c483ca69 100644 --- 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 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 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 index 1259ae729..cc817135d 100644 --- 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(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& 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 index 9feb1ce19..b8ecbf744 100644 --- 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; + 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 index e58388779..4ace8feab 100644 --- 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(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 index a3f24b51f..1141dcd4f 100644 --- a/include/pipettes/core/dispatch_builder.hpp +++ b/include/pipettes/core/dispatch_builder.hpp @@ -62,7 +62,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 GearMotionControllerDispatchTarget = can::dispatch::DispatchParseTarget< gear_motion_handler::GearMotorMotionHandler, @@ -70,7 +70,7 @@ using GearMotionControllerDispatchTarget = can::dispatch::DispatchParseTarget< 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 index 870a05842..93749af62 100644 --- 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; + 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 index 074629f01..7dcd6388d 100644 --- 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(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;