diff --git a/api/src/opentrons/hardware_control/backends/flex_protocol.py b/api/src/opentrons/hardware_control/backends/flex_protocol.py index 4d74e9401f0..8d26139ddd3 100644 --- a/api/src/opentrons/hardware_control/backends/flex_protocol.py +++ b/api/src/opentrons/hardware_control/backends/flex_protocol.py @@ -309,6 +309,14 @@ def engaged_axes(self) -> OT3AxisMap[bool]: """Get engaged axes.""" ... + async def update_engaged_axes(self) -> None: + """Update engaged axes.""" + ... + + async def is_motor_engaged(self, axis: Axis) -> bool: + """Check if axis is enabled.""" + ... + async def disengage_axes(self, axes: List[Axis]) -> None: """Disengage axes.""" ... diff --git a/api/src/opentrons/hardware_control/backends/ot3controller.py b/api/src/opentrons/hardware_control/backends/ot3controller.py index 2001f292a12..e6c22cce05d 100644 --- a/api/src/opentrons/hardware_control/backends/ot3controller.py +++ b/api/src/opentrons/hardware_control/backends/ot3controller.py @@ -86,6 +86,7 @@ set_disable_motor, set_enable_tip_motor, set_disable_tip_motor, + get_motor_enabled, ) from opentrons_hardware.hardware_control.motor_position_status import ( get_motor_position, @@ -259,6 +260,7 @@ class OT3Controller(FlexBackend): _encoder_position: Dict[NodeId, float] _motor_status: Dict[NodeId, MotorStatus] _subsystem_manager: SubsystemManager + _engaged_axes: OT3AxisMap[bool] @classmethod async def build( @@ -334,6 +336,7 @@ def __init__( self._gear_motor_position: Dict[NodeId, float] = {} self._encoder_position = self._get_home_position() self._motor_status = {} + self._engaged_axes = {} self._check_updates = check_updates self._initialized = False self._status_bar = status_bar.StatusBar(messenger=self._usb_messenger) @@ -1163,37 +1166,58 @@ async def watch(self, loop: asyncio.AbstractEventLoop) -> None: def axis_bounds(self) -> OT3AxisMap[Tuple[float, float]]: """Get the axis bounds.""" # TODO (AL, 2021-11-18): The bounds need to be defined - phony_bounds = (0, 500) return { - Axis.Z_L: phony_bounds, - Axis.Z_R: phony_bounds, - Axis.P_L: phony_bounds, - Axis.P_R: phony_bounds, - Axis.X: phony_bounds, - Axis.Y: phony_bounds, - Axis.Z_G: phony_bounds, - Axis.Q: phony_bounds, + Axis.Z_L: (0, 300), + Axis.Z_R: (0, 300), + Axis.P_L: (0, 200), + Axis.P_R: (0, 200), + Axis.X: (0, 550), + Axis.Y: (0, 550), + Axis.Z_G: (0, 300), + Axis.Q: (0, 200), } def engaged_axes(self) -> OT3AxisMap[bool]: """Get engaged axes.""" - return {} + return self._engaged_axes + + async def update_engaged_axes(self) -> None: + """Update engaged axes.""" + motor_nodes = self._motor_nodes() + results = await get_motor_enabled(self._messenger, motor_nodes) + for node, status in results.items(): + self._engaged_axes[node_to_axis(node)] = status + + async def is_motor_engaged(self, axis: Axis) -> bool: + node = axis_to_node(axis) + result = await get_motor_enabled(self._messenger, {node}) + engaged = result[node] + self._engaged_axes.update({axis: engaged}) + return engaged async def disengage_axes(self, axes: List[Axis]) -> None: """Disengage axes.""" if Axis.Q in axes: await set_disable_tip_motor(self._messenger, {axis_to_node(Axis.Q)}) - nodes = {axis_to_node(ax) for ax in axes if ax is not Axis.Q} - if len(nodes) > 0: - await set_disable_motor(self._messenger, nodes) + self._engaged_axes[Axis.Q] = False + axes = [ax for ax in axes if ax is not Axis.Q] + + if len(axes) > 0: + await set_disable_motor(self._messenger, {axis_to_node(ax) for ax in axes}) + for ax in axes: + self._engaged_axes[ax] = False async def engage_axes(self, axes: List[Axis]) -> None: """Engage axes.""" if Axis.Q in axes: await set_enable_tip_motor(self._messenger, {axis_to_node(Axis.Q)}) - nodes = {axis_to_node(ax) for ax in axes if ax is not Axis.Q} - if len(nodes) > 0: - await set_enable_motor(self._messenger, nodes) + self._engaged_axes[Axis.Q] = True + axes = [ax for ax in axes if ax is not Axis.Q] + + if len(axes) > 0: + await set_enable_motor(self._messenger, {axis_to_node(ax) for ax in axes}) + for ax in axes: + self._engaged_axes[ax] = True @requires_update async def set_lights(self, button: Optional[bool], rails: Optional[bool]) -> None: diff --git a/api/src/opentrons/hardware_control/backends/ot3simulator.py b/api/src/opentrons/hardware_control/backends/ot3simulator.py index e864dd1ee87..3be608ed810 100644 --- a/api/src/opentrons/hardware_control/backends/ot3simulator.py +++ b/api/src/opentrons/hardware_control/backends/ot3simulator.py @@ -97,6 +97,7 @@ class OT3Simulator(FlexBackend): _position: Dict[Axis, float] _encoder_position: Dict[Axis, float] _motor_status: Dict[Axis, MotorStatus] + _engaged_axes: Dict[Axis, bool] @classmethod async def build( @@ -148,6 +149,7 @@ def __init__( self._initialized = False self._lights = {"button": False, "rails": False} self._gear_motor_position: Dict[Axis, float] = {} + self._engaged_axes: Dict[Axis, bool] = {} self._feature_flags = feature_flags or HardwareFeatureFlags() def _sanitize_attached_instrument( @@ -374,6 +376,8 @@ async def move( Returns: None """ + for ax in origin: + self._engaged_axes[ax] = True self._position.update(target) self._encoder_position.update(target) @@ -396,6 +400,7 @@ async def home( for h in homed: self._position[h] = self._get_home_position()[h] self._motor_status[h] = MotorStatus(True, True) + self._engaged_axes[h] = True return axis_pad(self._position, 0.0) @ensure_yield @@ -643,16 +648,29 @@ async def update_firmware( def engaged_axes(self) -> OT3AxisMap[bool]: """Get engaged axes.""" - return {} + return self._engaged_axes + + async def update_engaged_axes(self) -> None: + """Update engaged axes.""" + return None + + async def is_motor_engaged(self, axis: Axis) -> bool: + if axis not in self._engaged_axes.keys(): + return False + return self._engaged_axes[axis] @ensure_yield async def disengage_axes(self, axes: List[Axis]) -> None: """Disengage axes.""" + for ax in axes: + self._engaged_axes.update({ax: False}) return None @ensure_yield async def engage_axes(self, axes: List[Axis]) -> None: """Engage axes.""" + for ax in axes: + self._engaged_axes.update({ax: True}) return None @ensure_yield diff --git a/api/src/opentrons/hardware_control/ot3api.py b/api/src/opentrons/hardware_control/ot3api.py index f1f9bc8da61..aa84db60ffd 100644 --- a/api/src/opentrons/hardware_control/ot3api.py +++ b/api/src/opentrons/hardware_control/ot3api.py @@ -256,13 +256,15 @@ def is_idle_mount(self, mount: Union[top_types.Mount, OT3Mount]) -> bool: the last moved mount. """ realmount = OT3Mount.from_mount(mount) - if not self._last_moved_mount or realmount == self._last_moved_mount: - return False - - return ( + if realmount == OT3Mount.GRIPPER or ( realmount == OT3Mount.LEFT and self._gantry_load == GantryLoad.HIGH_THROUGHPUT - ) or (realmount == OT3Mount.GRIPPER) + ): + ax = Axis.by_mount(realmount) + if ax in self.engaged_axes.keys(): + return not self.engaged_axes[ax] + + return False @property def door_state(self) -> DoorState: @@ -1318,29 +1320,33 @@ async def _cache_and_maybe_retract_mount(self, mount: OT3Mount) -> None: the 96-channel or gripper mount if it is about to move. """ last_moved = self._last_moved_mount - if self.is_idle_mount(mount): - # home the left/gripper mount if it is current disengaged - await self.home_z(mount) - - if mount != last_moved and last_moved: - await self.retract(last_moved, 10) - - # disengage Axis.Z_L motor and engage the brake to lower power - # consumption and reduce the chance of the 96-channel pipette dropping - if ( - self.gantry_load == GantryLoad.HIGH_THROUGHPUT - and last_moved == OT3Mount.LEFT - ): - await self.disengage_axes([Axis.Z_L]) + # if gripper exists and it's not the moving mount, it should retract + if ( + self.has_gripper() + and mount != OT3Mount.GRIPPER + and not self.is_idle_mount(OT3Mount.GRIPPER) + ): + await self.retract(OT3Mount.GRIPPER, 10) + await self.disengage_axes([Axis.Z_G]) + await self.idle_gripper() - # disegnage Axis.Z_G when we can to reduce the chance of - # the gripper dropping - if last_moved == OT3Mount.GRIPPER: - await self.disengage_axes([Axis.Z_G]) + # if 96-channel pipette is attached and not being moved, it should retract + if ( + mount != OT3Mount.LEFT + and self._gantry_load == GantryLoad.HIGH_THROUGHPUT + and not self.is_idle_mount(OT3Mount.LEFT) + ): + await self.retract(OT3Mount.LEFT, 10) + await self.disengage_axes([Axis.Z_L]) - if mount != OT3Mount.GRIPPER: - await self.idle_gripper() + # if the last moved mount is not covered in neither of the above scenario, + # simply retract the last moved mount + if last_moved and not self.is_idle_mount(last_moved) and mount != last_moved: + await self.retract(last_moved, 10) + # finally, home the current left/gripper mount to prepare for movement + if self.is_idle_mount(mount): + await self.home_z(mount) self._last_moved_mount = mount async def prepare_for_mount_movement( @@ -1480,6 +1486,22 @@ async def _retrieve_home_position( target_pos = {axis: self._backend.home_position()[axis]} return origin_pos, target_pos + async def _enable_before_update_estimation(self, axis: Axis) -> None: + enabled = await self._backend.is_motor_engaged(axis) + + if not enabled: + if axis == Axis.Z_L and self.gantry_load == GantryLoad.HIGH_THROUGHPUT: + # we're here if the left mount has been idle and the brake is engaged + # we want to temporarily increase its hold current to prevent the z + # stage from dropping when switching off the ebrake + async with self._backend.increase_z_l_hold_current(): + await self.engage_axes([axis]) + else: + await self.engage_axes([axis]) + + # now that motor is enabled, we can update position estimation + await self._update_position_estimation([axis]) + @_adjust_high_throughput_z_current async def _home_axis(self, axis: Axis) -> None: """ @@ -1501,22 +1523,12 @@ async def _home_axis(self, axis: Axis) -> None: assert axis not in [Axis.G, Axis.Q] encoder_ok = self._backend.check_encoder_status([axis]) - motor_ok = self._backend.check_motor_status([axis]) - if encoder_ok: - # ensure stepper position can be updated after boot - if axis == Axis.Z_L and self.gantry_load == GantryLoad.HIGH_THROUGHPUT: - # we're here if the left mount has been idle and the brake is engaged - # we want to temporarily increase its hold current to prevent the z - # stage from dropping when switching off the ebrake - async with self._backend.increase_z_l_hold_current(): - await self.engage_axes([axis]) - else: - await self.engage_axes([axis]) - await self._update_position_estimation([axis]) - # refresh motor and encoder statuses after position estimation update - motor_ok = self._backend.check_motor_status([axis]) - encoder_ok = self._backend.check_encoder_status([axis]) + # enable motor (if needed) and update estimation + await self._enable_before_update_estimation(axis) + + # refresh motor status after position estimation update + motor_ok = self._backend.check_motor_status([axis]) if Axis.to_kind(axis) == OT3AxisKind.P: await self._set_plunger_current_and_home(axis, motor_ok, encoder_ok) @@ -1548,22 +1560,21 @@ async def _home_axis(self, axis: Axis) -> None: async def _home(self, axes: Sequence[Axis]) -> None: """Home one axis at a time.""" - async with self._motion_lock: - for axis in axes: - try: - if axis == Axis.G: - await self.home_gripper_jaw() - elif axis == Axis.Q: - await self._backend.home([axis], self.gantry_load) - else: - await self._home_axis(axis) - except BaseException as e: - self._log.exception(f"Homing failed: {e}") - self._current_position.clear() - raise + for axis in axes: + try: + if axis == Axis.G: + await self.home_gripper_jaw() + elif axis == Axis.Q: + await self._backend.home([axis], self.gantry_load) else: - await self._cache_current_position() - await self._cache_encoder_position() + await self._home_axis(axis) + except BaseException as e: + self._log.exception(f"Homing failed: {e}") + self._current_position.clear() + raise + else: + await self._cache_current_position() + await self._cache_encoder_position() @ExecutionManagerProvider.wait_for_running async def home( @@ -1594,7 +1605,8 @@ async def home( if (ax in checked_axes and self._backend.axis_is_present(ax)) ] self._log.info(f"home was called with {axes} generating sequence {home_seq}") - await self._home(home_seq) + async with self._motion_lock: + await self._home(home_seq) def get_engaged_axes(self) -> Dict[Axis, bool]: """Which axes are engaged and holding.""" diff --git a/hardware/opentrons_hardware/firmware_bindings/constants.py b/hardware/opentrons_hardware/firmware_bindings/constants.py index 6d173e6effc..2ee86878eea 100644 --- a/hardware/opentrons_hardware/firmware_bindings/constants.py +++ b/hardware/opentrons_hardware/firmware_bindings/constants.py @@ -155,6 +155,7 @@ class MessageId(int, Enum): error_message = 0x02 get_status_request = 0x01 + get_gear_status_response = 0x4 get_status_response = 0x05 enable_motor_request = 0x06 diff --git a/hardware/opentrons_hardware/firmware_bindings/messages/message_definitions.py b/hardware/opentrons_hardware/firmware_bindings/messages/message_definitions.py index 49698329264..2f202daa158 100644 --- a/hardware/opentrons_hardware/firmware_bindings/messages/message_definitions.py +++ b/hardware/opentrons_hardware/firmware_bindings/messages/message_definitions.py @@ -156,6 +156,17 @@ class GetStatusResponse(BaseMessage): # noqa: D101 message_id: Literal[MessageId.get_status_response] = MessageId.get_status_response +@dataclass +class GearStatusResponse(BaseMessage): # noqa: D101 + payload: payloads.GetStatusResponsePayload + payload_type: Type[ + payloads.GetStatusResponsePayload + ] = payloads.GetStatusResponsePayload + message_id: Literal[ + MessageId.get_gear_status_response + ] = MessageId.get_gear_status_response + + @dataclass class MoveRequest(BaseMessage): # noqa: D101 payload: payloads.MoveRequestPayload diff --git a/hardware/opentrons_hardware/firmware_bindings/messages/messages.py b/hardware/opentrons_hardware/firmware_bindings/messages/messages.py index 930c82bab79..b12d24088c5 100644 --- a/hardware/opentrons_hardware/firmware_bindings/messages/messages.py +++ b/hardware/opentrons_hardware/firmware_bindings/messages/messages.py @@ -19,6 +19,7 @@ defs.StopRequest, defs.GetStatusRequest, defs.GetStatusResponse, + defs.GearStatusResponse, defs.EnableMotorRequest, defs.DisableMotorRequest, defs.MoveRequest, diff --git a/hardware/opentrons_hardware/firmware_bindings/messages/payloads.py b/hardware/opentrons_hardware/firmware_bindings/messages/payloads.py index c9e91d2e6f0..c78b19742d4 100644 --- a/hardware/opentrons_hardware/firmware_bindings/messages/payloads.py +++ b/hardware/opentrons_hardware/firmware_bindings/messages/payloads.py @@ -128,7 +128,6 @@ class GetStatusResponsePayload(EmptyPayload): """Get status response.""" status: utils.UInt8Field - data: utils.UInt32Field @dataclass(eq=False) diff --git a/hardware/opentrons_hardware/hardware_control/motor_enable_disable.py b/hardware/opentrons_hardware/hardware_control/motor_enable_disable.py index 5681e4ccd52..9928b841da9 100644 --- a/hardware/opentrons_hardware/hardware_control/motor_enable_disable.py +++ b/hardware/opentrons_hardware/hardware_control/motor_enable_disable.py @@ -1,14 +1,27 @@ """Utilities for updating the enable/disable state of an OT3 axis.""" -from typing import Set +from typing import Dict, Set import logging -from opentrons_hardware.drivers.can_bus.can_messenger import CanMessenger +import asyncio +from opentrons_hardware.drivers.can_bus.can_messenger import ( + CanMessenger, + WaitableCallback, +) from opentrons_hardware.firmware_bindings.messages.message_definitions import ( EnableMotorRequest, DisableMotorRequest, GearEnableMotorRequest, GearDisableMotorRequest, + GetStatusRequest, + GetStatusResponse, + GearStatusResponse, +) +from opentrons_hardware.firmware_bindings.arbitration_id import ArbitrationId +from opentrons_hardware.firmware_bindings.constants import ( + NodeId, + ErrorCode, + MessageId, ) -from opentrons_hardware.firmware_bindings.constants import NodeId, ErrorCode +from opentrons_hardware.firmware_bindings.messages.messages import MessageDefinition log = logging.getLogger(__name__) @@ -71,3 +84,70 @@ async def set_disable_tip_motor( ) if error != ErrorCode.ok: log.error(f"recieved error {str(error)} trying to disable {str(node)} ") + + +async def get_motor_enabled( + can_messenger: CanMessenger, + nodes: Set[NodeId], + timeout: float = 1.0, +) -> Dict[NodeId, bool]: + """Get motor status of a set of nodes.""" + expected = nodes or set() + reported: Dict[NodeId, bool] = {} + event = asyncio.Event() + + def _filter(arb_id: ArbitrationId) -> bool: + return MessageId(arb_id.parts.message_id) == GetStatusResponse.message_id + + def _listener(message: MessageDefinition, arb_id: ArbitrationId) -> None: + """Listener for receving motor status messages.""" + if isinstance(message, GetStatusResponse): + reported[NodeId(arb_id.parts.originating_node_id)] = bool( + message.payload.status.value + ) + + # found all expected nodes + if expected.issubset(reported): + event.set() + + can_messenger.add_listener(_listener, _filter) + await can_messenger.send(node_id=NodeId.broadcast, message=GetStatusRequest()) + try: + await asyncio.wait_for(event.wait(), timeout) + except asyncio.TimeoutError: + if expected: + log.warning( + "Read motor status timed out, missing nodes: " + f"{expected.difference(reported)}" + ) + else: + log.debug("Read motor status terminated, no missing nodes.") + return reported + + +async def get_tip_motor_enabled( + can_messenger: CanMessenger, + node: NodeId, + timeout: float = 0.5, +) -> bool: + """Get motor status of a node.""" + + def _filter(arbitration_id: ArbitrationId) -> bool: + return (NodeId(arbitration_id.parts.originating_node_id) == node) and ( + MessageId(arbitration_id.parts.message_id) == GetStatusResponse.message_id + ) + + async def _wait_for_response(reader: WaitableCallback) -> bool: + """Listener for receving motor status messages.""" + async for response, _ in reader: + if isinstance(response, GearStatusResponse): + return bool(response.payload.status.value) + raise StopAsyncIteration + + with WaitableCallback(can_messenger, _filter) as reader: + await can_messenger.send(node_id=node, message=GetStatusRequest()) + try: + return await asyncio.wait_for(_wait_for_response(reader), timeout) + except asyncio.TimeoutError: + log.warning("Read tip motor status timed out") + raise StopAsyncIteration