Expose STARBackend.iswap_rotation_drive_request_position()

pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py

@@ -1367,6 +1367,7 @@ def __init__(
     self._extended_conf: Optional[ExtendedConfiguration] = None
     self._channel_traversal_height: float = 245.0
     self._iswap_traversal_height: float = 280.0
+    self._iswap_rotation_drive_x_offset_mm: Optional[float] = None
     self.core_adjustment = Coordinate.zero()
     self._unsafe = UnSafe(self)
 
@@ -1763,6 +1764,8 @@ async def set_up_iswap():
           minimum_traverse_height_at_beginning_of_a_command=int(self._iswap_traversal_height * 10)
         )
 
+        self._iswap_rotation_drive_x_offset_mm = await self._iswap_rotation_drive_request_x_offset()
+
     async def set_up_core96_head():
       if self.extended_conf.left_x_drive.core_96_head_installed and not skip_core96_head:
         # Initialize 96-head
@@ -10026,85 +10029,179 @@ async def iswap_put_plate(
     self._iswap_parked = False
     return command_output
 
+  # -----------------------------------------------------------------------
+  # iSWAP: Rotation Drive (Joint 1)
+  # -----------------------------------------------------------------------
+
+  async def _iswap_rotation_drive_request_x_offset(self) -> float:
+    """Read the X-offset i.e. X-axis center <-> iSWAP rotation drive, in mm.
+
+    Stored in the master EEPROM as parameter `kg`.
+    Default: 34.0 mm, but typically tuned per machine during service calibration.
+    Required for deriving the iSWAP rotation drive's deck X coordinate from
+    the X-arm carriage center.
+    Cached on the backend as `_iswap_rotation_drive_x_offset_mm` during setup.
+    """
+    if not self.extended_conf.left_x_drive.iswap_installed:
+      raise RuntimeError("iSWAP is not installed")
+    resp = await self.send_command(module="C0", command="RA", ra="kg", fmt="kg###")
+    return cast(int, resp["kg"]) / 10.0
+
+  # Vertical drop from the iSWAP rotation drive plane to the gripper finger
+  # plane. R0 RZ is calibrated to the finger plane; the rotation drive sits
+  # 13 mm above it.
+  iswap_rotation_drive_z_offset_above_finger_mm = 13.0
+
+  async def iswap_rotation_drive_request_position(self) -> Coordinate:
+    """Position of the iSWAP rotation drive (joint 1) in deck coordinates, mm.
+
+    Composition:
+      x = request_left_x_arm_position() - _iswap_rotation_drive_x_offset_mm
+      y = iswap_rotation_drive_request_y()
+      z = (await request_iswap_position()).z + iswap_rotation_drive_z_offset_above_finger_mm
+
+    The Z offset (13 mm) is the structural drop from the rotation drive
+    plane to the gripper finger plane. R0 RZ is Hamilton-calibrated to
+    the finger plane, so we add 13 mm to recover the rotation drive's
+    true Z.
+    """
+
+    if not self.extended_conf.left_x_drive.iswap_installed:
+      raise RuntimeError("iSWAP is not installed")
+
+    if self._iswap_rotation_drive_x_offset_mm is None:
+      self._iswap_rotation_drive_x_offset_mm = await self._iswap_rotation_drive_request_x_offset()
+
+    x_arm_center = await self.request_left_x_arm_position()
+    rotation_drive_y = await self.iswap_rotation_drive_request_y()
+    finger_plane_z = (await self.request_iswap_position()).z
+
+    return Coordinate(
+      x=x_arm_center - self._iswap_rotation_drive_x_offset_mm,
+      y=rotation_drive_y,
+      z=finger_plane_z + self.iswap_rotation_drive_z_offset_above_finger_mm,
+    )
+
   async def request_iswap_rotation_drive_position_increments(self) -> int:
     """Query the iSWAP rotation drive position (units: increments) from the firmware."""
     response = await self.send_command(module="R0", command="RW", fmt="rw######")
     return cast(int, response["rw"])
 
   async def request_iswap_rotation_drive_orientation(self) -> "RotationDriveOrientation":
-    """
-    Request the iSWAP rotation drive orientation.
-    This is the orientation of the iSWAP rotation drive (relative to the machine).
-
-    Uses empirically determined increment values:
-      FRONT: -25 ± 50
-      RIGHT: +29068 ± 50
-      LEFT:  -29116 ± 50
+    """Request the iSWAP rotation drive orientation.
+
+    Uses nearest-neighbour classification against firmware default `pw`
+    values. Each machine's EEPROM stores its own `pw` adjustment so the
+    actual stop position can drift by up to a few hundred increments per
+    machine; an earlier implementation used +/-50 windows and faulted on
+    machines calibrated outside that band. We now pick whichever predefined
+    stop is closest and only raise if the drive is more than ~5 deg
+    (~1700 incr) from any of them, which catches "drive is mid-transit /
+    undefined" cases without being brittle to per-machine calibration.
+
+    Defaults (W-drive resolution = 0.00310 deg/incr):
+      LEFT          W1      -29068 incr  (~ -90 deg)
+      FRONT         W2          +0 incr  (~   0 deg)
+      RIGHT         W3      +29068 incr  (~ +90 deg)
+      PARKED_RIGHT  park    +29500 incr  (~ +91 deg, beyond W3 at the stop)
 
     Returns:
-      RotationDriveOrientation: The interpreted rotation orientation (LEFT, FRONT, RIGHT).
-    """
-    # Map motor increments to rotation orientations (constant lookup table).
-    rotation_orientation_to_motor_increment_dict = {
-      STARBackend.RotationDriveOrientation.FRONT: range(-75, 26),
-      STARBackend.RotationDriveOrientation.RIGHT: range(29018, 29119),
-      STARBackend.RotationDriveOrientation.LEFT: range(-29166, -29065),
-      STARBackend.RotationDriveOrientation.PARKED_RIGHT: range(29450, 29550),
-      # TODO: add range for STAR(let)s with "PARKED_LEFT" setting
+      RotationDriveOrientation: The interpreted rotation orientation
+        (LEFT, FRONT, RIGHT, or PARKED_RIGHT).
+
+    Raises:
+      ValueError: if the measured position is more than 1700 incr (~5 deg)
+        from any predefined stop (drive is in transit or drifted).
+    """
+    # Nearest-neighbour reference positions (firmware `pw` defaults).
+    # PARKED_RIGHT is kept as a distinct neighbour so we can report "parked"
+    # explicitly when the drive sits at the parking stop rather than the W3
+    # work stop.
+    # TODO: add PARKED_LEFT reference for STAR(let)s that park on the left.
+    rotation_reference_positions = {
+      STARBackend.RotationDriveOrientation.LEFT: -29068,
+      STARBackend.RotationDriveOrientation.FRONT: 0,
+      STARBackend.RotationDriveOrientation.RIGHT: 29068,
+      STARBackend.RotationDriveOrientation.PARKED_RIGHT: 29500,
     }
+    tolerance_incr = 1700  # ~5 deg at 0.00310 deg/incr (iSWAP W-drive resolution)
 
     motor_position_increments = await self.request_iswap_rotation_drive_position_increments()
 
-    for orientation, increment_range in rotation_orientation_to_motor_increment_dict.items():
-      if motor_position_increments in increment_range:
-        return orientation
-
-    raise ValueError(
-      f"Unknown rotation orientation: {motor_position_increments}. "
-      f"Expected one of {list(rotation_orientation_to_motor_increment_dict.values())}."
+    orientation, offset = min(
+      ((o, abs(p - motor_position_increments)) for o, p in rotation_reference_positions.items()),
+      key=lambda pair: pair[1],
     )
+    if offset > tolerance_incr:
+      raise ValueError(
+        f"Unknown rotation orientation: {motor_position_increments} incr is "
+        f"{offset} incr (~{offset * 0.00310:.2f} deg) from the nearest predefined "
+        f"stop ({orientation.name} at {rotation_reference_positions[orientation]}). "
+        "Is the rotation drive in transit or mis-calibrated?"
+      )
+    return orientation
 
   async def request_iswap_wrist_drive_position_increments(self) -> int:
     """Query the iSWAP wrist drive position (units: increments) from the firmware."""
     response = await self.send_command(module="R0", command="RT", fmt="rt######")
     return cast(int, response["rt"])
 
   async def request_iswap_wrist_drive_orientation(self) -> "WristDriveOrientation":
-    """
-    Request the iSWAP wrist drive orientation.
-    This is the orientation of the iSWAP wrist drive (always in relation to the iSWAP arm/rotation drive).
+    """Request the iSWAP wrist drive orientation (relative to the rotation drive).
 
     e.g.:
-    1) iSWAP RotationDriveOrientation.FRONT (i.e. pointing to the front of the machine) + iSWAP WristDriveOrientation.STRAIGHT (i.e. wrist is also pointing to the front)
-
-    2) iSWAP RotationDriveOrientation.LEFT (i.e. pointing to the left of the machine) + iSWAP WristDriveOrientation.STRAIGHT (i.e. wrist is also pointing to the left)
-
-    3) iSWAP RotationDriveOrientation.FRONT (i.e. pointing to the front of the machine) + iSWAP WristDriveOrientation.RIGHT (i.e. wrist is pointing to the left !)
-
-    The relative wrist orientation is reported as a motor position increment by the STAR firmware. This value is mapped to a `WristDriveOrientation` enum member.
+    1) RotationDriveOrientation.FRONT + WristDriveOrientation.STRAIGHT
+       => wrist also points to the front of the machine.
+    2) RotationDriveOrientation.LEFT + WristDriveOrientation.STRAIGHT
+       => wrist also points to the left of the machine.
+    3) RotationDriveOrientation.FRONT + WristDriveOrientation.RIGHT
+       => wrist points to the left of the machine.
+
+    Uses nearest-neighbour classification against firmware default `pt`
+    values. Each machine's EEPROM stores its own `pt` adjustment so the
+    actual stop position can drift by up to a few hundred increments per
+    machine; an earlier implementation used +/-50 windows and faulted on
+    machines calibrated outside that band. We now pick whichever predefined
+    stop is closest and only raise if the wrist is more than ~5 deg
+    (~1000 incr) from any of them.
+
+    Defaults (T-drive resolution = 0.00508 deg/incr):
+      RIGHT     T1   -26577 incr  (~ -135 deg)
+      STRAIGHT  T2    -8859 incr  (~  -45 deg)
+      LEFT      T3    +8859 incr  (~  +45 deg)
+      REVERSE   T4   +26577 incr  (~ +135 deg)
 
     Returns:
-      WristDriveOrientation: The interpreted wrist orientation (e.g., RIGHT, STRAIGHT, LEFT, REVERSE).
-    """
+      WristDriveOrientation: The interpreted wrist orientation
+        (RIGHT, STRAIGHT, LEFT, or REVERSE).
 
-    # Map motor increments to wrist orientations (constant lookup table).
-    wrist_orientation_to_motor_increment_dict = {
-      STARBackend.WristDriveOrientation.RIGHT: range(-26_627, -26_527),
-      STARBackend.WristDriveOrientation.STRAIGHT: range(-8_804, -8_704),
-      STARBackend.WristDriveOrientation.LEFT: range(9_051, 9_151),
-      STARBackend.WristDriveOrientation.REVERSE: range(26_802, 26_902),
+    Raises:
+      ValueError: if the measured position is more than 1000 incr (~5 deg)
+        from any predefined stop (drive is in transit or drifted).
+    """
+    # Nearest-neighbour reference positions (firmware `pt` defaults).
+    wrist_reference_positions = {
+      STARBackend.WristDriveOrientation.RIGHT: -26577,
+      STARBackend.WristDriveOrientation.STRAIGHT: -8859,
+      STARBackend.WristDriveOrientation.LEFT: 8859,
+      STARBackend.WristDriveOrientation.REVERSE: 26577,
     }
+    tolerance_incr = 1000  # ~5 deg at 0.00508 deg/incr (iSWAP T-drive resolution)
 
     motor_position_increments = await self.request_iswap_wrist_drive_position_increments()
 
-    for orientation, increment_range in wrist_orientation_to_motor_increment_dict.items():
-      if motor_position_increments in increment_range:
-        return orientation
-
-    raise ValueError(
-      f"Unknown wrist orientation: {motor_position_increments}. "
-      f"Expected one of {list(wrist_orientation_to_motor_increment_dict)}."
+    orientation, offset = min(
+      ((o, abs(p - motor_position_increments)) for o, p in wrist_reference_positions.items()),
+      key=lambda pair: pair[1],
     )
+    if offset > tolerance_incr:
+      raise ValueError(
+        f"Unknown wrist orientation: {motor_position_increments} incr is "
+        f"{offset} incr (~{offset * 0.00508:.2f} deg) from the nearest predefined "
+        f"stop ({orientation.name} at {wrist_reference_positions[orientation]}). "
+        "Is the wrist drive in transit or mis-calibrated?"
+      )
+    return orientation
 
   async def iswap_rotate(
     self,