From 4b69f9477908cc969a74235dff56259b9e2d0742 Mon Sep 17 00:00:00 2001 From: Camillo Moschner Date: Wed, 3 Sep 2025 15:36:11 +0100 Subject: [PATCH 1/9] test `request_iswap_relative_wrist_orientation` --- .../backends/hamilton/STAR_backend.py | 33 +++++++++++++++++-- 1 file changed, 31 insertions(+), 2 deletions(-) diff --git a/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py b/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py index 7d019f31e7f..f6b37a38064 100644 --- a/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py +++ b/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py @@ -6763,6 +6763,35 @@ async def iswap_put_plate( self._iswap_parked = False return command_output + # Map motor increments to wrist orientations (constant lookup table). + wrist_orientation_to_motor_increment_dict = { + -26_577: "RIGHT", + -8_754: "STRAIGHT", + 9_101: "LEFT", + 26_852: "REVERSE", + } + + async def request_iswap_relative_wrist_orientation(self): + """ + Request the relative iSWAP wrist orientation. + + Returns: + RelativeWristOrientation enum corresponding to the current orientation. + + Raises: + ValueError: If the returned motor increment is not recognized. + """ + response = await self.send_command(module="R0", command="RT", fmt="rt######") + motor_position_increments = response["rt"] + + if key := STARBackend.wrist_orientation_to_motor_increment_dict.get(motor_position_increments): + return getattr(self.RelativeWristOrientation, key) + + raise ValueError( + f"Unknown wrist orientation: {motor_position_increments}. " + f"Expected one of {list(STARBackend.wrist_orientation_to_motor_increment_dict)}." + ) + async def iswap_rotate( self, rotation_drive: "RotationDriveOrientation", @@ -7491,13 +7520,13 @@ async def rotate_iswap_rotation_drive(self, orientation: RotationDriveOrientatio wp=orientation.value, ) - class WristOrientation(enum.Enum): + class RelativeWristOrientation(enum.Enum): RIGHT = 1 STRAIGHT = 2 LEFT = 3 REVERSE = 4 - async def rotate_iswap_wrist(self, orientation: WristOrientation): + async def rotate_iswap_wrist(self, orientation: RelativeWristOrientation): return await self.send_command( module="R0", command="TP", From 2d2add396ba246696ca8fb68cfc555c768632103 Mon Sep 17 00:00:00 2001 From: Camillo Moschner Date: Wed, 3 Sep 2025 15:44:57 +0100 Subject: [PATCH 2/9] enhance docstring --- .../backends/hamilton/STAR_backend.py | 50 ++++++++++++------- 1 file changed, 31 insertions(+), 19 deletions(-) diff --git a/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py b/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py index f6b37a38064..93c1b583015 100644 --- a/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py +++ b/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py @@ -6765,32 +6765,44 @@ async def iswap_put_plate( # Map motor increments to wrist orientations (constant lookup table). wrist_orientation_to_motor_increment_dict = { - -26_577: "RIGHT", - -8_754: "STRAIGHT", - 9_101: "LEFT", - 26_852: "REVERSE", + -26_577: "RIGHT", + -8_754: "STRAIGHT", + 9_101: "LEFT", + 26_852: "REVERSE", } async def request_iswap_relative_wrist_orientation(self): - """ - Request the relative iSWAP wrist orientation. + """ + Request the relative iSWAP wrist orientation. + This is the orientation of the iSWAP wrist in relation to the iSWAP arm/rotation drive. - Returns: - RelativeWristOrientation enum corresponding to the current orientation. + e.g.: + 1) iSWAP RotationDrive.Front (i.e. pointing to the front of the machine) + + iSWAP RelativeWristOrientation.STRAIGHT (i.e. wrist is also pointing to the front) - Raises: - ValueError: If the returned motor increment is not recognized. - """ - response = await self.send_command(module="R0", command="RT", fmt="rt######") - motor_position_increments = response["rt"] + 2) iSWAP RotationDrive.LEFT (i.e. pointing to the left of the machine) + + iSWAP RelativeWristOrientation.STRAIGHT (i.e. wrist is also pointing to the left) - if key := STARBackend.wrist_orientation_to_motor_increment_dict.get(motor_position_increments): - return getattr(self.RelativeWristOrientation, key) + 3) iSWAP RotationDrive.Front (i.e. pointing to the back of the machine) + + iSWAP RelativeWristOrientation.RIGHT (i.e. wrist is pointing to the left !) - raise ValueError( - f"Unknown wrist orientation: {motor_position_increments}. " - f"Expected one of {list(STARBackend.wrist_orientation_to_motor_increment_dict)}." - ) + The relative wrist orientation is reported as a motor position increment by the STAR + firmware. This value is mapped to a `RelativeWristOrientation` enum member. + + Returns: + RelativeWristOrientation: The interpreted wrist orientation + (e.g., RIGHT, STRAIGHT, LEFT, REVERSE). + """ + response = await self.send_command(module="R0", command="RT", fmt="rt######") + motor_position_increments = response["rt"] + + if key := STARBackend.wrist_orientation_to_motor_increment_dict.get(motor_position_increments): + return getattr(self.RelativeWristOrientation, key) + + raise ValueError( + f"Unknown wrist orientation: {motor_position_increments}. " + f"Expected one of {list(STARBackend.wrist_orientation_to_motor_increment_dict)}." + ) async def iswap_rotate( self, From 18a0b3acfe5d996422b121ce6efc97463fa2aab1 Mon Sep 17 00:00:00 2001 From: Camillo Moschner Date: Wed, 3 Sep 2025 16:01:46 +0100 Subject: [PATCH 3/9] remove "dark magic" getattr() --- .../backends/hamilton/STAR_backend.py | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py b/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py index 93c1b583015..8305872abfa 100644 --- a/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py +++ b/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py @@ -6765,10 +6765,10 @@ async def iswap_put_plate( # Map motor increments to wrist orientations (constant lookup table). wrist_orientation_to_motor_increment_dict = { - -26_577: "RIGHT", - -8_754: "STRAIGHT", - 9_101: "LEFT", - 26_852: "REVERSE", + -26_577: STARBackend.RelativeWristOrientation.RIGHT, + -8_754: STARBackend.RelativeWristOrientation.STRAIGHT, + 9_101: STARBackend.RelativeWristOrientation.LEFT, + 26_852: STARBackend.RelativeWristOrientation.REVERSE, } async def request_iswap_relative_wrist_orientation(self): @@ -6796,12 +6796,12 @@ async def request_iswap_relative_wrist_orientation(self): response = await self.send_command(module="R0", command="RT", fmt="rt######") motor_position_increments = response["rt"] - if key := STARBackend.wrist_orientation_to_motor_increment_dict.get(motor_position_increments): - return getattr(self.RelativeWristOrientation, key) + if orientation := self.wrist_orientation_to_motor_increment_dict.get(motor_position_increments): + return orientation raise ValueError( f"Unknown wrist orientation: {motor_position_increments}. " - f"Expected one of {list(STARBackend.wrist_orientation_to_motor_increment_dict)}." + f"Expected one of {list(self.wrist_orientation_to_motor_increment_dict)}." ) async def iswap_rotate( From c3a75d18a8719a49194e0163bbe4ef942d0a7f81 Mon Sep 17 00:00:00 2001 From: Camillo Moschner Date: Wed, 3 Sep 2025 16:25:45 +0100 Subject: [PATCH 4/9] store all info in method --- .../backends/hamilton/STAR_backend.py | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py b/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py index 8305872abfa..8f8ec1e6d2b 100644 --- a/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py +++ b/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py @@ -6763,14 +6763,6 @@ async def iswap_put_plate( self._iswap_parked = False return command_output - # Map motor increments to wrist orientations (constant lookup table). - wrist_orientation_to_motor_increment_dict = { - -26_577: STARBackend.RelativeWristOrientation.RIGHT, - -8_754: STARBackend.RelativeWristOrientation.STRAIGHT, - 9_101: STARBackend.RelativeWristOrientation.LEFT, - 26_852: STARBackend.RelativeWristOrientation.REVERSE, - } - async def request_iswap_relative_wrist_orientation(self): """ Request the relative iSWAP wrist orientation. @@ -6793,15 +6785,23 @@ async def request_iswap_relative_wrist_orientation(self): RelativeWristOrientation: The interpreted wrist orientation (e.g., RIGHT, STRAIGHT, LEFT, REVERSE). """ + # Map motor increments to wrist orientations (constant lookup table). + wrist_orientation_to_motor_increment_dict = { + -26_577: STARBackend.RelativeWristOrientation.RIGHT, + -8_754: STARBackend.RelativeWristOrientation.STRAIGHT, + 9_101: STARBackend.RelativeWristOrientation.LEFT, + 26_852: STARBackend.RelativeWristOrientation.REVERSE, + } + response = await self.send_command(module="R0", command="RT", fmt="rt######") motor_position_increments = response["rt"] - if orientation := self.wrist_orientation_to_motor_increment_dict.get(motor_position_increments): + if orientation := wrist_orientation_to_motor_increment_dict.get(motor_position_increments): return orientation raise ValueError( f"Unknown wrist orientation: {motor_position_increments}. " - f"Expected one of {list(self.wrist_orientation_to_motor_increment_dict)}." + f"Expected one of {list(wrist_orientation_to_motor_increment_dict)}." ) async def iswap_rotate( From f6a6fc152452a2070e705e8ccaf5e0359f9c92be Mon Sep 17 00:00:00 2001 From: Camillo Moschner Date: Thu, 4 Sep 2025 22:01:44 +0100 Subject: [PATCH 5/9] update naming --- Untitled.ipynb | 110 ++++++++++++++++++ .../backends/hamilton/STAR_backend.py | 47 ++++---- 2 files changed, 137 insertions(+), 20 deletions(-) create mode 100644 Untitled.ipynb diff --git a/Untitled.ipynb b/Untitled.ipynb new file mode 100644 index 00000000000..202c9f4fec9 --- /dev/null +++ b/Untitled.ipynb @@ -0,0 +1,110 @@ +{ + "cells": [ + { + "cell_type": "code", + "execution_count": 2, + "id": "6174d785-e532-4df1-ba61-235f0fe9f000", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "SCARA Arm Position -> X: 134.69, Y: 163.40, Z: 35.00 | Joint Angles: [30°, 45°, -20°], Z Offset: -15\n" + ] + } + ], + "source": [ + "import math\n", + "\n", + "class SCARARobotArm:\n", + " def __init__(self, link_lengths, base_height=0.0):\n", + " \"\"\"\n", + " Initializes the SCARA robot arm with a dynamic number of revolute joints.\n", + "\n", + " :param link_lengths: List of link lengths [L1, L2, ...]\n", + " :param base_height: Base height for the prismatic joint (Z-axis)\n", + " \"\"\"\n", + " self.link_lengths = link_lengths\n", + " self.n_joints = len(link_lengths)\n", + " self.joint_angles = [0.0] * self.n_joints # In degrees\n", + " self.base_height = base_height\n", + " self.z_offset = 0.0 # Vertical prismatic joint (Z-axis)\n", + "\n", + " def set_joint_angles(self, angles):\n", + " \"\"\"\n", + " Sets the revolute joint angles (in degrees).\n", + "\n", + " :param angles: List of joint angles, same length as link_lengths\n", + " \"\"\"\n", + " if len(angles) != self.n_joints:\n", + " raise ValueError(\"Number of angles must match number of links.\")\n", + " self.joint_angles = angles\n", + "\n", + " def set_z_offset(self, z):\n", + " \"\"\"Set vertical offset for the prismatic joint (Z-axis).\"\"\"\n", + " self.z_offset = z\n", + "\n", + " def forward_kinematics(self):\n", + " \"\"\"\n", + " Calculates the end-effector position (x, y, z) based on current joint angles.\n", + " Returns:\n", + " (x, y, z): End-effector Cartesian coordinates\n", + " \"\"\"\n", + " x, y = 0.0, 0.0\n", + " angle_sum = 0.0\n", + "\n", + " for i in range(self.n_joints):\n", + " angle_sum += math.radians(self.joint_angles[i])\n", + " x += self.link_lengths[i] * math.cos(angle_sum)\n", + " y += self.link_lengths[i] * math.sin(angle_sum)\n", + "\n", + " z = self.base_height + self.z_offset\n", + " return (x, y, z)\n", + "\n", + " def __str__(self):\n", + " x, y, z = self.forward_kinematics()\n", + " angles_str = \", \".join([f\"{a}°\" for a in self.joint_angles])\n", + " return (f\"SCARA Arm Position -> X: {x:.2f}, Y: {y:.2f}, Z: {z:.2f} | \"\n", + " f\"Joint Angles: [{angles_str}], Z Offset: {self.z_offset}\")\n", + "\n", + "# Example usage:\n", + "if __name__ == \"__main__\":\n", + " # Create a 3-joint SCARA-like robot (not common, but for flexibility/testing)\n", + " arm = SCARARobotArm(link_lengths=[100, 75, 50], base_height=50)\n", + " arm.set_joint_angles([30, 45, -20])\n", + " arm.set_z_offset(-15)\n", + " print(arm)\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "259ff2bb-7891-4f33-af24-6ec565cd0cc7", + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3 (ipykernel)", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.12.11" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py b/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py index 8f8ec1e6d2b..48e1011fca5 100644 --- a/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py +++ b/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py @@ -6763,38 +6763,45 @@ async def iswap_put_plate( self._iswap_parked = False return command_output - async def request_iswap_relative_wrist_orientation(self): + async def get_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 response["rt"] + + async def request_iswap_wrist_drive_orientation(self) -> "WristDriveOrientation": """ - Request the relative iSWAP wrist orientation. - This is the orientation of the iSWAP wrist in relation to the iSWAP arm/rotation drive. + Request the iSWAP wrist drive orientation. + This is the orientation of the iSWAP wrist drive (always in relation to the + iSWAP arm/rotation drive). e.g.: - 1) iSWAP RotationDrive.Front (i.e. pointing to the front of the machine) + - iSWAP RelativeWristOrientation.STRAIGHT (i.e. wrist is also pointing to the front) + 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 RotationDrive.LEFT (i.e. pointing to the left of the machine) + - iSWAP RelativeWristOrientation.STRAIGHT (i.e. wrist is also pointing to the left) + 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 RotationDrive.Front (i.e. pointing to the back of the machine) + - iSWAP RelativeWristOrientation.RIGHT (i.e. wrist is 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 `RelativeWristOrientation` enum member. + firmware. This value is mapped to a `WristDriveOrientation` enum member. Returns: - RelativeWristOrientation: The interpreted wrist orientation - (e.g., RIGHT, STRAIGHT, LEFT, REVERSE). + WristDriveOrientation: The interpreted wrist orientation + (e.g., RIGHT, STRAIGHT, LEFT, REVERSE). """ + # Map motor increments to wrist orientations (constant lookup table). + # TODO: this might not be the same for all machines. wrist_orientation_to_motor_increment_dict = { - -26_577: STARBackend.RelativeWristOrientation.RIGHT, - -8_754: STARBackend.RelativeWristOrientation.STRAIGHT, - 9_101: STARBackend.RelativeWristOrientation.LEFT, - 26_852: STARBackend.RelativeWristOrientation.REVERSE, + -26_577: STARBackend.WristDriveOrientation.RIGHT, + -8_754: STARBackend.WristDriveOrientation.STRAIGHT, + 9_101: STARBackend.WristDriveOrientation.LEFT, + 26_852: STARBackend.WristDriveOrientation.REVERSE, } - response = await self.send_command(module="R0", command="RT", fmt="rt######") - motor_position_increments = response["rt"] + motor_position_increments = await self.get_iswap_wrist_drive_position_increments() if orientation := wrist_orientation_to_motor_increment_dict.get(motor_position_increments): return orientation @@ -7532,13 +7539,13 @@ async def rotate_iswap_rotation_drive(self, orientation: RotationDriveOrientatio wp=orientation.value, ) - class RelativeWristOrientation(enum.Enum): + class WristDriveOrientation(enum.Enum): RIGHT = 1 STRAIGHT = 2 LEFT = 3 REVERSE = 4 - async def rotate_iswap_wrist(self, orientation: RelativeWristOrientation): + async def rotate_iswap_wrist(self, orientation: WristDriveOrientation): return await self.send_command( module="R0", command="TP", From 40e8e20543bc1e2ac8df0354fcda061272963e04 Mon Sep 17 00:00:00 2001 From: Camillo Moschner Date: Thu, 4 Sep 2025 22:54:52 +0100 Subject: [PATCH 6/9] fix cast int --- pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py b/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py index 48e1011fca5..751093fa3c4 100644 --- a/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py +++ b/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py @@ -6766,7 +6766,7 @@ async def iswap_put_plate( async def get_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 response["rt"] + return cast(int, response["rt"]) async def request_iswap_wrist_drive_orientation(self) -> "WristDriveOrientation": """ From 26cfd6b8373f49481b4abd642b784a30969a01ee Mon Sep 17 00:00:00 2001 From: Camillo Moschner Date: Thu, 4 Sep 2025 23:08:20 +0100 Subject: [PATCH 7/9] update name request --- pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py b/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py index 751093fa3c4..8057322a056 100644 --- a/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py +++ b/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py @@ -6763,7 +6763,7 @@ async def iswap_put_plate( self._iswap_parked = False return command_output - async def get_iswap_wrist_drive_position_increments(self) -> int: + 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"]) @@ -6801,7 +6801,7 @@ async def request_iswap_wrist_drive_orientation(self) -> "WristDriveOrientation" 26_852: STARBackend.WristDriveOrientation.REVERSE, } - motor_position_increments = await self.get_iswap_wrist_drive_position_increments() + motor_position_increments = await self.request_iswap_wrist_drive_position_increments() if orientation := wrist_orientation_to_motor_increment_dict.get(motor_position_increments): return orientation From 92cdbc38517936d92e63e3823b3bb9347e592667 Mon Sep 17 00:00:00 2001 From: Camillo Moschner Date: Thu, 4 Sep 2025 23:10:01 +0100 Subject: [PATCH 8/9] use range +-50 --- .../backends/hamilton/STAR_backend.py | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py b/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py index 8057322a056..c8eaf9accd7 100644 --- a/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py +++ b/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py @@ -6793,18 +6793,18 @@ async def request_iswap_wrist_drive_orientation(self) -> "WristDriveOrientation" """ # Map motor increments to wrist orientations (constant lookup table). - # TODO: this might not be the same for all machines. wrist_orientation_to_motor_increment_dict = { - -26_577: STARBackend.WristDriveOrientation.RIGHT, - -8_754: STARBackend.WristDriveOrientation.STRAIGHT, - 9_101: STARBackend.WristDriveOrientation.LEFT, - 26_852: STARBackend.WristDriveOrientation.REVERSE, + 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), } motor_position_increments = await self.request_iswap_wrist_drive_position_increments() - if orientation := wrist_orientation_to_motor_increment_dict.get(motor_position_increments): - return orientation + 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}. " From fd53b5cc10f1d613264318124862151ddc413cb6 Mon Sep 17 00:00:00 2001 From: Camillo Moschner Date: Thu, 4 Sep 2025 23:15:54 +0100 Subject: [PATCH 9/9] add request_iswap_rotation_drive_orientation --- .../backends/hamilton/STAR_backend.py | 36 +++++++++++++++++++ 1 file changed, 36 insertions(+) diff --git a/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py b/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py index c8eaf9accd7..3019d8eb5b2 100644 --- a/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py +++ b/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py @@ -6763,6 +6763,42 @@ async def iswap_put_plate( self._iswap_parked = False return command_output + 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="RS", fmt="rs######") + return cast(int, response["rs"]) + + 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 + + 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), + } + + 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)}." + ) + 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######")