diff --git a/docs/_static/devices.json b/docs/_static/devices.json index 4dfbc20060a..e772ce667d9 100644 --- a/docs/_static/devices.json +++ b/docs/_static/devices.json @@ -57,6 +57,7 @@ "api": "pylabrobot.agilent.vspin.Access2", "api_version": "v1", "code_slug": "agilent/vspin", + "doc_slug": "agilent/vspin/hello-world", "manager": "https://discuss.pylabrobot.org/u/rickwierenga", "oem": "https://www.agilent.com/en/product/automated-liquid-handling/automated-microplate-management/microplate-centrifuge" }, diff --git a/docs/api/pylabrobot.agilent.rst b/docs/api/pylabrobot.agilent.rst index b67cf81071c..d8c71da79f4 100644 --- a/docs/api/pylabrobot.agilent.rst +++ b/docs/api/pylabrobot.agilent.rst @@ -99,6 +99,8 @@ VSpin VSpin Access2 Access2Driver + ServoStatus + Access2Status PlateLoc diff --git a/docs/user_guide/agilent/vspin/hello-world.ipynb b/docs/user_guide/agilent/vspin/hello-world.ipynb index 86c8dbe8075..5372f56f3ad 100644 --- a/docs/user_guide/agilent/vspin/hello-world.ipynb +++ b/docs/user_guide/agilent/vspin/hello-world.ipynb @@ -22,7 +22,7 @@ "```{warning}\n", "Follow the centrifuge manufacturer's installation, plate-compatibility, balancing, and safety\n", "instructions. Before every run, use two opposing loads of equal mass and make sure both are fully\n", - "seated. The driver does not provide a public software abort command, so keep the instrument's\n", + "seated. `stop_spin()` provides a controlled software abort, but always keep the instrument's\n", "physical controls accessible.\n", "```" ] @@ -275,6 +275,22 @@ ")" ] }, + { + "cell_type": "markdown", + "id": "vspin-abort-md", + "metadata": {}, + "source": [ + "## Abort an active spin safely\n", + "\n", + "If another task needs to abort a running `spin()` call, use `stop_spin()`. It sends a controlled\n", + "zero-velocity trajectory and returns only after the tachometer confirms that the rotor stopped.\n", + "The physical emergency stop remains the authority for an emergency.\n", + "\n", + "```python\n", + "await vspin.stop_spin(deceleration=0.8)\n", + "```" + ] + }, { "cell_type": "markdown", "id": "vspin-return-bucket1-md", diff --git a/pylabrobot/agilent/vspin/README.md b/pylabrobot/agilent/vspin/README.md new file mode 100644 index 00000000000..a33053bc8da --- /dev/null +++ b/pylabrobot/agilent/vspin/README.md @@ -0,0 +1,3 @@ +# VSpin + +Special thanks to Reed Kelso for generously sharing his code on Agilent VSpin and Access2 in the [`vspin-cockpit`](https://github.com/kelsorj/vspin-cockpit) repository. diff --git a/pylabrobot/agilent/vspin/__init__.py b/pylabrobot/agilent/vspin/__init__.py index c0a0a19e902..6f3f447df9f 100644 --- a/pylabrobot/agilent/vspin/__init__.py +++ b/pylabrobot/agilent/vspin/__init__.py @@ -1,2 +1,4 @@ +from pylabrobot.agilent.vspin._access2_protocol import Access2Status +from pylabrobot.agilent.vspin._nmc import ServoStatus from pylabrobot.agilent.vspin.access2 import Access2, Access2Driver from pylabrobot.agilent.vspin.vspin import VSpin diff --git a/pylabrobot/agilent/vspin/_access2_protocol.py b/pylabrobot/agilent/vspin/_access2_protocol.py new file mode 100644 index 00000000000..1d6e380843e --- /dev/null +++ b/pylabrobot/agilent/vspin/_access2_protocol.py @@ -0,0 +1,398 @@ +"""Agilent Access2 command and FTDI framing primitives. + +The Access2 command layer uses a one-byte command identifier followed by a +little-endian 16-bit payload length. The PLR-supported FTDI connection wraps +that command in a Velocity11 envelope and protects it with CRC-16/XMODEM:: + + 0x11 | 0x05 | command length (big endian) | 0x00 | command | CRC (big endian) +""" + +from __future__ import annotations + +import dataclasses + +from pylabrobot.io.binary import Reader, Writer + +VELOCITY11_HEADER = 0x11 +VELOCITY11_PACKET_TYPE = 0x05 +VELOCITY11_CHANNEL = 0x00 +MAX_INNER_FRAME_LENGTH = 4096 + +# Access2 command identifiers. +GET_FIRMWARE_VERSION = 0x00 +INITIALIZE = 0x10 +CLOSE = 0x12 +PING = 0x14 +GET_HARDWARE_VERSION = 0x16 +GET_STATUS = 0x20 +WRITE_FLASH = 0x22 +READ_FLASH = 0x24 +USE_FLASH = 0x26 +FORMAT_FLASH = 0x28 +RESET_ACCESS2_CIRCUIT_BREAKER = 0x30 +RESET_VSPIN_CIRCUIT_BREAKER = 0x32 +RESET_ESTOP = 0x34 +SERVO_SWITCH = 0x36 +HOME = 0x40 +JOG_AXIS = 0x42 +MOVE_TO_LOCATION = 0x44 +MOVE_TO_POSITION = 0x46 +GET_SENSOR_VALUES = 0x50 + +# Axis addresses. +AXIS_GRIPPER = 1 +AXIS_Y = 2 +AXIS_Z = 3 + +# Stored teachpoint indices. +TEACHPOINT_PARK = 0 +TEACHPOINT_PICK = 1 +TEACHPOINT_BUCKET_1 = 2 +TEACHPOINT_BUCKET_2 = 3 +TEACHPOINT_HOVER = 4 + +# Motion profile indices. +PROFILE_STATIC = 0 +PROFILE_HOMING = 1 +PROFILE_DYNAMIC_EMPTY = 2 +PROFILE_DYNAMIC_FULL = 3 +PROFILE_GRIP_NORMALLY = 4 +PROFILE_GRIP_GENTLY = 5 + +# Speed indices. +SPEED_SLOW = 0 +SPEED_MEDIUM = 1 +SPEED_FAST = 2 + +# Access2 status bits. +STATUS_INITIALIZED = 0x01 +STATUS_HOMED = 0x02 +STATUS_ESTOP_SET = 0x04 +STATUS_ESTOP_ACTIVE = 0x08 +STATUS_MOTOR_POWER_FAULT = 0x10 +STATUS_OPTICAL_PLATE_SENSOR = 0x20 + +# Access2 exposes a status byte for each axis. Bit 0 is used as the motion-complete +# indicator. The remaining bits are intentionally left opaque: Access2-specific +# meanings have not been verified and cannot safely be inferred from raw PIC-SERVO. +AXIS_STATUS_MOVE_DONE = 0x01 + +# Captured sensor word returned when no plate is present at the queried handoff. +SENSOR_NO_PLATE = 0x00000003 + + +class Access2ProtocolError(ValueError): + """Raised when an Access2 command, response, or FTDI envelope is invalid.""" + + +@dataclasses.dataclass(frozen=True) +class Access2Reply: + """A validated Access2 response with its command result removed.""" + + response_id: int + data: bytes + + +@dataclasses.dataclass(frozen=True) +class Access2Status: + """Decoded controller status and optional per-axis positions.""" + + access2_status: int + vspin_status: int + gripper_status: int | None = None + gripper_position: float | None = None + y_status: int | None = None + y_position: float | None = None + z_status: int | None = None + z_position: float | None = None + + @property + def initialized(self) -> bool: + return bool(self.access2_status & STATUS_INITIALIZED) + + @property + def homed(self) -> bool: + return bool(self.access2_status & STATUS_HOMED) + + @property + def estop_set(self) -> bool: + return bool(self.access2_status & STATUS_ESTOP_SET) + + @property + def estop_active(self) -> bool: + return bool(self.access2_status & STATUS_ESTOP_ACTIVE) + + @property + def motor_power_fault(self) -> bool: + return bool(self.access2_status & STATUS_MOTOR_POWER_FAULT) + + @property + def optical_plate_sensor(self) -> bool: + return bool(self.access2_status & STATUS_OPTICAL_PLATE_SENSOR) + + def axis_status(self, axis: int) -> int | None: + """Return the PIC-SERVO status byte for ``axis`` when full status is available.""" + if axis == AXIS_GRIPPER: + return self.gripper_status + if axis == AXIS_Y: + return self.y_status + if axis == AXIS_Z: + return self.z_status + raise ValueError(f"Unknown Access2 axis: {axis}") + + def axis_position(self, axis: int) -> float | None: + """Return the position for ``axis`` when full status is available.""" + if axis == AXIS_GRIPPER: + return self.gripper_position + if axis == AXIS_Y: + return self.y_position + if axis == AXIS_Z: + return self.z_position + raise ValueError(f"Unknown Access2 axis: {axis}") + + +def crc16_xmodem(data: bytes) -> int: + """Return CRC-16/CCITT-XMODEM for ``data``.""" + crc = 0 + for value in data: + crc ^= value << 8 + for _ in range(8): + if crc & 0x8000: + crc = ((crc << 1) ^ 0x1021) & 0xFFFF + else: + crc = (crc << 1) & 0xFFFF + return crc + + +def build_command(command_id: int, data: bytes = b"") -> bytes: + """Build the transport-independent Access2 command frame.""" + _validate_u8(command_id, "command_id") + if len(data) > 0xFFFF: + raise ValueError(f"Access2 payload is too long: {len(data)} bytes") + return Writer().u8(command_id).u16(len(data)).raw_bytes(data).finish() + + +def build_ftdi_frame(command: bytes) -> bytes: + """Wrap one Access2 command in the current PLR FTDI envelope.""" + if not 3 <= len(command) <= MAX_INNER_FRAME_LENGTH: + raise ValueError( + f"Access2 command must contain from 3 through {MAX_INNER_FRAME_LENGTH} bytes, " + f"got {len(command)}" + ) + body = ( + Writer(little_endian=False) + .u8(VELOCITY11_HEADER) + .u8(VELOCITY11_PACKET_TYPE) + .u16(len(command)) + .u8(VELOCITY11_CHANNEL) + .raw_bytes(command) + .finish() + ) + return body + Writer(little_endian=False).u16(crc16_xmodem(body)).finish() + + +def parse_ftdi_header(header: bytes) -> int: + """Validate a five-byte FTDI header and return its inner-frame length.""" + if len(header) != 5: + raise Access2ProtocolError(f"Access2 FTDI header has {len(header)} bytes, expected 5") + reader = Reader(header, little_endian=False) + header_byte = reader.u8() + packet_type = reader.u8() + inner_length = reader.u16() + channel = reader.u8() + if header_byte != VELOCITY11_HEADER or packet_type != VELOCITY11_PACKET_TYPE: + raise Access2ProtocolError( + f"Unexpected Access2 FTDI header 0x{header_byte:02x} 0x{packet_type:02x}" + ) + if channel != VELOCITY11_CHANNEL: + raise Access2ProtocolError(f"Unexpected Access2 FTDI channel 0x{channel:02x}") + if inner_length > MAX_INNER_FRAME_LENGTH: + raise Access2ProtocolError( + f"Access2 FTDI inner frame exceeds {MAX_INNER_FRAME_LENGTH} bytes: {inner_length}" + ) + return inner_length + + +def parse_ftdi_frame(frame: bytes) -> bytes: + """Validate an Access2 FTDI envelope and return its inner frame.""" + if len(frame) < 10: + raise Access2ProtocolError(f"Access2 FTDI frame is too short: {frame.hex()}") + inner_length = parse_ftdi_header(frame[:5]) + expected_length = inner_length + 7 + if len(frame) != expected_length: + raise Access2ProtocolError( + f"Access2 FTDI frame has {len(frame)} bytes, expected {expected_length}" + ) + reader = Reader(frame[5:], little_endian=False) + inner = reader.raw_bytes(inner_length) + received_crc = reader.u16() + expected_crc = crc16_xmodem(frame[:-2]) + if received_crc != expected_crc: + raise Access2ProtocolError( + f"Access2 FTDI CRC mismatch: received 0x{received_crc:04x}, expected 0x{expected_crc:04x}" + ) + return inner + + +def parse_reply(frame: bytes, request_id: int) -> Access2Reply: + """Parse an inner Access2 response and validate its response ID and result.""" + _validate_u8(request_id, "request_id") + if len(frame) < 4: + raise Access2ProtocolError(f"Access2 response is too short: {frame.hex()}") + reader = Reader(frame) + response_id = reader.u8() + data_length = reader.u16() + data = reader.remaining() + if len(data) != data_length: + raise Access2ProtocolError( + f"Access2 response has {len(data)} data bytes, expected {data_length}" + ) + if not data: + raise Access2ProtocolError("Access2 response does not contain a command result byte") + expected_response_id = (request_id + 1) & 0xFF + if response_id != expected_response_id: + raise Access2ProtocolError( + f"Access2 response ID is 0x{response_id:02x}, expected 0x{expected_response_id:02x}" + ) + result = Reader(data).u8() + if result != 0: + raise Access2ProtocolError( + f"Access2 command 0x{request_id:02x} failed with result 0x{result:02x}" + ) + return Access2Reply(response_id=response_id, data=data[1:]) + + +def parse_ftdi_reply(frame: bytes, request_id: int) -> Access2Reply: + """Parse an FTDI-wrapped reply to ``request_id``.""" + return parse_reply(parse_ftdi_frame(frame), request_id) + + +def decode_status(data: bytes) -> Access2Status: + """Decode either the short or full Access2 status payload.""" + if len(data) < 4: + raise Access2ProtocolError(f"Access2 status has only {len(data)} bytes") + reader = Reader(data) + access2_status = reader.u8() + vspin_status = reader.u8() + if len(data) == 4: + return Access2Status(access2_status=access2_status, vspin_status=vspin_status) + if len(data) < 17: + raise Access2ProtocolError( + f"Access2 status has {len(data)} bytes, expected either 4 or at least 17" + ) + return Access2Status( + access2_status=access2_status, + vspin_status=vspin_status, + gripper_status=reader.u8(), + gripper_position=reader.f32(), + y_status=reader.u8(), + y_position=reader.f32(), + z_status=reader.u8(), + z_position=reader.f32(), + ) + + +def decode_sensor_values(data: bytes) -> int: + """Decode the Access2 sensor bit word.""" + if len(data) != 4: + raise Access2ProtocolError(f"Access2 sensor response has {len(data)} bytes, expected 4") + return Reader(data).u32() + + +def decode_firmware_version(data: bytes) -> str: + """Decode the controller's null-padded ASCII firmware version.""" + return data.rstrip(b"\x00").decode("ascii", errors="replace") + + +def decode_hardware_version(data: bytes) -> int: + """Decode the controller's signed 16-bit hardware version.""" + if len(data) < 2: + raise Access2ProtocolError( + f"Access2 hardware version has {len(data)} bytes, expected at least 2" + ) + return Reader(data).i16() + + +def build_ping(data: bytes = b"") -> bytes: + return build_command(PING, data) + + +def build_get_firmware_version() -> bytes: + return build_command(GET_FIRMWARE_VERSION) + + +def build_get_hardware_version() -> bytes: + return build_command(GET_HARDWARE_VERSION) + + +def build_initialize() -> bytes: + return build_command(INITIALIZE) + + +def build_close() -> bytes: + return build_command(CLOSE) + + +def build_get_status() -> bytes: + return build_command(GET_STATUS) + + +def build_home() -> bytes: + return build_command(HOME) + + +def build_get_sensor_values() -> bytes: + return build_command(GET_SENSOR_VALUES) + + +def build_read_flash(address: int, length: int) -> bytes: + _validate_u16(address, "address") + _validate_u16(length, "length") + return build_command(READ_FLASH, Writer().u16(address).u16(length).finish()) + + +def build_move_to_teachpoint( + teachpoint: int, + z_offset: float, + plate_height: float, + profile: int = PROFILE_DYNAMIC_EMPTY, + speed: int = SPEED_SLOW, +) -> bytes: + for value, name in ((teachpoint, "teachpoint"), (profile, "profile"), (speed, "speed")): + _validate_u8(value, name) + data = Writer().u8(teachpoint).f32(z_offset).f32(plate_height).u8(profile).u8(speed).finish() + return build_command(MOVE_TO_LOCATION, data) + + +def build_move_axis_to_position( + axis: int, + position: float, + profile: int = PROFILE_DYNAMIC_EMPTY, + speed: int = SPEED_SLOW, +) -> bytes: + for value, name in ((axis, "axis"), (profile, "profile"), (speed, "speed")): + _validate_u8(value, name) + data = Writer().u8(axis).f32(position).u8(profile).u8(speed).finish() + return build_command(MOVE_TO_POSITION, data) + + +def build_jog_axis( + axis: int, + displacement: float, + profile: int = PROFILE_DYNAMIC_EMPTY, + speed: int = SPEED_SLOW, +) -> bytes: + for value, name in ((axis, "axis"), (profile, "profile"), (speed, "speed")): + _validate_u8(value, name) + data = Writer().u8(axis).f32(displacement).u8(profile).u8(speed).finish() + return build_command(JOG_AXIS, data) + + +def _validate_u8(value: int, name: str) -> None: + if not 0 <= value <= 0xFF: + raise ValueError(f"{name} must fit in an unsigned 8-bit integer") + + +def _validate_u16(value: int, name: str) -> None: + if not 0 <= value <= 0xFFFF: + raise ValueError(f"{name} must fit in an unsigned 16-bit integer") diff --git a/pylabrobot/agilent/vspin/_nmc.py b/pylabrobot/agilent/vspin/_nmc.py new file mode 100644 index 00000000000..cf678d2610b --- /dev/null +++ b/pylabrobot/agilent/vspin/_nmc.py @@ -0,0 +1,654 @@ +"""JR Kerr NMC protocol primitives used by the Agilent VSpin. + +The VSpin contains a PIC-SERVO module for the rotor and a PIC-IO module for +the door, bucket lock, and safety signals. Commands share this frame shape:: + + 0xAA | module address | (payload length << 4) | command | payload | checksum + +The response shape is determined by the status mask configured for each +module. Responses do not have a delimiter. +""" + +from __future__ import annotations + +import dataclasses +import math +from typing import Optional + +from pylabrobot.io.binary import Reader, Writer + +SYNC_BYTE = 0xAA + +PIC_SERVO_ADDRESS = 0x01 +PIC_IO_ADDRESS = 0x02 +GROUP_ADDRESS = 0xFF + +PIC_SERVO_MODULE_TYPE = 0 +PIC_IO_MODULE_TYPE = 2 + +# NMC command codes. The command occupies the low nibble of the command byte. +CMD_RESET_POSITION = 0x0 +CMD_SET_IO_DIRECTION = 0x0 +CMD_SET_ADDRESS = 0x1 +CMD_DEFINE_STATUS = 0x2 +CMD_READ_STATUS = 0x3 +CMD_LOAD_TRAJECTORY = 0x4 +CMD_START_MOTION = 0x5 +CMD_SET_OUTPUT = 0x6 +CMD_SET_GAIN = 0x6 +CMD_STOP_MOTOR = 0x7 +CMD_IO_CONTROL = 0x8 +CMD_SET_HOMING = 0x9 +CMD_SET_BAUD = 0xA +CMD_CLEAR_BITS = 0xB +CMD_NO_OP = 0xE +CMD_HARD_RESET = 0xF + +# PIC-SERVO LOAD_TRAJECTORY mode bits. +LOAD_POSITION = 0x01 +LOAD_VELOCITY = 0x02 +LOAD_ACCELERATION = 0x04 +LOAD_PWM = 0x08 +ENABLE_SERVO = 0x10 +VELOCITY_MODE = 0x20 +START_NOW = 0x80 + +# PIC-SERVO STOP_MOTOR mode bits. +AMPLIFIER_ENABLE = 0x01 +MOTOR_OFF = 0x02 +STOP_ABRUPT = 0x04 +STOP_SMOOTH = 0x08 +STOP_HERE = 0x10 + +# PIC-SERVO status-mask fields. +SEND_POSITION = 0x01 +SEND_ANALOG = 0x02 +SEND_VELOCITY = 0x04 +SEND_AUXILIARY = 0x08 +SEND_HOME = 0x10 +SEND_MODULE_ID = 0x20 +SEND_POSITION_ERROR = 0x40 +SEND_PATH_POINTS = 0x80 + +# PIC-IO status-mask fields. Some values overlap the PIC-SERVO fields. +SEND_INPUTS = 0x01 +SEND_ANALOG_1 = 0x02 +SEND_ANALOG_2 = 0x04 +SEND_ANALOG_3 = 0x08 +SEND_TIMER = 0x10 +SEND_SYNC_INPUTS = 0x40 +SEND_SYNC_TIMER = 0x80 + +# Response status-byte bits. +STATUS_MOVE_DONE = 0x01 +STATUS_CHECKSUM_ERROR = 0x02 +STATUS_OVERCURRENT = 0x04 +STATUS_POWER_ON = 0x08 +STATUS_POSITION_ERROR = 0x10 +STATUS_LIMIT_1 = 0x20 +STATUS_LIMIT_2 = 0x40 +STATUS_HOMING_IN_PROGRESS = 0x80 + +# VSpin PIC-IO input-bit indices and output-bit indices. +INPUT_AMPLIFIER_FAULT = 0 +INPUT_SPINNING = 1 +INPUT_IMBALANCE = 2 +INPUT_BUCKET_UNLOCKED = 3 +INPUT_BUCKET_LOCKED = 4 +INPUT_DOOR_OPEN = 6 +INPUT_DOOR_LOCKED = 7 +INPUT_AMPLIFIER_ENABLED = 11 + +OUTPUT_VERSION_TOGGLE = 5 +OUTPUT_BUCKET_LOCK_CYLINDER = 8 +OUTPUT_DOOR_CYLINDER = 9 +OUTPUT_DOOR_LOCK_CYLINDER = 10 + +# VSpin trajectory constants. +COUNTS_PER_REVOLUTION = 8000 +DEFAULT_ROTOR_RADIUS_CM = 10.0 +DEFAULT_MAX_VELOCITY_RPM = 3000.0 +NMC_VELOCITY_PER_RPM = 4473.925 +NMC_ACCELERATION_AT_FULL_SCALE = 916.19328 +NOMINAL_MAX_ACCELERATION_RPM_PER_SECOND = 400.0 +DEFAULT_SPIN_TARGET_HEADROOM = 5.0 + +BAUD_RATE_CODES = { + 19200: 63, + 57600: 20, + 115200: 10, +} + + +class NMCProtocolError(ValueError): + """Raised when an NMC frame or response is malformed.""" + + +@dataclasses.dataclass(frozen=True) +class NMCResponse: + """A checksum-verified NMC response.""" + + status: int + data: bytes + + +@dataclasses.dataclass(frozen=True) +class ServoStatus: + """Decoded PIC-SERVO response fields selected by a status mask.""" + + status: int + position: Optional[int] = None + analog: Optional[int] = None + velocity: Optional[int] = None + auxiliary: Optional[int] = None + home_position: Optional[int] = None + module_type: Optional[int] = None + module_version: Optional[int] = None + position_error: Optional[int] = None + path_points: Optional[int] = None + + +@dataclasses.dataclass(frozen=True) +class IOStatus: + """Decoded PIC-IO response fields selected by a status mask.""" + + status: int + inputs: Optional[int] = None + analog_1: Optional[int] = None + analog_2: Optional[int] = None + analog_3: Optional[int] = None + timer: Optional[int] = None + module_type: Optional[int] = None + module_version: Optional[int] = None + sync_inputs: Optional[int] = None + sync_timer: Optional[int] = None + + +@dataclasses.dataclass(frozen=True) +class ServoGains: + """PIC-SERVO gain values in controller-native units.""" + + proportional: int + derivative: int + integral: int + integration_limit: int + output_limit: int + current_limit: int + position_error_limit: int + servo_rate: int + deadband: int + + +def build_command(address: int, command: int, data: bytes = b"") -> bytes: + """Build one NMC command frame. + + Args: + address: Module address from 0 through 32, or the group address ``0xFF``. + command: Four-bit NMC command code. + data: Command payload of at most 15 bytes. + + Returns: + A complete command including sync byte and checksum. + """ + if not (0 <= address <= 32 or address == GROUP_ADDRESS): + raise ValueError(f"NMC address must be from 0 through 32 or 0xFF, got {address}") + if not 0 <= command <= 0x0F: + raise ValueError(f"NMC command must fit in four bits, got {command}") + if len(data) > 0x0F: + raise ValueError(f"NMC payload must contain at most 15 bytes, got {len(data)}") + + command_byte = (len(data) << 4) | command + body = bytes([address, command_byte]) + data + return bytes([SYNC_BYTE]) + body + bytes([sum(body) & 0xFF]) + + +def parse_response(frame: bytes, expected_data_length: int) -> NMCResponse: + """Parse and checksum one fixed-length NMC response.""" + if expected_data_length < 0: + raise ValueError("expected_data_length must not be negative") + expected_frame_length = expected_data_length + 2 + if len(frame) != expected_frame_length: + raise NMCProtocolError( + f"NMC response has {len(frame)} bytes, expected {expected_frame_length}: {frame.hex()}" + ) + + status = frame[0] + data = frame[1:-1] + checksum = frame[-1] + expected_checksum = (status + sum(data)) & 0xFF + if checksum != expected_checksum: + raise NMCProtocolError( + "NMC response checksum mismatch: " + f"received 0x{checksum:02x}, expected 0x{expected_checksum:02x}; " + f"response was {frame.hex()}" + ) + if status & STATUS_CHECKSUM_ERROR: + raise NMCProtocolError(f"NMC module rejected the command: status 0x{status:02x}") + return NMCResponse(status=status, data=data) + + +def servo_status_data_length(mask: int) -> int: + """Return the PIC-SERVO response data length for ``mask``.""" + _validate_status_mask(mask) + lengths = ( + (SEND_POSITION, 4), + (SEND_ANALOG, 1), + (SEND_VELOCITY, 2), + (SEND_AUXILIARY, 1), + (SEND_HOME, 4), + (SEND_MODULE_ID, 2), + (SEND_POSITION_ERROR, 2), + (SEND_PATH_POINTS, 1), + ) + return sum(length for bit, length in lengths if mask & bit) + + +def io_status_data_length(mask: int) -> int: + """Return the PIC-IO response data length for ``mask``.""" + _validate_status_mask(mask) + lengths = ( + (SEND_INPUTS, 2), + (SEND_ANALOG_1, 1), + (SEND_ANALOG_2, 1), + (SEND_ANALOG_3, 1), + (SEND_TIMER, 4), + (SEND_MODULE_ID, 2), + (SEND_SYNC_INPUTS, 2), + (SEND_SYNC_TIMER, 4), + ) + return sum(length for bit, length in lengths if mask & bit) + + +def parse_servo_status(frame: bytes, mask: int) -> ServoStatus: + """Parse a PIC-SERVO response using its active status mask.""" + response = parse_response(frame, servo_status_data_length(mask)) + return decode_servo_status(response, mask) + + +def decode_servo_status(response: NMCResponse, mask: int) -> ServoStatus: + """Decode a checksum-verified PIC-SERVO response using its status mask.""" + expected_length = servo_status_data_length(mask) + if len(response.data) != expected_length: + raise NMCProtocolError( + f"PIC-SERVO status has {len(response.data)} data bytes, expected {expected_length}" + ) + reader = Reader(response.data) + + position = None + analog = None + velocity = None + auxiliary = None + home_position = None + module_type = None + module_version = None + position_error = None + path_points = None + + if mask & SEND_POSITION: + position = reader.i32() + if mask & SEND_ANALOG: + analog = reader.u8() + if mask & SEND_VELOCITY: + velocity = reader.i16() + if mask & SEND_AUXILIARY: + auxiliary = reader.u8() + if mask & SEND_HOME: + home_position = reader.i32() + if mask & SEND_MODULE_ID: + module_type = reader.u8() + module_version = reader.u8() + if mask & SEND_POSITION_ERROR: + position_error = reader.i16() + if mask & SEND_PATH_POINTS: + path_points = reader.u8() + + return ServoStatus( + status=response.status, + position=position, + analog=analog, + velocity=velocity, + auxiliary=auxiliary, + home_position=home_position, + module_type=module_type, + module_version=module_version, + position_error=position_error, + path_points=path_points, + ) + + +def parse_io_status(frame: bytes, mask: int) -> IOStatus: + """Parse a PIC-IO response using its active status mask.""" + response = parse_response(frame, io_status_data_length(mask)) + return decode_io_status(response, mask) + + +def decode_io_status(response: NMCResponse, mask: int) -> IOStatus: + """Decode a checksum-verified PIC-IO response using its status mask.""" + expected_length = io_status_data_length(mask) + if len(response.data) != expected_length: + raise NMCProtocolError( + f"PIC-IO status has {len(response.data)} data bytes, expected {expected_length}" + ) + reader = Reader(response.data) + + inputs = None + analog_1 = None + analog_2 = None + analog_3 = None + timer = None + module_type = None + module_version = None + sync_inputs = None + sync_timer = None + + if mask & SEND_INPUTS: + inputs = reader.u16() + if mask & SEND_ANALOG_1: + analog_1 = reader.u8() + if mask & SEND_ANALOG_2: + analog_2 = reader.u8() + if mask & SEND_ANALOG_3: + analog_3 = reader.u8() + if mask & SEND_TIMER: + timer = reader.u32() + if mask & SEND_MODULE_ID: + module_type = reader.u8() + module_version = reader.u8() + if mask & SEND_SYNC_INPUTS: + sync_inputs = reader.u16() + if mask & SEND_SYNC_TIMER: + sync_timer = reader.u32() + + return IOStatus( + status=response.status, + inputs=inputs, + analog_1=analog_1, + analog_2=analog_2, + analog_3=analog_3, + timer=timer, + module_type=module_type, + module_version=module_version, + sync_inputs=sync_inputs, + sync_timer=sync_timer, + ) + + +def build_set_address(address: int, group_address: int = GROUP_ADDRESS) -> bytes: + """Build an address-assignment command for the next unaddressed module.""" + return build_command(0, CMD_SET_ADDRESS, bytes([address, group_address])) + + +def build_define_status(address: int, mask: int) -> bytes: + """Build a command that sets the module's response status mask.""" + _validate_status_mask(mask) + return build_command(address, CMD_DEFINE_STATUS, bytes([mask])) + + +def build_read_status(address: int, mask: int) -> bytes: + """Build a one-time status read using ``mask``.""" + _validate_status_mask(mask) + return build_command(address, CMD_READ_STATUS, bytes([mask])) + + +def build_no_op(address: int) -> bytes: + """Build a no-op command, normally used to request current status.""" + return build_command(address, CMD_NO_OP) + + +def build_set_baud(baud_rate: int) -> bytes: + """Build a group command that changes the NMC bus baud rate.""" + try: + code = BAUD_RATE_CODES[baud_rate] + except KeyError as exc: + supported = ", ".join(str(rate) for rate in sorted(BAUD_RATE_CODES)) + raise ValueError(f"unsupported NMC baud rate {baud_rate}; expected one of {supported}") from exc + return build_command(GROUP_ADDRESS, CMD_SET_BAUD, bytes([code])) + + +def build_hard_reset() -> bytes: + """Build an NMC group hard-reset command.""" + return build_command(GROUP_ADDRESS, CMD_HARD_RESET) + + +def build_set_output(address: int, output_word: int) -> bytes: + """Build a PIC-IO output-word command.""" + _validate_u16(output_word, "output_word") + return build_command(address, CMD_SET_OUTPUT, Writer().u16(output_word).finish()) + + +def build_set_io_direction(address: int, direction_word: int) -> bytes: + """Build a PIC-IO direction-word command.""" + _validate_u16(direction_word, "direction_word") + return build_command(address, CMD_SET_IO_DIRECTION, Writer().u16(direction_word).finish()) + + +def build_stop_motor(address: int, mode: int) -> bytes: + """Build a PIC-SERVO stop command.""" + _validate_u8(mode, "mode") + return build_command(address, CMD_STOP_MOTOR, bytes([mode])) + + +def build_clear_bits(address: int) -> bytes: + """Build a PIC-SERVO command that clears sticky status bits.""" + return build_command(address, CMD_CLEAR_BITS) + + +def build_reset_position(address: int) -> bytes: + """Build a PIC-SERVO command that resets the current position to zero.""" + return build_command(address, CMD_RESET_POSITION) + + +def build_set_homing(address: int, mode: int) -> bytes: + """Build a PIC-SERVO homing-mode command.""" + _validate_u8(mode, "mode") + return build_command(address, CMD_SET_HOMING, bytes([mode])) + + +def build_set_gain(address: int, gains: ServoGains) -> bytes: + """Build a PIC-SERVO gain command.""" + for value, name in ( + (gains.proportional, "proportional"), + (gains.derivative, "derivative"), + (gains.integral, "integral"), + (gains.integration_limit, "integration_limit"), + (gains.position_error_limit, "position_error_limit"), + ): + if not -0x8000 <= value <= 0x7FFF: + raise ValueError(f"{name} must fit in a signed 16-bit integer") + _validate_u8(gains.output_limit, "output_limit") + _validate_u8(gains.current_limit, "current_limit") + _validate_u8(gains.servo_rate, "servo_rate") + _validate_u8(gains.deadband, "deadband") + data = ( + Writer() + .i16(gains.proportional) + .i16(gains.derivative) + .i16(gains.integral) + .i16(gains.integration_limit) + .u8(gains.output_limit) + .u8(gains.current_limit) + .i16(gains.position_error_limit) + .u8(gains.servo_rate) + .u8(gains.deadband) + .finish() + ) + return build_command(address, CMD_SET_GAIN, data) + + +def build_load_trajectory( + address: int, + mode: int, + *, + position: Optional[int] = None, + velocity: Optional[int] = None, + acceleration: Optional[int] = None, + pwm: Optional[int] = None, +) -> bytes: + """Build a PIC-SERVO trajectory command from the fields selected by ``mode``.""" + _validate_u8(mode, "mode") + writer = Writer().u8(mode) + + if mode & LOAD_POSITION: + if position is None: + raise ValueError("position is required when LOAD_POSITION is set") + _validate_i32(position, "position") + writer.i32(position) + elif position is not None: + raise ValueError("position was provided but LOAD_POSITION is not set") + + if mode & LOAD_VELOCITY: + if velocity is None: + raise ValueError("velocity is required when LOAD_VELOCITY is set") + _validate_u32(velocity, "velocity") + writer.u32(velocity) + elif velocity is not None: + raise ValueError("velocity was provided but LOAD_VELOCITY is not set") + + if mode & LOAD_ACCELERATION: + if acceleration is None: + raise ValueError("acceleration is required when LOAD_ACCELERATION is set") + _validate_u32(acceleration, "acceleration") + writer.u32(acceleration) + elif acceleration is not None: + raise ValueError("acceleration was provided but LOAD_ACCELERATION is not set") + + if mode & LOAD_PWM: + if pwm is None: + raise ValueError("pwm is required when LOAD_PWM is set") + _validate_u8(pwm, "pwm") + writer.u8(pwm) + elif pwm is not None: + raise ValueError("pwm was provided but LOAD_PWM is not set") + + return build_command(address, CMD_LOAD_TRAJECTORY, writer.finish()) + + +def rcf_to_rpm(rcf: float, rotor_radius: float = DEFAULT_ROTOR_RADIUS_CM) -> float: + """Convert relative centrifugal force to RPM for a radius in centimeters.""" + if rcf < 0: + raise ValueError("rcf must not be negative") + if rotor_radius <= 0: + raise ValueError("rotor_radius must be greater than zero") + return math.sqrt(rcf / (1.118e-5 * rotor_radius)) + + +def rpm_to_rcf(rpm: float, rotor_radius: float = DEFAULT_ROTOR_RADIUS_CM) -> float: + """Convert RPM to relative centrifugal force for a radius in centimeters.""" + if rpm < 0: + raise ValueError("rpm must not be negative") + if rotor_radius <= 0: + raise ValueError("rotor_radius must be greater than zero") + return 1.118e-5 * rotor_radius * rpm**2 + + +def rpm_to_nmc_velocity(rpm: float, servo_rate: int = 1) -> int: + """Convert RPM to the unsigned NMC trajectory velocity field.""" + if rpm < 0: + raise ValueError("rpm must not be negative") + if servo_rate < 1: + raise ValueError("servo_rate must be at least 1") + return int(NMC_VELOCITY_PER_RPM * rpm * servo_rate) + + +def acceleration_to_nmc(acceleration: float, servo_rate: int = 1) -> int: + """Convert a PLR acceleration fraction to the NMC trajectory field.""" + _validate_fraction(acceleration, "acceleration") + if servo_rate < 1: + raise ValueError("servo_rate must be at least 1") + return int(NMC_ACCELERATION_AT_FULL_SCALE * servo_rate**2 * acceleration) + + +def acceleration_rpm_per_second(acceleration: float) -> float: + """Return nominal physical acceleration for a PLR acceleration fraction.""" + _validate_fraction(acceleration, "acceleration") + return NOMINAL_MAX_ACCELERATION_RPM_PER_SECOND * acceleration + + +def acceleration_counts_per_second_squared(acceleration: float) -> float: + """Return nominal encoder acceleration for a PLR acceleration fraction.""" + return acceleration_rpm_per_second(acceleration) * COUNTS_PER_REVOLUTION / 60.0 + + +def predicted_ramp_time(rpm: float, acceleration: float) -> float: + """Return nominal seconds required to ramp from zero to ``rpm``.""" + if rpm < 0: + raise ValueError("rpm must not be negative") + return rpm / acceleration_rpm_per_second(acceleration) + + +def acceleration_distance(rpm: float, acceleration: float) -> int: + """Return encoder counts traversed during a nominal zero-to-``rpm`` ramp.""" + if rpm < 0: + raise ValueError("rpm must not be negative") + velocity_counts_per_second = rpm * COUNTS_PER_REVOLUTION / 60.0 + distance = velocity_counts_per_second**2 / ( + 2.0 * acceleration_counts_per_second_squared(acceleration) + ) + return int(distance) + + +def spin_target_distance( + rpm: float, + duration: float, + acceleration: float, + headroom: float = DEFAULT_SPIN_TARGET_HEADROOM, +) -> int: + """Return the reference trajectory's deliberately distant target delta. + + The VSpin is stopped by a later zero-velocity trajectory, not by reaching + this position. The additional distance prevents the position target from + ending the spin before PLR commands deceleration. + """ + if rpm < 0: + raise ValueError("rpm must not be negative") + if duration < 0: + raise ValueError("duration must not be negative") + if headroom < 0: + raise ValueError("headroom must not be negative") + cruise_and_headroom = int(COUNTS_PER_REVOLUTION * rpm * (duration + headroom) / 60.0) + return cruise_and_headroom + 2 * acceleration_distance(rpm, acceleration) + + +def nearest_encoder_position( + current_position: int, + target_remainder: int, + counts_per_revolution: int = COUNTS_PER_REVOLUTION, +) -> int: + """Return the nearest absolute encoder position matching ``target_remainder``.""" + if counts_per_revolution <= 0: + raise ValueError("counts_per_revolution must be greater than zero") + target_remainder %= counts_per_revolution + current_remainder = current_position % counts_per_revolution + delta = (target_remainder - current_remainder) % counts_per_revolution + if delta > counts_per_revolution / 2: + delta -= counts_per_revolution + return current_position + delta + + +def _validate_status_mask(mask: int) -> None: + _validate_u8(mask, "status mask") + + +def _validate_fraction(value: float, name: str) -> None: + if not 0 < value <= 1: + raise ValueError(f"{name} must be greater than 0 and at most 1") + + +def _validate_u8(value: int, name: str) -> None: + if not 0 <= value <= 0xFF: + raise ValueError(f"{name} must fit in an unsigned byte") + + +def _validate_u16(value: int, name: str) -> None: + if not 0 <= value <= 0xFFFF: + raise ValueError(f"{name} must fit in an unsigned 16-bit integer") + + +def _validate_u32(value: int, name: str) -> None: + if not 0 <= value <= 0xFFFFFFFF: + raise ValueError(f"{name} must fit in an unsigned 32-bit integer") + + +def _validate_i32(value: int, name: str) -> None: + if not -(2**31) <= value <= 2**31 - 1: + raise ValueError(f"{name} must fit in a signed 32-bit integer") diff --git a/pylabrobot/agilent/vspin/access2.py b/pylabrobot/agilent/vspin/access2.py index 6f1e2757662..9cb9accbd6d 100644 --- a/pylabrobot/agilent/vspin/access2.py +++ b/pylabrobot/agilent/vspin/access2.py @@ -1,7 +1,7 @@ import asyncio import logging -import time +from pylabrobot.agilent.vspin import _access2_protocol as protocol from pylabrobot.agilent.vspin.errors import ( BucketHasPlateError, BucketNoPlateError, @@ -16,6 +16,16 @@ logger = logging.getLogger(__name__) +_MOTION_POLL_INTERVAL = 0.1 +_AXIS_POSITION_TOLERANCE = 0.1 +_DEFAULT_GRIPPER_OPEN_POSITION = 0.0 +_DEFAULT_GRIPPER_CLOSED_POSITION = 5.68 +_AXIS_NAMES: dict[int, str] = { + protocol.AXIS_GRIPPER: "gripper", + protocol.AXIS_Y: "Y", + protocol.AXIS_Z: "Z", +} + def _loader_load_event_context(self: "Access2") -> dict: plate = self.resource @@ -41,106 +51,408 @@ def _loader_unload_event_context(self: "Access2") -> dict: class Access2Driver: """FTDI driver for the Agilent Access2 centrifuge loader.""" - def __init__(self, device_id: str, timeout: int = 60): + def __init__( + self, + device_id: str, + timeout: int = 60, + gripper_open_position: float = _DEFAULT_GRIPPER_OPEN_POSITION, + gripper_closed_position: float = _DEFAULT_GRIPPER_CLOSED_POSITION, + ): """ Args: device_id: The libftdi id for the loader. Find using `python3 -m pylibftdi.examples.list_devices` + timeout: Communication and operation timeout in seconds. + gripper_open_position: Absolute gripper-axis position used when opening. + gripper_closed_position: Absolute gripper-axis position used when closing. """ super().__init__() self.io = FTDI(human_readable_device_name="Agilent Access2 Loader", device_id=device_id) self.timeout = timeout - - async def _read(self) -> bytes: - x = b"" - r = None - start = time.time() - while r != b"" or x == b"": - r = await self.io.read(1) - x += r - if r == b"": - await asyncio.sleep(0.1) - if x == b"" and (time.time() - start) > self.timeout: - raise TimeoutError("No data received within the specified timeout period") - return x - - async def send_command(self, command: bytes) -> bytes: - logger.debug("[loader] Sending %s", command.hex()) - await self.io.write(command) - return await self._read() + self.gripper_open_position = gripper_open_position + self.gripper_closed_position = gripper_closed_position + self._command_lock = asyncio.Lock() + self._operation_lock = asyncio.Lock() + + async def _read_exact(self, length: int) -> bytes: + loop = asyncio.get_running_loop() + deadline = loop.time() + self.timeout + response = bytearray() + while len(response) < length: + chunk = await self.io.read(length - len(response)) + if chunk: + response.extend(chunk) + continue + if loop.time() >= deadline: + raise TimeoutError( + f"Access2 sent {len(response)} of {length} expected bytes within " + f"{self.timeout} seconds: {bytes(response).hex()}" + ) + await asyncio.sleep(0) + return bytes(response) + + async def _read_frame(self) -> bytes: + header = await self._read_exact(5) + inner_length = protocol.parse_ftdi_header(header) + return header + await self._read_exact(inner_length + 2) + + async def send_command(self, command: bytes) -> protocol.Access2Reply: + """Send one transport-independent command through the FTDI envelope.""" + frame = protocol.build_ftdi_frame(command) + logger.debug("[loader] Sending %s", frame.hex()) + async with self._command_lock: + written = await self.io.write(frame) + if written != len(frame): + raise RuntimeError(f"Access2 wrote {written} of {len(frame)} command bytes") + response_frame = await self._read_frame() + logger.debug("[loader] Received %s", response_frame.hex()) + return protocol.parse_ftdi_reply(response_frame, request_id=command[0]) async def setup(self): logger.debug("[loader] setup") - - await self.io.setup() - await self.io.set_baudrate(115384) - - status = await self.request_status() - if not status.startswith(bytes.fromhex("1105")): - raise RuntimeError("Failed to get status") - - await self.send_command(bytes.fromhex("110500030014000072b1")) - await self.send_command(bytes.fromhex("1105000300100000ae71")) - await self.send_command(bytes.fromhex("110500070024040000008000be89")) - await self.send_command(bytes.fromhex("11050007002404008000800063b1")) - await self.send_command(bytes.fromhex("11050007002404000001800089b9")) - await self.send_command(bytes.fromhex("1105000700240400800180005481")) - await self.send_command(bytes.fromhex("110500070024040000024000c6bd")) - await self.send_command(bytes.fromhex("1105000300400000f0bf")) - await self.send_command(bytes.fromhex("1105000a004607000100000000020235bf")) - await self.send_command(bytes.fromhex("1105000e00440b00000000000000007041020203c7")) + async with self._operation_lock: + await self.io.setup() + await self.io.set_baudrate(115384) + + self._raise_on_fault(await self.request_status(), operation="setup precondition") + + await self.send_command(protocol.build_ping()) + await self.send_command(protocol.build_initialize()) + await self._home() + await self._move_axis_to_position( + protocol.AXIS_GRIPPER, + self.gripper_open_position, + profile=protocol.PROFILE_GRIP_NORMALLY, + speed=protocol.SPEED_FAST, + ) + await self._move_to_teachpoint( + protocol.TEACHPOINT_PARK, + 0, + 15, + profile=protocol.PROFILE_DYNAMIC_EMPTY, + speed=protocol.SPEED_FAST, + ) + await self._require_ready(operation="setup postcondition") async def stop(self): logger.debug("[loader] stop") await self.io.stop() - async def request_status(self) -> bytes: + async def request_status(self) -> protocol.Access2Status: logger.debug("[loader] request_status") - return await self.send_command(bytes.fromhex("11050003002000006bd4")) + response = await self.send_command(protocol.build_get_status()) + return protocol.decode_status(response.data) + + async def request_firmware_version(self) -> str: + """Return the Access2 controller firmware version.""" + response = await self.send_command(protocol.build_get_firmware_version()) + return protocol.decode_firmware_version(response.data) + + async def request_hardware_version(self) -> int: + """Return the Access2 controller hardware version.""" + response = await self.send_command(protocol.build_get_hardware_version()) + return protocol.decode_hardware_version(response.data) + + @staticmethod + def _raise_on_fault(status: protocol.Access2Status, *, operation: str | None = None) -> None: + context = "" if operation is None else f" during {operation}" + if status.estop_active or status.estop_set: + raise RuntimeError( + f"Access2 emergency stop is active{context} (status 0x{status.access2_status:02x})" + ) + if status.motor_power_fault: + raise RuntimeError( + f"Access2 motor power fault is active{context} (status 0x{status.access2_status:02x})" + ) - async def park(self): - logger.debug("[loader] park") - await self.send_command(bytes.fromhex("1105000e00440b0000000000410000704103007539")) + async def _require_ready(self, operation: str = "readiness check") -> protocol.Access2Status: + status = await self.request_status() + self._raise_on_fault(status, operation=operation) + if not status.initialized or not status.homed: + raise RuntimeError( + f"Access2 is not initialized and homed during {operation}: " + f"status 0x{status.access2_status:02x}" + ) + return status + + async def _home(self) -> protocol.Access2Status: + logger.debug("[loader] home") + await self.send_command(protocol.build_home()) + return await self._wait_until_homed() - async def close(self): - logger.debug("[loader] close") - await self.send_command(bytes.fromhex("1105000a00420700010000803f02008c64")) + async def home(self) -> None: + """Home all Access2 axes and wait for the controller to confirm completion.""" + async with self._operation_lock: + await self._home() - async def open(self): - logger.debug("[loader] open") - await self.send_command(bytes.fromhex("1105000a0042070001000080bf0200b73e")) + async def _wait_until_homed(self) -> protocol.Access2Status: + loop = asyncio.get_running_loop() + deadline = loop.time() + self.timeout + status = await self.request_status() + while True: + self._raise_on_fault(status, operation="homing") + if status.homed: + return status + if loop.time() >= deadline: + raise TimeoutError( + f"Access2 did not report homed within {self.timeout} seconds; " + f"last status was 0x{status.access2_status:02x}" + ) + await asyncio.sleep(0.1) + status = await self.request_status() + + @staticmethod + def _axis_name(axis: int) -> str: + try: + return _AXIS_NAMES[axis] + except KeyError as error: + raise ValueError(f"Unknown Access2 axis: {axis}") from error + + @staticmethod + def _gripper_is_at_position(status: protocol.Access2Status, position: float) -> bool: + return ( + status.gripper_status is not None + and bool(status.gripper_status & protocol.AXIS_STATUS_MOVE_DONE) + and status.gripper_position is not None + and abs(status.gripper_position - position) <= _AXIS_POSITION_TOLERANCE + ) + + async def _wait_until_motion_complete( + self, + axes: tuple[int, ...], + operation: str, + *, + target_axis: int | None = None, + target_position: float | None = None, + ) -> protocol.Access2Status: + """Wait for full status to confirm that the selected axes finished moving.""" + loop = asyncio.get_running_loop() + deadline = loop.time() + self.timeout + status = await self.request_status() + while True: + self._raise_on_fault(status, operation=operation) + if not status.initialized or not status.homed: + raise RuntimeError( + f"Access2 lost initialized/homed state during {operation}: " + f"status 0x{status.access2_status:02x}" + ) + + axis_details: list[str] = [] + motion_done = True + for axis in axes: + axis_status = status.axis_status(axis) + if axis_status is None: + raise RuntimeError( + f"Access2 cannot confirm {operation}: full axis status was not returned" + ) + axis_details.append(f"{self._axis_name(axis)}=0x{axis_status:02x}") + motion_done = motion_done and bool(axis_status & protocol.AXIS_STATUS_MOVE_DONE) + + position_matches = True + if target_axis is not None and target_position is not None: + position = status.axis_position(target_axis) + if position is None: + raise RuntimeError( + f"Access2 cannot confirm {operation}: full axis position was not returned" + ) + axis_details.append(f"{self._axis_name(target_axis)}={position:.3f} mm") + position_matches = abs(position - target_position) <= _AXIS_POSITION_TOLERANCE + + if motion_done and position_matches: + return status + if loop.time() >= deadline: + details = ", ".join(axis_details) + raise TimeoutError( + f"Access2 did not complete {operation} within {self.timeout} seconds; " + f"last status: {details}" + ) + await asyncio.sleep(_MOTION_POLL_INTERVAL) + status = await self.request_status() + + async def _move_to_teachpoint( + self, + teachpoint: int, + z_offset: float, + plate_height: float, + profile: int = protocol.PROFILE_DYNAMIC_EMPTY, + speed: int = protocol.SPEED_SLOW, + ) -> None: + await self.send_command( + protocol.build_move_to_teachpoint( + teachpoint, + z_offset, + plate_height, + profile, + speed, + ) + ) + await self._wait_until_motion_complete( + (protocol.AXIS_Y, protocol.AXIS_Z), + operation=f"move to teachpoint {teachpoint}", + ) + + async def _move_axis_to_position( + self, + axis: int, + position: float, + profile: int = protocol.PROFILE_DYNAMIC_EMPTY, + speed: int = protocol.SPEED_SLOW, + ) -> None: + await self.send_command(protocol.build_move_axis_to_position(axis, position, profile, speed)) + await self._wait_until_motion_complete( + (axis,), + operation=f"{self._axis_name(axis)} move to {position:.3f} mm", + target_axis=axis, + target_position=position, + ) + + async def _jog_axis( + self, + axis: int, + displacement: float, + profile: int = protocol.PROFILE_DYNAMIC_EMPTY, + speed: int = protocol.SPEED_SLOW, + ) -> None: + await self.send_command(protocol.build_jog_axis(axis, displacement, profile, speed)) + await self._wait_until_motion_complete( + (axis,), + operation=f"{self._axis_name(axis)} jog by {displacement:.3f} mm", + ) + + async def _tighten_grip(self) -> None: + """Move the gripper one relative step toward a tighter grip.""" + await self._jog_axis( + protocol.AXIS_GRIPPER, + 1, + protocol.PROFILE_DYNAMIC_EMPTY, + protocol.SPEED_SLOW, + ) + + async def _loosen_grip(self) -> None: + """Move the gripper one relative step toward a looser grip.""" + await self._jog_axis( + protocol.AXIS_GRIPPER, + -1, + protocol.PROFILE_DYNAMIC_EMPTY, + protocol.SPEED_SLOW, + ) + + async def request_sensor_values(self) -> int: + response = await self.send_command(protocol.build_get_sensor_values()) + return protocol.decode_sensor_values(response.data) + + async def park(self): + logger.debug("[loader] park") + async with self._operation_lock: + await self._require_ready(operation="park precondition") + await self._move_to_teachpoint( + protocol.TEACHPOINT_PARK, + 8, + 15, + profile=protocol.PROFILE_DYNAMIC_FULL, + speed=protocol.SPEED_SLOW, + ) + await self._require_ready(operation="park postcondition") + + async def close_gripper(self) -> None: + """Move the gripper to its normal closed position.""" + logger.debug("[loader] close gripper") + async with self._operation_lock: + status = await self._require_ready(operation="gripper-close precondition") + if self._gripper_is_at_position(status, self.gripper_closed_position): + return + await self._move_axis_to_position( + protocol.AXIS_GRIPPER, + self.gripper_closed_position, + profile=protocol.PROFILE_GRIP_NORMALLY, + ) + await self._require_ready(operation="gripper-close postcondition") + + async def open_gripper(self) -> None: + """Move the gripper to its open position.""" + logger.debug("[loader] open gripper") + async with self._operation_lock: + status = await self._require_ready(operation="gripper-open precondition") + if self._gripper_is_at_position(status, self.gripper_open_position): + return + await self._move_axis_to_position( + protocol.AXIS_GRIPPER, + self.gripper_open_position, + profile=protocol.PROFILE_GRIP_NORMALLY, + ) + await self._require_ready(operation="gripper-open postcondition") async def load(self): """Only tested for 1cm plate, 3mm pickup height.""" logger.debug("[loader] load") + async with self._operation_lock: + await self._require_ready(operation="load precondition") + + await self._move_axis_to_position( + protocol.AXIS_GRIPPER, + self.gripper_open_position, + profile=protocol.PROFILE_GRIP_NORMALLY, + speed=protocol.SPEED_FAST, + ) + await self._move_to_teachpoint(protocol.TEACHPOINT_PICK, 3, 10) - await self.send_command(bytes.fromhex("1105000a004607000100000000020235bf")) - await self.send_command(bytes.fromhex("1105000e00440b000100004040000020410200a5cb")) - - r = await self.send_command(bytes.fromhex("1105000300500000b3dc")) - if r == bytes.fromhex("1105000800510500000300000079f1"): - raise RuntimeError("no plate found on stage") + if await self.request_sensor_values() == protocol.SENSOR_NO_PLATE: + raise RuntimeError("no plate found on stage") - await self.send_command(bytes.fromhex("1105000a00460700018fc2b540020023dc")) - await self.send_command(bytes.fromhex("1105000e00440b000200004040000020410300ee00")) - await self.send_command(bytes.fromhex("1105000a004607000100000000020015fd")) - await self.send_command(bytes.fromhex("1105000e00440b0000000040400000204102007d82")) + await self._move_axis_to_position( + protocol.AXIS_GRIPPER, + self.gripper_closed_position, + profile=protocol.PROFILE_GRIP_NORMALLY, + ) + await self._move_to_teachpoint( + protocol.TEACHPOINT_BUCKET_1, + 3, + 10, + profile=protocol.PROFILE_DYNAMIC_FULL, + ) + await self._move_axis_to_position( + protocol.AXIS_GRIPPER, + self.gripper_open_position, + profile=protocol.PROFILE_GRIP_NORMALLY, + ) + await self._move_to_teachpoint(protocol.TEACHPOINT_PARK, 3, 10) + await self._require_ready(operation="load postcondition") async def unload(self): """Only tested for 1cm plate, 3mm pickup height.""" logger.debug("[loader] unload") + async with self._operation_lock: + await self._require_ready(operation="unload precondition") + + await self._move_axis_to_position( + protocol.AXIS_GRIPPER, + self.gripper_open_position, + profile=protocol.PROFILE_GRIP_NORMALLY, + speed=protocol.SPEED_FAST, + ) + await self._move_to_teachpoint(protocol.TEACHPOINT_BUCKET_1, 3, 10) - await self.send_command(bytes.fromhex("1105000a004607000100000000020235bf")) - await self.send_command(bytes.fromhex("1105000e00440b000200004040000020410200dd31")) - - r = await self.send_command(bytes.fromhex("1105000300500000b3dc")) - if r == bytes.fromhex("1105000800510500000300000079f1"): - raise RuntimeError("no plate found in centrifuge") + if await self.request_sensor_values() == protocol.SENSOR_NO_PLATE: + raise RuntimeError("no plate found in centrifuge") - await self.send_command(bytes.fromhex("1105000a00460700017b14b6400200d57a")) - await self.send_command(bytes.fromhex("1105000e00440b00010000404000002041030096fa")) - await self.send_command(bytes.fromhex("1105000a004607000100000000020015fd")) - await self.send_command(bytes.fromhex("1105000e00440b00000000000000002041020056be")) + await self._move_axis_to_position( + protocol.AXIS_GRIPPER, + self.gripper_closed_position, + profile=protocol.PROFILE_GRIP_NORMALLY, + ) + await self._move_to_teachpoint( + protocol.TEACHPOINT_PICK, + 3, + 10, + profile=protocol.PROFILE_DYNAMIC_FULL, + ) + await self._move_axis_to_position( + protocol.AXIS_GRIPPER, + self.gripper_open_position, + profile=protocol.PROFILE_GRIP_NORMALLY, + ) + await self._move_to_teachpoint(protocol.TEACHPOINT_PARK, 0, 10) + await self._require_ready(operation="unload postcondition") class Access2(ResourceHolder): @@ -154,8 +466,26 @@ def __init__( size_x: float = 0.0, size_y: float = 0.0, size_z: float = 0.0, + gripper_open_position: float = _DEFAULT_GRIPPER_OPEN_POSITION, + gripper_closed_position: float = _DEFAULT_GRIPPER_CLOSED_POSITION, ): - driver = Access2Driver(device_id=device_id) + """Create an Access2 loader with configurable absolute gripper positions. + + Args: + name: Resource name. + device_id: The libftdi identifier for the loader. + vspin: Paired VSpin centrifuge. + size_x: Resource width in millimeters. + size_y: Resource depth in millimeters. + size_z: Resource height in millimeters. + gripper_open_position: Absolute gripper-axis position used when opening. + gripper_closed_position: Absolute gripper-axis position used when closing. + """ + driver = Access2Driver( + device_id=device_id, + gripper_open_position=gripper_open_position, + gripper_closed_position=gripper_closed_position, + ) ResourceHolder.__init__( self, name=name, @@ -169,6 +499,15 @@ def __init__( self.driver: Access2Driver = driver self._vspin = vspin + async def _require_vspin_ready_for_transfer(self) -> None: + """Confirm the paired VSpin is physically safe for loader motion.""" + if not await self._vspin.request_door_open(): + raise CentrifugeDoorError("Centrifuge door-open sensor must be active for plate transfer.") + if not await self._vspin.request_bucket_locked(): + raise RuntimeError("Centrifuge bucket must be physically locked for plate transfer.") + if await self._vspin.request_spinning(): + raise RuntimeError("Centrifuge must be stopped for plate transfer.") + @evented_operation("centrifuge_loader.load", _loader_load_event_context) async def load(self) -> None: if not self._vspin.door_open: @@ -183,6 +522,8 @@ async def load(self) -> None: if self._vspin.at_bucket.resource is not None: raise BucketHasPlateError("Bucket must be empty to load a plate.") + await self._require_vspin_ready_for_transfer() + await self.driver.load() self._vspin.at_bucket.assign_child_resource(self.resource, location=Coordinate.zero()) @@ -199,6 +540,8 @@ async def unload(self) -> None: if self._vspin.at_bucket.resource is None: raise BucketNoPlateError("Bucket must have a plate to unload.") + await self._require_vspin_ready_for_transfer() + await self.driver.unload() self.assign_child_resource(self._vspin.at_bucket.resource) diff --git a/pylabrobot/agilent/vspin/access2_protocol_tests.py b/pylabrobot/agilent/vspin/access2_protocol_tests.py new file mode 100644 index 00000000000..1ff2486764a --- /dev/null +++ b/pylabrobot/agilent/vspin/access2_protocol_tests.py @@ -0,0 +1,231 @@ +import unittest + +from pylabrobot.agilent.vspin import _access2_protocol as protocol +from pylabrobot.io.binary import Writer + + +class Access2CommandTests(unittest.TestCase): + def test_status_ftdi_capture(self): + command = protocol.build_get_status() + + self.assertEqual(command.hex(), "200000") + self.assertEqual(protocol.build_ftdi_frame(command).hex(), "11050003002000006bd4") + + def test_setup_captures(self): + commands = ( + (protocol.build_ping(), "110500030014000072b1"), + (protocol.build_initialize(), "1105000300100000ae71"), + (protocol.build_read_flash(0, 128), "110500070024040000008000be89"), + (protocol.build_read_flash(128, 128), "11050007002404008000800063b1"), + (protocol.build_read_flash(256, 128), "11050007002404000001800089b9"), + (protocol.build_read_flash(384, 128), "1105000700240400800180005481"), + (protocol.build_read_flash(512, 64), "110500070024040000024000c6bd"), + (protocol.build_home(), "1105000300400000f0bf"), + ) + for command, expected in commands: + with self.subTest(command=command.hex()): + self.assertEqual(protocol.build_ftdi_frame(command).hex(), expected) + + def test_motion_captures(self): + self.assertEqual( + protocol.build_ftdi_frame( + protocol.build_move_axis_to_position( + protocol.AXIS_GRIPPER, + 0, + protocol.PROFILE_DYNAMIC_EMPTY, + protocol.SPEED_FAST, + ) + ).hex(), + "1105000a004607000100000000020235bf", + ) + self.assertEqual( + protocol.build_ftdi_frame( + protocol.build_move_to_teachpoint( + protocol.TEACHPOINT_PARK, + 0, + 15, + protocol.PROFILE_DYNAMIC_EMPTY, + protocol.SPEED_FAST, + ) + ).hex(), + "1105000e00440b00000000000000007041020203c7", + ) + + def test_load_motion_captures(self): + commands = ( + ( + protocol.build_move_to_teachpoint(protocol.TEACHPOINT_PICK, 3, 10), + "1105000e00440b000100004040000020410200a5cb", + ), + ( + protocol.build_move_axis_to_position(protocol.AXIS_GRIPPER, 5.68), + "1105000a00460700018fc2b540020023dc", + ), + ( + protocol.build_move_to_teachpoint( + protocol.TEACHPOINT_BUCKET_1, + 3, + 10, + protocol.PROFILE_DYNAMIC_FULL, + ), + "1105000e00440b000200004040000020410300ee00", + ), + ( + protocol.build_move_axis_to_position(protocol.AXIS_GRIPPER, 0), + "1105000a004607000100000000020015fd", + ), + ( + protocol.build_move_to_teachpoint(protocol.TEACHPOINT_PARK, 3, 10), + "1105000e00440b0000000040400000204102007d82", + ), + ) + for command, expected in commands: + with self.subTest(command=command.hex()): + self.assertEqual(protocol.build_ftdi_frame(command).hex(), expected) + + def test_unload_motion_captures(self): + commands = ( + ( + protocol.build_move_to_teachpoint(protocol.TEACHPOINT_BUCKET_1, 3, 10), + "1105000e00440b000200004040000020410200dd31", + ), + ( + protocol.build_move_axis_to_position(protocol.AXIS_GRIPPER, 5.69), + "1105000a00460700017b14b6400200d57a", + ), + ( + protocol.build_move_to_teachpoint( + protocol.TEACHPOINT_PICK, + 3, + 10, + protocol.PROFILE_DYNAMIC_FULL, + ), + "1105000e00440b00010000404000002041030096fa", + ), + ( + protocol.build_move_to_teachpoint(protocol.TEACHPOINT_PARK, 0, 10), + "1105000e00440b00000000000000002041020056be", + ), + ) + for command, expected in commands: + with self.subTest(command=command.hex()): + self.assertEqual(protocol.build_ftdi_frame(command).hex(), expected) + + def test_park_and_gripper_jog_captures(self): + commands = ( + ( + protocol.build_move_to_teachpoint( + protocol.TEACHPOINT_PARK, + 8, + 15, + protocol.PROFILE_DYNAMIC_FULL, + ), + "1105000e00440b0000000000410000704103007539", + ), + ( + protocol.build_jog_axis(protocol.AXIS_GRIPPER, 1), + "1105000a00420700010000803f02008c64", + ), + ( + protocol.build_jog_axis(protocol.AXIS_GRIPPER, -1), + "1105000a0042070001000080bf0200b73e", + ), + ) + for command, expected in commands: + with self.subTest(command=command.hex()): + self.assertEqual(protocol.build_ftdi_frame(command).hex(), expected) + + +class Access2ResponseTests(unittest.TestCase): + def test_parse_captured_no_plate_sensor_response(self): + frame = bytes.fromhex("1105000800510500000300000079f1") + + response = protocol.parse_ftdi_reply(frame, protocol.GET_SENSOR_VALUES) + + self.assertEqual(response.response_id, 0x51) + self.assertEqual(protocol.decode_sensor_values(response.data), protocol.SENSOR_NO_PLATE) + + def test_rejects_bad_crc(self): + frame = bytearray.fromhex("1105000800510500000300000079f1") + frame[-1] ^= 0xFF + + with self.assertRaisesRegex(protocol.Access2ProtocolError, "CRC mismatch"): + protocol.parse_ftdi_frame(bytes(frame)) + + def test_rejects_truncated_frame(self): + frame = bytes.fromhex("1105000800510500000300000079f1") + + with self.assertRaisesRegex(protocol.Access2ProtocolError, "bytes, expected"): + protocol.parse_ftdi_frame(frame[:-1]) + + def test_rejects_oversized_ftdi_header(self): + header = ( + Writer(little_endian=False) + .u8(protocol.VELOCITY11_HEADER) + .u8(protocol.VELOCITY11_PACKET_TYPE) + .u16(protocol.MAX_INNER_FRAME_LENGTH + 1) + .u8(protocol.VELOCITY11_CHANNEL) + .finish() + ) + + with self.assertRaisesRegex(protocol.Access2ProtocolError, "exceeds"): + protocol.parse_ftdi_header(header) + + def test_rejects_wrong_response_id(self): + inner = Writer().u8(0x52).u16(1).u8(0).finish() + + with self.assertRaisesRegex(protocol.Access2ProtocolError, "response ID"): + protocol.parse_reply(inner, protocol.GET_SENSOR_VALUES) + + def test_rejects_command_error(self): + inner = Writer().u8(0x51).u16(1).u8(7).finish() + + with self.assertRaisesRegex(protocol.Access2ProtocolError, "result 0x07"): + protocol.parse_reply(inner, protocol.GET_SENSOR_VALUES) + + def test_decode_full_status(self): + data = ( + Writer() + .u8(protocol.STATUS_INITIALIZED | protocol.STATUS_HOMED) + .u8(0x12) + .u8(1) + .f32(5.68) + .u8(2) + .f32(100.5) + .u8(3) + .f32(20.25) + .finish() + ) + + status = protocol.decode_status(data) + + self.assertTrue(status.initialized) + self.assertTrue(status.homed) + self.assertFalse(status.estop_active) + gripper_position = status.gripper_position + self.assertIsNotNone(gripper_position) + assert gripper_position is not None + self.assertAlmostEqual(gripper_position, 5.68, places=5) + self.assertEqual(status.y_position, 100.5) + self.assertEqual(status.z_position, 20.25) + self.assertEqual(status.axis_status(protocol.AXIS_GRIPPER), 1) + self.assertEqual(status.axis_status(protocol.AXIS_Y), 2) + self.assertEqual(status.axis_status(protocol.AXIS_Z), 3) + gripper_axis_position = status.axis_position(protocol.AXIS_GRIPPER) + self.assertIsNotNone(gripper_axis_position) + assert gripper_axis_position is not None + self.assertAlmostEqual(gripper_axis_position, 5.68, places=5) + self.assertEqual(status.axis_position(protocol.AXIS_Y), 100.5) + self.assertEqual(status.axis_position(protocol.AXIS_Z), 20.25) + + def test_rejects_partial_full_status(self): + with self.assertRaisesRegex(protocol.Access2ProtocolError, "either 4 or at least 17"): + protocol.decode_status(bytes(16)) + + def test_decode_versions(self): + self.assertEqual(protocol.decode_firmware_version(b"1.2.3\x00\x00"), "1.2.3") + self.assertEqual(protocol.decode_hardware_version(Writer().i16(-2).finish()), -2) + + def test_hardware_version_requires_signed_word(self): + with self.assertRaisesRegex(protocol.Access2ProtocolError, "expected at least 2"): + protocol.decode_hardware_version(b"\x01") diff --git a/pylabrobot/agilent/vspin/access2_tests.py b/pylabrobot/agilent/vspin/access2_tests.py new file mode 100644 index 00000000000..fe856b613f5 --- /dev/null +++ b/pylabrobot/agilent/vspin/access2_tests.py @@ -0,0 +1,652 @@ +import dataclasses +import unittest +from collections import deque +from unittest.mock import AsyncMock, call, patch + +from pylabrobot.agilent.vspin import _access2_protocol as protocol +from pylabrobot.agilent.vspin.access2 import Access2Driver +from pylabrobot.io.binary import Writer + + +_READY_FLAGS = protocol.STATUS_INITIALIZED | protocol.STATUS_HOMED + + +def _status(*, flags: int) -> protocol.Access2Status: + return protocol.Access2Status(access2_status=flags, vspin_status=0) + + +def _short_status_data(flags: int = _READY_FLAGS) -> bytes: + return Writer().u8(flags).u8(0).u8(0).u8(0).finish() + + +def _full_status_data( + *, + flags: int = _READY_FLAGS, + gripper_status: int = protocol.AXIS_STATUS_MOVE_DONE, + gripper_position: float = 0, + y_status: int = protocol.AXIS_STATUS_MOVE_DONE, + y_position: float = 100, + z_status: int = protocol.AXIS_STATUS_MOVE_DONE, + z_position: float = 20, +) -> bytes: + return ( + Writer() + .u8(flags) + .u8(0) + .u8(gripper_status) + .f32(gripper_position) + .u8(y_status) + .f32(y_position) + .u8(z_status) + .f32(z_position) + .finish() + ) + + +def _build_ftdi_reply(command: bytes, data: bytes = b"", result: int = 0) -> bytes: + inner = ( + Writer().u8((command[0] + 1) & 0xFF).u16(len(data) + 1).u8(result).raw_bytes(data).finish() + ) + return protocol.build_ftdi_frame(inner) + + +@dataclasses.dataclass(frozen=True) +class _ScriptStep: + command: bytes + response_data: bytes = b"" + result: int = 0 + + +class _ScriptedFTDI: + """Validate writes and replay partial FTDI reads from a fixed script.""" + + def __init__(self, steps: list[_ScriptStep], max_read_size: int = 3): + self._steps = deque(steps) + self._response = bytearray() + self._max_read_size = max_read_size + self.setup_called = False + self.stopped = False + self.baudrate: int | None = None + self.writes: list[bytes] = [] + + async def setup(self) -> None: + self.setup_called = True + + async def stop(self) -> None: + self.stopped = True + + async def set_baudrate(self, baudrate: int) -> None: + self.baudrate = baudrate + + async def write(self, data: bytes) -> int: + if self._response: + raise AssertionError(f"Access2 wrote before consuming response {self._response.hex()}") + if not self._steps: + raise AssertionError(f"Unexpected Access2 write: {data.hex()}") + step = self._steps.popleft() + expected = protocol.build_ftdi_frame(step.command) + if data != expected: + raise AssertionError(f"Access2 wrote {data.hex()}, expected {expected.hex()}") + self.writes.append(data) + self._response.extend(_build_ftdi_reply(step.command, step.response_data, step.result)) + return len(data) + + async def read(self, length: int) -> bytes: + count = min(length, self._max_read_size, len(self._response)) + if count == 0: + return b"" + chunk = bytes(self._response[:count]) + del self._response[:count] + return chunk + + def assert_complete(self, test: unittest.TestCase) -> None: + test.assertEqual(list(self._steps), []) + test.assertEqual(bytes(self._response), b"") + + +class Access2TransportTests(unittest.IsolatedAsyncioTestCase): + def setUp(self): + self.ftdi_patch = patch("pylabrobot.agilent.vspin.access2.FTDI", autospec=True) + ftdi_class = self.ftdi_patch.start() + self.addCleanup(self.ftdi_patch.stop) + self.io = ftdi_class.return_value + self.driver = Access2Driver(device_id="test", timeout=0) + + async def test_status_response_supports_partial_reads(self): + inner_response = ( + Writer().u8(protocol.GET_STATUS + 1).u16(5).u8(0).raw_bytes(_short_status_data()).finish() + ) + response = protocol.build_ftdi_frame(inner_response) + command_frame = protocol.build_ftdi_frame(protocol.build_get_status()) + self.io.write = AsyncMock(return_value=len(command_frame)) + self.io.read = AsyncMock(side_effect=[response[:2], response[2:5], response[5:8], response[8:]]) + + status = await self.driver.request_status() + + self.assertTrue(status.initialized) + self.assertTrue(status.homed) + self.io.write.assert_awaited_once_with(command_frame) + self.assertEqual( + [read.args[0] for read in self.io.read.await_args_list], + [5, 3, 10, 7], + ) + + async def test_partial_header_times_out_with_context(self): + self.io.read = AsyncMock(side_effect=[b"\x11\x05", b""]) + + with self.assertRaisesRegex(TimeoutError, "2 of 5 expected bytes"): + await self.driver._read_frame() + + +class Access2ScriptedFTDITests(unittest.IsolatedAsyncioTestCase): + def setUp(self): + self.ftdi_patch = patch("pylabrobot.agilent.vspin.access2.FTDI", autospec=True) + self.ftdi_patch.start() + self.addCleanup(self.ftdi_patch.stop) + + def _make_driver( + self, steps: list[_ScriptStep], *, timeout: int = 60 + ) -> tuple[Access2Driver, _ScriptedFTDI]: + driver = Access2Driver(device_id="test", timeout=timeout) + io = _ScriptedFTDI(steps) + driver.io = io # type: ignore[assignment] + return driver, io + + async def test_complete_setup_ftdi_transcript(self): + steps = [ + _ScriptStep(protocol.build_get_status(), _short_status_data(flags=0)), + _ScriptStep(protocol.build_ping()), + _ScriptStep(protocol.build_initialize()), + ] + steps.extend( + [ + _ScriptStep(protocol.build_home()), + _ScriptStep(protocol.build_get_status(), _full_status_data()), + _ScriptStep( + protocol.build_move_axis_to_position( + protocol.AXIS_GRIPPER, + 0, + protocol.PROFILE_GRIP_NORMALLY, + protocol.SPEED_FAST, + ) + ), + _ScriptStep(protocol.build_get_status(), _full_status_data(gripper_position=0)), + _ScriptStep( + protocol.build_move_to_teachpoint( + protocol.TEACHPOINT_PARK, + 0, + 15, + protocol.PROFILE_DYNAMIC_EMPTY, + protocol.SPEED_FAST, + ) + ), + _ScriptStep(protocol.build_get_status(), _full_status_data()), + _ScriptStep(protocol.build_get_status(), _short_status_data()), + ] + ) + driver, io = self._make_driver(steps) + + await driver.setup() + + io.assert_complete(self) + self.assertTrue(io.setup_called) + self.assertEqual(io.baudrate, 115384) + + async def test_complete_home_ftdi_transcript(self): + steps = [ + _ScriptStep(protocol.build_home()), + _ScriptStep(protocol.build_get_status(), _full_status_data()), + ] + driver, io = self._make_driver(steps) + + await driver.home() + + io.assert_complete(self) + + async def test_home_timeout_reports_last_status(self): + steps = [ + _ScriptStep(protocol.build_home()), + _ScriptStep( + protocol.build_get_status(), + _short_status_data(flags=protocol.STATUS_INITIALIZED), + ), + ] + driver, io = self._make_driver(steps, timeout=0) + + with self.assertRaisesRegex(TimeoutError, "last status was 0x01"): + await driver.home() + + io.assert_complete(self) + + async def test_complete_park_ftdi_transcript(self): + steps = [ + _ScriptStep(protocol.build_get_status(), _short_status_data()), + _ScriptStep( + protocol.build_move_to_teachpoint( + protocol.TEACHPOINT_PARK, + 8, + 15, + protocol.PROFILE_DYNAMIC_FULL, + protocol.SPEED_SLOW, + ) + ), + _ScriptStep(protocol.build_get_status(), _full_status_data()), + _ScriptStep(protocol.build_get_status(), _short_status_data()), + ] + driver, io = self._make_driver(steps) + + await driver.park() + + io.assert_complete(self) + + async def test_complete_load_ftdi_transcript(self): + steps = [ + _ScriptStep(protocol.build_get_status(), _short_status_data()), + _ScriptStep( + protocol.build_move_axis_to_position( + protocol.AXIS_GRIPPER, + 0, + protocol.PROFILE_GRIP_NORMALLY, + protocol.SPEED_FAST, + ) + ), + _ScriptStep(protocol.build_get_status(), _full_status_data(gripper_position=0)), + _ScriptStep(protocol.build_move_to_teachpoint(protocol.TEACHPOINT_PICK, 3, 10)), + _ScriptStep(protocol.build_get_status(), _full_status_data()), + _ScriptStep(protocol.build_get_sensor_values(), Writer().u32(0).finish()), + _ScriptStep( + protocol.build_move_axis_to_position( + protocol.AXIS_GRIPPER, + 5.68, + protocol.PROFILE_GRIP_NORMALLY, + ) + ), + _ScriptStep(protocol.build_get_status(), _full_status_data(gripper_position=5.68)), + _ScriptStep( + protocol.build_move_to_teachpoint( + protocol.TEACHPOINT_BUCKET_1, + 3, + 10, + protocol.PROFILE_DYNAMIC_FULL, + ) + ), + _ScriptStep(protocol.build_get_status(), _full_status_data()), + _ScriptStep( + protocol.build_move_axis_to_position( + protocol.AXIS_GRIPPER, + 0, + protocol.PROFILE_GRIP_NORMALLY, + ) + ), + _ScriptStep(protocol.build_get_status(), _full_status_data(gripper_position=0)), + _ScriptStep(protocol.build_move_to_teachpoint(protocol.TEACHPOINT_PARK, 3, 10)), + _ScriptStep(protocol.build_get_status(), _full_status_data()), + _ScriptStep(protocol.build_get_status(), _short_status_data()), + ] + driver, io = self._make_driver(steps) + + await driver.load() + + io.assert_complete(self) + + async def test_complete_unload_ftdi_transcript(self): + steps = [ + _ScriptStep(protocol.build_get_status(), _short_status_data()), + _ScriptStep( + protocol.build_move_axis_to_position( + protocol.AXIS_GRIPPER, + 0, + protocol.PROFILE_GRIP_NORMALLY, + protocol.SPEED_FAST, + ) + ), + _ScriptStep(protocol.build_get_status(), _full_status_data(gripper_position=0)), + _ScriptStep(protocol.build_move_to_teachpoint(protocol.TEACHPOINT_BUCKET_1, 3, 10)), + _ScriptStep(protocol.build_get_status(), _full_status_data()), + _ScriptStep(protocol.build_get_sensor_values(), Writer().u32(0).finish()), + _ScriptStep( + protocol.build_move_axis_to_position( + protocol.AXIS_GRIPPER, + 5.68, + protocol.PROFILE_GRIP_NORMALLY, + ) + ), + _ScriptStep(protocol.build_get_status(), _full_status_data(gripper_position=5.68)), + _ScriptStep( + protocol.build_move_to_teachpoint( + protocol.TEACHPOINT_PICK, + 3, + 10, + protocol.PROFILE_DYNAMIC_FULL, + ) + ), + _ScriptStep(protocol.build_get_status(), _full_status_data()), + _ScriptStep( + protocol.build_move_axis_to_position( + protocol.AXIS_GRIPPER, + 0, + protocol.PROFILE_GRIP_NORMALLY, + ) + ), + _ScriptStep(protocol.build_get_status(), _full_status_data(gripper_position=0)), + _ScriptStep(protocol.build_move_to_teachpoint(protocol.TEACHPOINT_PARK, 0, 10)), + _ScriptStep(protocol.build_get_status(), _full_status_data()), + _ScriptStep(protocol.build_get_status(), _short_status_data()), + ] + driver, io = self._make_driver(steps) + + await driver.unload() + + io.assert_complete(self) + + async def test_load_stops_after_captured_no_plate_response(self): + steps = [ + _ScriptStep(protocol.build_get_status(), _short_status_data()), + _ScriptStep( + protocol.build_move_axis_to_position( + protocol.AXIS_GRIPPER, + 0, + protocol.PROFILE_GRIP_NORMALLY, + protocol.SPEED_FAST, + ) + ), + _ScriptStep(protocol.build_get_status(), _full_status_data(gripper_position=0)), + _ScriptStep(protocol.build_move_to_teachpoint(protocol.TEACHPOINT_PICK, 3, 10)), + _ScriptStep(protocol.build_get_status(), _full_status_data()), + _ScriptStep( + protocol.build_get_sensor_values(), Writer().u32(protocol.SENSOR_NO_PLATE).finish() + ), + ] + driver, io = self._make_driver(steps) + + with self.assertRaisesRegex(RuntimeError, "no plate found on stage"): + await driver.load() + + io.assert_complete(self) + + async def test_motion_polls_until_axis_is_done_and_at_target(self): + command = protocol.build_move_axis_to_position(protocol.AXIS_GRIPPER, 5.68) + steps = [ + _ScriptStep(command), + _ScriptStep( + protocol.build_get_status(), + _full_status_data(gripper_status=0, gripper_position=1), + ), + _ScriptStep(protocol.build_get_status(), _full_status_data(gripper_position=5.68)), + ] + driver, io = self._make_driver(steps) + + with patch("pylabrobot.agilent.vspin.access2.asyncio.sleep", new=AsyncMock()): + await driver._move_axis_to_position(protocol.AXIS_GRIPPER, 5.68) + + io.assert_complete(self) + + async def test_motion_timeout_reports_last_axis_state(self): + command = protocol.build_move_axis_to_position(protocol.AXIS_GRIPPER, 5.68) + steps = [ + _ScriptStep(command), + _ScriptStep( + protocol.build_get_status(), + _full_status_data(gripper_status=0, gripper_position=1), + ), + ] + driver, io = self._make_driver(steps, timeout=0) + + with self.assertRaisesRegex(TimeoutError, "gripper=0x00, gripper=1.000 mm"): + await driver._move_axis_to_position(protocol.AXIS_GRIPPER, 5.68) + + io.assert_complete(self) + + async def test_estop_during_motion_prevents_follow_up_commands(self): + command = protocol.build_move_to_teachpoint(protocol.TEACHPOINT_PICK, 3, 10) + steps = [ + _ScriptStep(command), + _ScriptStep( + protocol.build_get_status(), + _full_status_data(flags=_READY_FLAGS | protocol.STATUS_ESTOP_ACTIVE), + ), + ] + driver, io = self._make_driver(steps) + + with self.assertRaisesRegex(RuntimeError, "emergency stop"): + await driver._move_to_teachpoint(protocol.TEACHPOINT_PICK, 3, 10) + + io.assert_complete(self) + + async def test_motor_fault_during_motion_names_failed_transition(self): + command = protocol.build_move_to_teachpoint(protocol.TEACHPOINT_PICK, 3, 10) + steps = [ + _ScriptStep(command), + _ScriptStep( + protocol.build_get_status(), + _full_status_data(flags=_READY_FLAGS | protocol.STATUS_MOTOR_POWER_FAULT), + ), + ] + driver, io = self._make_driver(steps) + + with self.assertRaisesRegex(RuntimeError, "during move to teachpoint 1"): + await driver._move_to_teachpoint(protocol.TEACHPOINT_PICK, 3, 10) + + io.assert_complete(self) + + async def test_unverified_axis_status_bits_are_not_treated_as_faults(self): + command = protocol.build_move_to_teachpoint(protocol.TEACHPOINT_PICK, 3, 10) + steps = [ + _ScriptStep(command), + _ScriptStep( + protocol.build_get_status(), + _full_status_data(y_status=0x13), + ), + ] + driver, io = self._make_driver(steps) + + await driver._move_to_teachpoint(protocol.TEACHPOINT_PICK, 3, 10) + + io.assert_complete(self) + + async def test_motion_requires_full_axis_status(self): + command = protocol.build_move_to_teachpoint(protocol.TEACHPOINT_PICK, 3, 10) + steps = [ + _ScriptStep(command), + _ScriptStep(protocol.build_get_status(), _short_status_data()), + ] + driver, io = self._make_driver(steps) + + with self.assertRaisesRegex(RuntimeError, "full axis status was not returned"): + await driver._move_to_teachpoint(protocol.TEACHPOINT_PICK, 3, 10) + + io.assert_complete(self) + + async def test_version_queries_use_ftdi_protocol(self): + steps = [ + _ScriptStep(protocol.build_get_firmware_version(), b"1.2.3\x00"), + _ScriptStep(protocol.build_get_hardware_version(), Writer().i16(7).finish()), + ] + driver, io = self._make_driver(steps) + + self.assertEqual(await driver.request_firmware_version(), "1.2.3") + self.assertEqual(await driver.request_hardware_version(), 7) + + io.assert_complete(self) + + +class Access2WorkflowTests(unittest.IsolatedAsyncioTestCase): + def setUp(self): + self.ftdi_patch = patch("pylabrobot.agilent.vspin.access2.FTDI", autospec=True) + self.ftdi_patch.start() + self.addCleanup(self.ftdi_patch.stop) + self.driver = Access2Driver(device_id="test") + self.driver._move_axis_to_position = AsyncMock() # type: ignore[method-assign] + self.driver._move_to_teachpoint = AsyncMock() # type: ignore[method-assign] + + async def test_gripper_state_methods_use_absolute_positions(self): + ready = _status(flags=_READY_FLAGS) + self.driver.request_status = AsyncMock( # type: ignore[method-assign] + side_effect=[ready, ready, ready, ready] + ) + + await self.driver.close_gripper() + await self.driver.open_gripper() + + self.driver._move_axis_to_position.assert_has_awaits( # type: ignore[attr-defined] + [ + call( + protocol.AXIS_GRIPPER, + 5.68, + profile=protocol.PROFILE_GRIP_NORMALLY, + ), + call( + protocol.AXIS_GRIPPER, + 0.0, + profile=protocol.PROFILE_GRIP_NORMALLY, + ), + ] + ) + + async def test_gripper_positions_are_configurable(self): + driver = Access2Driver( + device_id="test", + gripper_open_position=0.25, + gripper_closed_position=4.75, + ) + driver._move_axis_to_position = AsyncMock() # type: ignore[method-assign] + driver.request_status = AsyncMock( # type: ignore[method-assign] + return_value=_status(flags=_READY_FLAGS) + ) + + await driver.close_gripper() + await driver.open_gripper() + + driver._move_axis_to_position.assert_has_awaits( # type: ignore[attr-defined] + [ + call( + protocol.AXIS_GRIPPER, + 4.75, + profile=protocol.PROFILE_GRIP_NORMALLY, + ), + call( + protocol.AXIS_GRIPPER, + 0.25, + profile=protocol.PROFILE_GRIP_NORMALLY, + ), + ] + ) + + async def test_setup_opens_gripper_with_configured_position_and_profile(self): + driver = Access2Driver(device_id="test", gripper_open_position=0.25) + driver.io.setup = AsyncMock() # type: ignore[method-assign] + driver.io.set_baudrate = AsyncMock() # type: ignore[method-assign] + driver.request_status = AsyncMock( # type: ignore[method-assign] + return_value=_status(flags=_READY_FLAGS) + ) + driver.send_command = AsyncMock() # type: ignore[method-assign] + driver._home = AsyncMock() # type: ignore[method-assign] + driver._move_axis_to_position = AsyncMock() # type: ignore[method-assign] + driver._move_to_teachpoint = AsyncMock() # type: ignore[method-assign] + + await driver.setup() + + driver._move_axis_to_position.assert_awaited_once_with( # type: ignore[attr-defined] + protocol.AXIS_GRIPPER, + 0.25, + profile=protocol.PROFILE_GRIP_NORMALLY, + speed=protocol.SPEED_FAST, + ) + + async def test_close_gripper_is_idempotent_at_closed_position(self): + closed = protocol.Access2Status( + access2_status=_READY_FLAGS, + vspin_status=0, + gripper_status=protocol.AXIS_STATUS_MOVE_DONE, + gripper_position=5.68, + ) + self.driver.request_status = AsyncMock(return_value=closed) # type: ignore[method-assign] + + await self.driver.close_gripper() + + self.driver._move_axis_to_position.assert_not_awaited() # type: ignore[attr-defined] + + async def test_load_uses_named_motion_sequence(self): + ready = _status(flags=_READY_FLAGS) + self.driver.request_status = AsyncMock(side_effect=[ready, ready]) # type: ignore[method-assign] + self.driver.request_sensor_values = AsyncMock(return_value=0) # type: ignore[method-assign] + + await self.driver.load() + + self.driver._move_axis_to_position.assert_has_awaits( # type: ignore[attr-defined] + [ + call( + protocol.AXIS_GRIPPER, + 0, + profile=protocol.PROFILE_GRIP_NORMALLY, + speed=protocol.SPEED_FAST, + ), + call( + protocol.AXIS_GRIPPER, + 5.68, + profile=protocol.PROFILE_GRIP_NORMALLY, + ), + call( + protocol.AXIS_GRIPPER, + 0, + profile=protocol.PROFILE_GRIP_NORMALLY, + ), + ] + ) + self.driver._move_to_teachpoint.assert_has_awaits( # type: ignore[attr-defined] + [ + call(protocol.TEACHPOINT_PICK, 3, 10), + call( + protocol.TEACHPOINT_BUCKET_1, + 3, + 10, + profile=protocol.PROFILE_DYNAMIC_FULL, + ), + call(protocol.TEACHPOINT_PARK, 3, 10), + ] + ) + + async def test_load_stops_before_gripping_when_plate_is_absent(self): + ready = _status(flags=_READY_FLAGS) + self.driver.request_status = AsyncMock(return_value=ready) # type: ignore[method-assign] + self.driver.request_sensor_values = AsyncMock( # type: ignore[method-assign] + return_value=protocol.SENSOR_NO_PLATE + ) + + with self.assertRaisesRegex(RuntimeError, "no plate found on stage"): + await self.driver.load() + + self.driver._move_axis_to_position.assert_awaited_once() # type: ignore[attr-defined] + self.driver._move_to_teachpoint.assert_awaited_once_with( # type: ignore[attr-defined] + protocol.TEACHPOINT_PICK, 3, 10 + ) + + async def test_estop_prevents_load_motion(self): + self.driver.request_status = AsyncMock( # type: ignore[method-assign] + return_value=_status(flags=_READY_FLAGS | protocol.STATUS_ESTOP_ACTIVE) + ) + + with self.assertRaisesRegex(RuntimeError, "emergency stop"): + await self.driver.load() + + self.driver._move_axis_to_position.assert_not_awaited() # type: ignore[attr-defined] + self.driver._move_to_teachpoint.assert_not_awaited() # type: ignore[attr-defined] + + async def test_motor_fault_prevents_load_motion(self): + self.driver.request_status = AsyncMock( # type: ignore[method-assign] + return_value=_status(flags=_READY_FLAGS | protocol.STATUS_MOTOR_POWER_FAULT) + ) + + with self.assertRaisesRegex(RuntimeError, "motor power fault"): + await self.driver.load() + + self.driver._move_axis_to_position.assert_not_awaited() # type: ignore[attr-defined] + self.driver._move_to_teachpoint.assert_not_awaited() # type: ignore[attr-defined] + + async def test_homed_status_does_not_hide_estop(self): + self.driver.request_status = AsyncMock( # type: ignore[method-assign] + return_value=_status(flags=protocol.STATUS_HOMED | protocol.STATUS_ESTOP_ACTIVE) + ) + + with self.assertRaisesRegex(RuntimeError, "emergency stop"): + await self.driver._wait_until_homed() diff --git a/pylabrobot/agilent/vspin/nmc_tests.py b/pylabrobot/agilent/vspin/nmc_tests.py new file mode 100644 index 00000000000..66929c42709 --- /dev/null +++ b/pylabrobot/agilent/vspin/nmc_tests.py @@ -0,0 +1,270 @@ +import unittest + +from pylabrobot.agilent.vspin import _nmc +from pylabrobot.io.binary import Writer + + +def _response(status: int, data: bytes) -> bytes: + return bytes([status]) + data + bytes([(status + sum(data)) & 0xFF]) + + +class NMCCommandTests(unittest.TestCase): + def test_known_setup_commands(self): + self.assertEqual(_nmc.build_set_address(1).hex(), "aa002101ff21") + self.assertEqual( + _nmc.build_read_status(_nmc.PIC_SERVO_ADDRESS, _nmc.SEND_MODULE_ID).hex(), + "aa01132034", + ) + self.assertEqual( + _nmc.build_define_status( + _nmc.PIC_SERVO_ADDRESS, + _nmc.SEND_POSITION + | _nmc.SEND_ANALOG + | _nmc.SEND_VELOCITY + | _nmc.SEND_AUXILIARY + | _nmc.SEND_HOME, + ).hex(), + "aa01121f32", + ) + self.assertEqual(_nmc.build_no_op(_nmc.PIC_SERVO_ADDRESS).hex(), "aa010e0f") + self.assertEqual(_nmc.build_no_op(_nmc.PIC_IO_ADDRESS).hex(), "aa020e10") + + def test_known_io_output_command(self): + self.assertEqual( + _nmc.build_set_output(_nmc.PIC_IO_ADDRESS, 0x0600).hex(), + "aa022600062e", + ) + + def test_known_baud_and_reset_commands(self): + self.assertEqual(_nmc.build_set_baud(57600).hex(), "aaff1a142d") + self.assertEqual(_nmc.build_hard_reset().hex(), "aaff0f0e") + + def test_known_gain_and_servo_state_commands(self): + position_gains = _nmc.ServoGains( + proportional=200, + derivative=1200, + integral=150, + integration_limit=15, + output_limit=75, + current_limit=0, + position_error_limit=4000, + servo_rate=5, + deadband=0, + ) + self.assertEqual( + _nmc.build_set_gain(_nmc.PIC_SERVO_ADDRESS, position_gains).hex(), + "aa01e6c800b00496000f004b00a00f050007", + ) + self.assertEqual( + _nmc.build_stop_motor(_nmc.PIC_SERVO_ADDRESS, _nmc.MOTOR_OFF).hex(), + "aa0117021a", + ) + self.assertEqual(_nmc.build_clear_bits(_nmc.PIC_SERVO_ADDRESS).hex(), "aa010b0c") + self.assertEqual(_nmc.build_reset_position(_nmc.PIC_SERVO_ADDRESS).hex(), "aa010001") + self.assertEqual(_nmc.build_set_homing(_nmc.PIC_SERVO_ADDRESS, 0x28).hex(), "aa01192842") + + def test_known_position_trajectory(self): + mode = ( + _nmc.LOAD_POSITION + | _nmc.LOAD_VELOCITY + | _nmc.LOAD_ACCELERATION + | _nmc.ENABLE_SERVO + | _nmc.START_NOW + ) + command = _nmc.build_load_trajectory( + _nmc.PIC_SERVO_ADDRESS, + mode, + position=0, + velocity=0x28F5C3, + acceleration=0x1AD7, + ) + self.assertEqual(command.hex(), "aa01d49700000000c3f52800d71a00003d") + + def test_known_deceleration_trajectory(self): + mode = ( + _nmc.LOAD_VELOCITY + | _nmc.LOAD_ACCELERATION + | _nmc.ENABLE_SERVO + | _nmc.VELOCITY_MODE + | _nmc.START_NOW + ) + command = _nmc.build_load_trajectory( + _nmc.PIC_SERVO_ADDRESS, + mode, + velocity=0, + acceleration=732, + ) + self.assertEqual(command.hex(), "aa0194b600000000dc02000029") + + def test_trajectory_requires_fields_selected_by_mode(self): + with self.assertRaisesRegex(ValueError, "velocity is required"): + _nmc.build_load_trajectory( + _nmc.PIC_SERVO_ADDRESS, + _nmc.LOAD_VELOCITY, + ) + with self.assertRaisesRegex(ValueError, "LOAD_POSITION is not set"): + _nmc.build_load_trajectory( + _nmc.PIC_SERVO_ADDRESS, + 0, + position=1, + ) + with self.assertRaisesRegex(ValueError, "signed 32-bit"): + _nmc.build_load_trajectory( + _nmc.PIC_SERVO_ADDRESS, + _nmc.LOAD_POSITION, + position=2**31, + ) + + def test_command_validation(self): + with self.assertRaisesRegex(ValueError, "address"): + _nmc.build_command(33, _nmc.CMD_NO_OP) + with self.assertRaisesRegex(ValueError, "four bits"): + _nmc.build_command(1, 16) + with self.assertRaisesRegex(ValueError, "at most 15"): + _nmc.build_command(1, _nmc.CMD_SET_GAIN, b"x" * 16) + + +class NMCResponseTests(unittest.TestCase): + def test_status_data_lengths(self): + servo_mask = ( + _nmc.SEND_POSITION + | _nmc.SEND_ANALOG + | _nmc.SEND_VELOCITY + | _nmc.SEND_AUXILIARY + | _nmc.SEND_HOME + ) + self.assertEqual(_nmc.servo_status_data_length(servo_mask), 12) + self.assertEqual( + _nmc.io_status_data_length(_nmc.SEND_INPUTS | _nmc.SEND_ANALOG_1), + 3, + ) + + def test_parse_captured_servo_status(self): + mask = ( + _nmc.SEND_POSITION + | _nmc.SEND_ANALOG + | _nmc.SEND_VELOCITY + | _nmc.SEND_AUXILIARY + | _nmc.SEND_HOME + ) + status = _nmc.parse_servo_status( + bytes.fromhex("11222500004f000018e0050000a4"), + mask, + ) + self.assertEqual(status.status, 0x11) + self.assertEqual(status.position, 0x2522) + self.assertEqual(status.analog, 0x4F) + self.assertEqual(status.velocity, 0) + self.assertEqual(status.auxiliary, 0x18) + self.assertEqual(status.home_position, 0x05E0) + + def test_parse_signed_servo_fields_and_module_id(self): + mask = ( + _nmc.SEND_POSITION + | _nmc.SEND_VELOCITY + | _nmc.SEND_HOME + | _nmc.SEND_MODULE_ID + | _nmc.SEND_POSITION_ERROR + ) + data = ( + Writer() + .i32(-123456) + .i16(-321) + .i32(-4000) + .u8(_nmc.PIC_SERVO_MODULE_TYPE) + .u8(12) + .i16(-7) + .finish() + ) + status = _nmc.parse_servo_status(_response(0x09, data), mask) + self.assertEqual(status.position, -123456) + self.assertEqual(status.velocity, -321) + self.assertEqual(status.home_position, -4000) + self.assertEqual(status.module_type, _nmc.PIC_SERVO_MODULE_TYPE) + self.assertEqual(status.module_version, 12) + self.assertEqual(status.position_error, -7) + + def test_parse_signed_position_boundaries(self): + for position in (-(2**31), 0, 2**31 - 1): + with self.subTest(position=position): + status = _nmc.parse_servo_status( + _response(0x01, Writer().i32(position).finish()), + _nmc.SEND_POSITION, + ) + self.assertEqual(status.position, position) + + def test_parse_io_status(self): + mask = _nmc.SEND_INPUTS | _nmc.SEND_ANALOG_1 + status = _nmc.parse_io_status(_response(0x09, bytes.fromhex("341256")), mask) + self.assertEqual(status.inputs, 0x1234) + self.assertEqual(status.analog_1, 0x56) + + def test_response_rejects_wrong_length(self): + with self.assertRaisesRegex(_nmc.NMCProtocolError, "expected 3"): + _nmc.parse_response(b"\x01\x01", expected_data_length=1) + + def test_response_rejects_bad_checksum(self): + with self.assertRaisesRegex(_nmc.NMCProtocolError, "checksum mismatch"): + _nmc.parse_response(b"\x01\x02\x00", expected_data_length=1) + + def test_response_rejects_module_checksum_error(self): + with self.assertRaisesRegex(_nmc.NMCProtocolError, "rejected"): + _nmc.parse_response(b"\x02\x02", expected_data_length=0) + + +class VSpinTrajectoryMathTests(unittest.TestCase): + def test_rcf_rpm_round_trip(self): + rpm = _nmc.rcf_to_rpm(500) + self.assertAlmostEqual(rpm, 2114.774672189068, places=9) + self.assertAlmostEqual(_nmc.rpm_to_rcf(rpm), 500, places=9) + + def test_reference_500g_80_percent_case(self): + rpm = _nmc.rcf_to_rpm(500) + self.assertEqual(_nmc.rpm_to_nmc_velocity(rpm), 9_461_343) + self.assertEqual(_nmc.acceleration_to_nmc(0.8), 732) + self.assertAlmostEqual(_nmc.acceleration_rpm_per_second(0.8), 320.0) + self.assertAlmostEqual( + _nmc.acceleration_counts_per_second_squared(0.8), + 42_666.666666666664, + ) + self.assertAlmostEqual(_nmc.predicted_ramp_time(rpm, 0.8), 6.6086708495779) + self.assertEqual(_nmc.acceleration_distance(rpm, 0.8), 931_723) + self.assertEqual(_nmc.spin_target_distance(rpm, 60, 0.8), 20_191_493) + + def test_servo_rate_scales_velocity_and_acceleration(self): + self.assertEqual( + _nmc.rpm_to_nmc_velocity(100, servo_rate=5), + int(_nmc.NMC_VELOCITY_PER_RPM * 100 * 5), + ) + self.assertEqual( + _nmc.acceleration_to_nmc(1, servo_rate=5), + int(_nmc.NMC_ACCELERATION_AT_FULL_SCALE * 25), + ) + + def test_nearest_encoder_position_uses_shortest_path(self): + self.assertEqual(_nmc.nearest_encoder_position(7900, 100), 8100) + self.assertEqual(_nmc.nearest_encoder_position(100, 7900), -100) + self.assertEqual(_nmc.nearest_encoder_position(-100, 100), 100) + + def test_trajectory_rejects_positions_outside_signed_controller_range(self): + for position in (-(2**31) - 1, 2**31): + with self.subTest(position=position): + with self.assertRaisesRegex(ValueError, "signed 32-bit"): + _nmc.build_load_trajectory( + _nmc.PIC_SERVO_ADDRESS, + _nmc.LOAD_POSITION, + position=position, + ) + + def test_math_validation(self): + for acceleration in (0, -0.1, 1.1): + with self.assertRaisesRegex(ValueError, "acceleration"): + _nmc.acceleration_to_nmc(acceleration) + with self.assertRaisesRegex(ValueError, "rotor_radius"): + _nmc.rcf_to_rpm(500, rotor_radius=0) + with self.assertRaisesRegex(ValueError, "duration"): + _nmc.spin_target_distance(1000, -1, 0.8) + + +if __name__ == "__main__": + unittest.main() diff --git a/pylabrobot/agilent/vspin/vspin.py b/pylabrobot/agilent/vspin/vspin.py index 59169ec346a..5291e9115ca 100644 --- a/pylabrobot/agilent/vspin/vspin.py +++ b/pylabrobot/agilent/vspin/vspin.py @@ -1,13 +1,11 @@ import asyncio -import ctypes import json import logging -import math import os -import time import warnings from typing import Optional +from pylabrobot.agilent.vspin import _nmc from pylabrobot.events import device_reference, evented_operation, resource_reference from pylabrobot.io.ftdi import FTDI from pylabrobot.resources import Coordinate, ResourceHolder @@ -31,10 +29,13 @@ def _load_vspin_calibrations(device_id: str) -> Optional[int]: ) return None with open(_vspin_bucket_calibrations_path, "r") as f: - return json.load(f).get(device_id) # type: ignore + remainder = json.load(f).get(device_id) + if remainder is None: + return None + return int(remainder) % _nmc.COUNTS_PER_REVOLUTION -def _save_vspin_calibrations(device_id, remainder: int): +def _save_vspin_calibrations(device_id: str, remainder: int): if os.path.exists(_vspin_bucket_calibrations_path): with open(_vspin_bucket_calibrations_path, "r") as f: data = json.load(f) @@ -46,7 +47,73 @@ def _save_vspin_calibrations(device_id, remainder: int): json.dump(data, f) -FULL_ROTATION: int = 8000 +FULL_ROTATION: int = _nmc.COUNTS_PER_REVOLUTION + +_POSITION_GAINS = _nmc.ServoGains( + proportional=200, + derivative=1200, + integral=150, + integration_limit=15, + output_limit=75, + current_limit=0, + position_error_limit=4000, + servo_rate=5, + deadband=0, +) + +_VELOCITY_GAINS = _nmc.ServoGains( + proportional=5, + derivative=100, + integral=0, + integration_limit=0, + output_limit=253, + current_limit=0, + position_error_limit=16000, + servo_rate=1, + deadband=0, +) + +_HOMING_GAINS = _nmc.ServoGains( + proportional=5, + derivative=100, + integral=0, + integration_limit=0, + output_limit=50, + current_limit=0, + position_error_limit=1000, + servo_rate=1, + deadband=0, +) + +_POSITION_TRAJECTORY_MODE = ( + _nmc.LOAD_POSITION + | _nmc.LOAD_VELOCITY + | _nmc.LOAD_ACCELERATION + | _nmc.ENABLE_SERVO + | _nmc.START_NOW +) + +_VELOCITY_TRAJECTORY_MODE = ( + _nmc.LOAD_VELOCITY + | _nmc.LOAD_ACCELERATION + | _nmc.ENABLE_SERVO + | _nmc.VELOCITY_MODE + | _nmc.START_NOW +) + +_STATUS_POLL_INTERVAL = 0.1 +_MOTION_TIMEOUT = 15.0 +_SPIN_TIMEOUT_MARGIN = 5.0 +_TARGET_SPEED_FRACTION = 0.95 +_IO_TRANSITION_TIMEOUT = 5.0 +_SERVO_TRANSITION_SETTLE_TIME = 0.1 +_TACHOMETER_TO_RPM = -14.69320388 +_NETWORK_PROBE_TIMEOUT = 0.2 +_INITIAL_BAUD_RATES = (19200, 115200, 57600, 9600) +_NMC_RESET_SETTLE_TIME = 0.1 +_NMC_BAUD_SETTLE_TIME = 0.1 +_BUCKET_POSITION_TOLERANCE = 10 +_BUCKET_PRESENT_RETRIES = 1 bucket_1_not_set_error = RuntimeError( "Bucket 1 position not set. " @@ -55,6 +122,10 @@ def _save_vspin_calibrations(device_id, remainder: int): ) +class _PositionAlignmentError(RuntimeError): + """Raised when a completed rotor move settles outside its target tolerance.""" + + def _vspin_event_context( self: "VSpin", g: float = 500, @@ -103,7 +174,14 @@ def __init__(self, name: str, device_id: Optional[str] = None): self.name = name self.io = FTDI(human_readable_device_name="Agilent VSpin Centrifuge", device_id=device_id) self.device_id = device_id + self._servo_status_mask = 0 + self._io_status_mask = 0 + self._io_output_word = 0 + self._command_lock = asyncio.Lock() + self._spin_active = False + self._spin_cancel_requested = False self._bucket_1_remainder: Optional[int] = None + self._home_position: Optional[int] = None if device_id is not None: self._bucket_1_remainder = _load_vspin_calibrations(device_id) @@ -140,174 +218,561 @@ def at_bucket(self) -> Optional[ResourceHolder]: async def setup(self): logger.info("[vSpin %s] connected", self.device_id) await self.io.setup() - for _ in range(3): - await self.configure_and_initialize() - await self.send_command(bytes.fromhex("aa002101ff21")) - await self.send_command(bytes.fromhex("aa002101ff21")) - await self.send_command(bytes.fromhex("aa01132034")) - await self.send_command(bytes.fromhex("aa002102ff22")) - await self.send_command(bytes.fromhex("aa02132035")) - await self.send_command(bytes.fromhex("aa002103ff23")) - await self.send_command(bytes.fromhex("aaff1a142d")) - - await self.io.set_baudrate(57600) + await self._configure_ftdi() + await self._initialize_nmc_network() await self.io.set_rts(True) await self.io.set_dtr(True) - await self.send_command(bytes.fromhex("aa01121f32")) + servo_status_mask = ( + _nmc.SEND_POSITION + | _nmc.SEND_ANALOG + | _nmc.SEND_VELOCITY + | _nmc.SEND_AUXILIARY + | _nmc.SEND_HOME + ) + await self._send_nmc( + _nmc.build_define_status(_nmc.PIC_SERVO_ADDRESS, servo_status_mask), + response_data_length=_nmc.servo_status_data_length(servo_status_mask), + ) + self._servo_status_mask = servo_status_mask for _ in range(8): - await self.send_command(bytes.fromhex("aa0220ff0f30")) - await self.send_command(bytes.fromhex("aa0220df0f10")) - await self.send_command(bytes.fromhex("aa0220df0e0f")) - await self.send_command(bytes.fromhex("aa0220df0c0d")) - await self.send_command(bytes.fromhex("aa0220df0809")) + await self._send_nmc(_nmc.build_set_io_direction(_nmc.PIC_IO_ADDRESS, 0x0FFF)) + await self._send_nmc(_nmc.build_set_io_direction(_nmc.PIC_IO_ADDRESS, 0x0FDF)) + await self._send_nmc(_nmc.build_set_io_direction(_nmc.PIC_IO_ADDRESS, 0x0EDF)) + await self._send_nmc(_nmc.build_set_io_direction(_nmc.PIC_IO_ADDRESS, 0x0CDF)) + await self._send_nmc(_nmc.build_set_io_direction(_nmc.PIC_IO_ADDRESS, 0x08DF)) for _ in range(4): - await self.send_command(bytes.fromhex("aa0226000028")) - await self.send_command(bytes.fromhex("aa02120317")) + await self._write_io_output(0x0000) + io_status_mask = _nmc.SEND_INPUTS | _nmc.SEND_ANALOG_1 + await self._send_nmc( + _nmc.build_define_status(_nmc.PIC_IO_ADDRESS, io_status_mask), + response_data_length=_nmc.io_status_data_length(io_status_mask), + ) + self._io_status_mask = io_status_mask for _ in range(5): - await self.send_command(bytes.fromhex("aa0226200048")) - await self.send_command(bytes.fromhex("aa0226000028")) + await self._write_io_output(1 << _nmc.OUTPUT_VERSION_TOGGLE) + await self._write_io_output(0x0000) await self.lock_door() - await self.send_command(bytes.fromhex("aa0226000028")) - - await self.send_command(bytes.fromhex("aa0117021a")) - await self.send_command(bytes.fromhex("aa01e6c800b00496000f004b00a00f050007")) - await self.send_command(bytes.fromhex("aa0117041c")) - await self.send_command(bytes.fromhex("aa01170119")) - - await self.send_command(bytes.fromhex("aa010b0c")) - await self.send_command(bytes.fromhex("aa010001")) - await self.send_command(bytes.fromhex("aa01e605006400000000003200e80301006e")) - await self.send_command(bytes.fromhex("aa0194b61283000012010000f3")) - await self.send_command(bytes.fromhex("aa01192842")) + await self._write_io_output(0x0000) + await self._wait_for_io_bit( + _nmc.INPUT_BUCKET_UNLOCKED, + True, + active_low=True, + name="bucket-unlock sensor", + ) - resp = 0x89 - while resp == 0x89: - resp = (await self.request_positions_and_tachometer()).status + await self._enable_amplifier_and_reset_servo_status() + await self._send_nmc(_nmc.build_reset_position(_nmc.PIC_SERVO_ADDRESS)) + await self._send_nmc(_nmc.build_set_gain(_nmc.PIC_SERVO_ADDRESS, _HOMING_GAINS)) + await self._send_nmc( + _nmc.build_load_trajectory( + _nmc.PIC_SERVO_ADDRESS, + _VELOCITY_TRAJECTORY_MODE, + velocity=0x8312, + acceleration=0x0112, + ) + ) + await self._send_nmc(_nmc.build_set_homing(_nmc.PIC_SERVO_ADDRESS, 0x28)) + + loop = asyncio.get_running_loop() + homing_deadline = loop.time() + _MOTION_TIMEOUT + homing_status = await self.request_positions_and_tachometer() + while homing_status.status & _nmc.STATUS_HOMING_IN_PROGRESS: + self._raise_on_servo_fault(homing_status, operation="homing") + if loop.time() >= homing_deadline: + raise TimeoutError( + f"VSpin homing did not finish within {_MOTION_TIMEOUT} seconds; " + f"last status was 0x{homing_status.status:02x}" + ) + await asyncio.sleep(_STATUS_POLL_INTERVAL) + homing_status = await self.request_positions_and_tachometer() + self._raise_on_servo_fault(homing_status, operation="homing") + if homing_status.home_position is None: + raise RuntimeError("VSpin homing response did not include the home position") + self._home_position = homing_status.home_position % FULL_ROTATION # --- almost the same as go to position --- - await self.send_command(bytes.fromhex("aa0117021a")) - await self.send_command(bytes.fromhex("aa01e6c800b00496000f004b00a00f050007")) - await self.send_command(bytes.fromhex("aa0117041c")) - await self.send_command(bytes.fromhex("aa01170119")) - - await self.send_command(bytes.fromhex("aa010b0c")) - await self.send_command(bytes.fromhex("aa01e6c800b00496000f004b00a00f050007")) - new_position = (0).to_bytes(4, byteorder="little") - await self.send_command( - bytes.fromhex("aa01d497") + new_position + bytes.fromhex("c3f52800d71a000049") + await self._enable_amplifier_and_reset_servo_status() + await self._send_nmc(_nmc.build_set_gain(_nmc.PIC_SERVO_ADDRESS, _POSITION_GAINS)) + await self._send_nmc( + _nmc.build_load_trajectory( + _nmc.PIC_SERVO_ADDRESS, + _POSITION_TRAJECTORY_MODE, + position=0, + velocity=0x28F5C3, + acceleration=0x1AD7, + ) ) # ----------------------------------------- - resp = 0x08 - while resp != 0x09: - resp = (await self.request_positions_and_tachometer()).status - - await self.send_command(bytes.fromhex("aa0117021a")) + move_deadline = loop.time() + _MOTION_TIMEOUT + move_status = await self.request_positions_and_tachometer() + while not ( + move_status.status & _nmc.STATUS_MOVE_DONE + and move_status.position is not None + and abs(move_status.position) <= _BUCKET_POSITION_TOLERANCE + ): + self._raise_on_servo_fault(move_status, operation="setup positioning") + if loop.time() >= move_deadline: + raise TimeoutError( + f"VSpin setup motion did not finish within {_MOTION_TIMEOUT} seconds; " + f"last status was 0x{move_status.status:02x}, " + f"last position was {move_status.position}" + ) + await asyncio.sleep(_STATUS_POLL_INTERVAL) + move_status = await self.request_positions_and_tachometer() + self._raise_on_servo_fault(move_status, operation="setup positioning") + + await self._disable_servo_after_motion() await self.lock_door() async def stop(self): logger.info("[vSpin %s] disconnected", self.device_id) - await self.configure_and_initialize() await self.io.stop() + async def _enable_amplifier_and_reset_servo_status(self) -> None: + """Apply the vendor transition delays before clearing status for motion.""" + await asyncio.sleep(_SERVO_TRANSITION_SETTLE_TIME) + await self._send_nmc(_nmc.build_stop_motor(_nmc.PIC_SERVO_ADDRESS, _nmc.MOTOR_OFF)) + await self._send_nmc(_nmc.build_set_gain(_nmc.PIC_SERVO_ADDRESS, _POSITION_GAINS)) + await self._send_nmc(_nmc.build_stop_motor(_nmc.PIC_SERVO_ADDRESS, _nmc.STOP_ABRUPT)) + await self._send_nmc(_nmc.build_stop_motor(_nmc.PIC_SERVO_ADDRESS, _nmc.AMPLIFIER_ENABLE)) + await asyncio.sleep(_SERVO_TRANSITION_SETTLE_TIME) + await self._send_nmc(_nmc.build_clear_bits(_nmc.PIC_SERVO_ADDRESS)) + + async def _disable_servo_after_motion(self) -> None: + """Allow the completed move to settle around the vendor motor-off transition.""" + await asyncio.sleep(_SERVO_TRANSITION_SETTLE_TIME) + await self._send_nmc(_nmc.build_stop_motor(_nmc.PIC_SERVO_ADDRESS, _nmc.MOTOR_OFF)) + await asyncio.sleep(_SERVO_TRANSITION_SETTLE_TIME) + # -- low-level protocol -- - async def _read_resp(self, timeout: float = 20) -> bytes: - data = b"" - end_byte_found = False - start_time = time.time() + async def _read_exact_response(self, length: int, timeout: float) -> bytes: + """Read one fixed-length NMC response. - while True: - chunk = await self.io.read(25) + NMC responses do not have a delimiter. Their length is determined by the + status mask configured for the addressed module. + """ + if length < 1: + raise ValueError("NMC response length must be positive") + loop = asyncio.get_running_loop() + deadline = loop.time() + timeout + response = bytearray() + while len(response) < length: + chunk = await self.io.read(length - len(response)) if chunk: - data += chunk - end_byte_found = data[-1] == 0x0D - if len(chunk) < 25 and end_byte_found: - break - else: - if end_byte_found or time.time() - start_time > timeout: - break - await asyncio.sleep(0.0001) - + response.extend(chunk) + continue + if loop.time() >= deadline: + raise TimeoutError( + f"VSpin sent {len(response)} of {length} expected response bytes " + f"within {timeout} seconds: {bytes(response).hex()}" + ) + await asyncio.sleep(0) + data = bytes(response) logger.debug("Read %s", data.hex()) return data - async def send_command(self, cmd: bytes, read_timeout=0.2) -> bytes: - written = await self.io.write(bytes(cmd)) - if written != len(cmd): - raise RuntimeError("Failed to write all bytes") - return await self._read_resp(timeout=read_timeout) + async def send_command( + self, + cmd: bytes, + expected_response_length: int, + read_timeout: float = 0.2, + ) -> bytes: + """Send a VSpin command and read its fixed-length response.""" + async with self._command_lock: + written = await self.io.write(bytes(cmd)) + if written != len(cmd): + raise RuntimeError(f"VSpin wrote {written} of {len(cmd)} bytes for NMC command {cmd.hex()}") + return await self._read_exact_response(expected_response_length, timeout=read_timeout) + + async def _send_nmc( + self, + command: bytes, + *, + response_data_length: Optional[int] = None, + expect_response: bool = True, + timeout: float = 0.2, + ) -> _nmc.NMCResponse: + """Send a framed NMC command and consume its complete response.""" + if len(command) < 4 or command[0] != _nmc.SYNC_BYTE: + raise ValueError(f"Invalid NMC command: {command.hex()}") + if not expect_response: + async with self._command_lock: + written = await self.io.write(command) + if written != len(command): + raise RuntimeError( + f"VSpin wrote {written} of {len(command)} bytes for NMC command {command.hex()}" + ) + return _nmc.NMCResponse(status=0, data=b"") + + if response_data_length is None: + address = command[1] + if address == _nmc.PIC_SERVO_ADDRESS: + response_data_length = _nmc.servo_status_data_length(self._servo_status_mask) + elif address == _nmc.PIC_IO_ADDRESS: + response_data_length = _nmc.io_status_data_length(self._io_status_mask) + else: + response_data_length = 0 - async def configure_and_initialize(self): - await self.set_configuration_data() - await self.initialize() + try: + response = await self.send_command( + command, + expected_response_length=response_data_length + 2, + read_timeout=timeout, + ) + except TimeoutError as error: + raise TimeoutError(f"VSpin NMC command {command.hex()} timed out: {error}") from error + try: + return _nmc.parse_response(response, response_data_length) + except _nmc.NMCProtocolError as error: + raise _nmc.NMCProtocolError( + f"VSpin NMC command {command.hex()} failed for response {response.hex()}: {error}" + ) from error + + async def _initialize_nmc_network(self) -> None: + """Find the controller baud, reset the bus, and assign the two known modules.""" + last_error: Exception | None = None + for initial_baudrate in _INITIAL_BAUD_RATES: + await self.io.set_baudrate(initial_baudrate) + await self._reset_nmc_network() + await self._reopen_ftdi(19200) + await self._reset_nmc_network() + # Closing the transport is intentional. Some reset/NOP replies have already crossed the + # USB boundary when the FTDI receive buffer is purged, and can otherwise become the reply + # to SET_ADDRESS. Reopening discards those host-side bytes as well. + await self._reopen_ftdi(19200) + await self.io.usb_purge_rx_buffer() + + try: + await self._send_nmc( + _nmc.build_set_address(_nmc.PIC_SERVO_ADDRESS), + timeout=_NETWORK_PROBE_TIMEOUT, + ) + except (TimeoutError, _nmc.NMCProtocolError) as error: + last_error = error + await self.io.usb_purge_rx_buffer() + continue + + modules: dict[int, tuple[int, int]] = {} + servo_id_response = await self._send_nmc( + _nmc.build_read_status(_nmc.PIC_SERVO_ADDRESS, _nmc.SEND_MODULE_ID), + response_data_length=2, + ) + modules[_nmc.PIC_SERVO_ADDRESS] = ( + servo_id_response.data[0], + servo_id_response.data[1], + ) + + try: + await self._send_nmc( + _nmc.build_set_address(_nmc.PIC_IO_ADDRESS), + timeout=_NETWORK_PROBE_TIMEOUT, + ) + except (TimeoutError, _nmc.NMCProtocolError) as error: + raise RuntimeError( + "VSpin found only one NMC module; expected one PIC-SERVO and one PIC-IO" + ) from error + io_id_response = await self._send_nmc( + _nmc.build_read_status(_nmc.PIC_IO_ADDRESS, _nmc.SEND_MODULE_ID), + response_data_length=2, + ) + modules[_nmc.PIC_IO_ADDRESS] = ( + io_id_response.data[0], + io_id_response.data[1], + ) - async def set_configuration_data(self): - """Set the device configuration data.""" + try: + await self._send_nmc( + _nmc.build_set_address(3), + timeout=_NETWORK_PROBE_TIMEOUT, + ) + except TimeoutError: + await self.io.usb_purge_rx_buffer() + else: + extra_id_response = await self._send_nmc( + _nmc.build_read_status(3, _nmc.SEND_MODULE_ID), + response_data_length=2, + ) + raise RuntimeError( + "VSpin found an unexpected third NMC module: " + f"type {extra_id_response.data[0]}, version {extra_id_response.data[1]}" + ) + + if modules[_nmc.PIC_SERVO_ADDRESS][0] != _nmc.PIC_SERVO_MODULE_TYPE: + raise RuntimeError( + "VSpin expected a PIC-SERVO at address 1, found module type " + f"{modules[_nmc.PIC_SERVO_ADDRESS][0]}" + ) + if modules[_nmc.PIC_IO_ADDRESS][0] != _nmc.PIC_IO_MODULE_TYPE: + raise RuntimeError( + "VSpin expected a PIC-IO at address 2, found module type " + f"{modules[_nmc.PIC_IO_ADDRESS][0]}" + ) + + await self._send_nmc(_nmc.build_set_baud(57600), expect_response=False) + await asyncio.sleep(_NMC_BAUD_SETTLE_TIME) + await self._reopen_ftdi(57600) + await asyncio.sleep(_NMC_BAUD_SETTLE_TIME) + await self.io.usb_purge_rx_buffer() + return + + context = "" if last_error is None else f": {last_error}" + raise RuntimeError( + f"VSpin NMC initialization found no modules at supported baud rates{context}" + ) + + async def _configure_ftdi(self, baudrate: int = 19200) -> None: + """Configure the FTDI UART before probing the NMC network.""" await self.io.set_latency_timer(16) await self.io.set_line_property(bits=8, stopbits=1, parity=0) await self.io.set_flowctrl(0) - await self.io.set_baudrate(19200) + await self.io.set_baudrate(baudrate) + + async def _reopen_ftdi(self, baudrate: int) -> None: + """Reopen the FTDI transport and restore all UART settings.""" + await self.io.stop() + await self.io.setup() + await self._configure_ftdi(baudrate) - async def initialize(self): + async def _reset_nmc_network(self) -> None: + """Send the vendor hard-reset sequence at the currently selected baud rate.""" + self._servo_status_mask = 0 + self._io_status_mask = 0 + self._io_output_word = 0 await self.io.write(b"\x00" * 20) - for i in range(33): - packet = b"\xaa" + bytes([i & 0xFF, 0x0E, 0x0E + (i & 0xFF)]) + b"\x00" * 8 - await self.io.write(packet) - await self.send_command(bytes.fromhex("aaff0f0e")) + for address in range(33): + await self.io.write(_nmc.build_no_op(address) + b"\x00" * 8) + await self._send_nmc(_nmc.build_hard_reset(), expect_response=False) + await asyncio.sleep(_NMC_RESET_SETTLE_TIME) # -- hardware status queries -- - class _StatusPositionTachometer(ctypes.LittleEndianStructure): - _pack_ = 1 - _fields_ = [ - ("status", ctypes.c_uint8), - ("current_position", ctypes.c_uint32), - ("unknown1", ctypes.c_uint8), - ("tachometer", ctypes.c_int16), - ("unknown2", ctypes.c_uint8), - ("home_position", ctypes.c_uint32), - ("checksum", ctypes.c_uint8), - ] - - async def request_positions_and_tachometer(self) -> "VSpin._StatusPositionTachometer": - resp = await self.send_command(bytes.fromhex("aa010e0f")) - if len(resp) == 0: - raise IOError("Empty status from centrifuge") - return VSpin._StatusPositionTachometer.from_buffer_copy(resp) + async def request_positions_and_tachometer(self) -> _nmc.ServoStatus: + status_mask = ( + _nmc.SEND_POSITION + | _nmc.SEND_ANALOG + | _nmc.SEND_VELOCITY + | _nmc.SEND_AUXILIARY + | _nmc.SEND_HOME + ) + response = await self._send_nmc( + _nmc.build_no_op(_nmc.PIC_SERVO_ADDRESS), + response_data_length=_nmc.servo_status_data_length(status_mask), + ) + return _nmc.decode_servo_status(response, status_mask) async def request_position(self) -> int: - return (await self.request_positions_and_tachometer()).current_position # type: ignore + position = (await self.request_positions_and_tachometer()).position + if position is None: + raise RuntimeError("VSpin position was absent from the configured servo status") + return position - async def request_tachometer(self) -> int: + async def request_tachometer(self) -> float: """Current speed in rpm.""" - tack_to_rpm = -14.69320388 - return (await self.request_positions_and_tachometer()).tachometer * tack_to_rpm # type: ignore + velocity = (await self.request_positions_and_tachometer()).velocity + if velocity is None: + raise RuntimeError("VSpin velocity was absent from the configured servo status") + return velocity * _TACHOMETER_TO_RPM + + @staticmethod + def _raise_on_servo_fault(status: _nmc.ServoStatus, *, operation: str) -> None: + if status.status & _nmc.STATUS_OVERCURRENT: + raise RuntimeError( + f"VSpin servo overcurrent detected during {operation} (status 0x{status.status:02x})" + ) + if status.status & _nmc.STATUS_POSITION_ERROR: + raise RuntimeError( + f"VSpin servo position error detected during {operation} (status 0x{status.status:02x})" + ) + + async def _wait_for_target_speed(self, rpm: float, acceleration: float) -> None: + """Wait until measured speed reaches the requested spin speed.""" + timeout = _nmc.predicted_ramp_time(rpm, acceleration) + _SPIN_TIMEOUT_MARGIN + loop = asyncio.get_running_loop() + deadline = loop.time() + timeout + await self._raise_for_spin_faults() + measured_rpm = await self.request_tachometer() + while measured_rpm < rpm * _TARGET_SPEED_FRACTION: + await self._raise_for_spin_faults() + if self._spin_cancel_requested: + return + if loop.time() >= deadline: + raise TimeoutError( + f"VSpin reached only {measured_rpm:.1f} RPM of the requested {rpm:.1f} RPM " + f"within {timeout:.1f} seconds" + ) + await asyncio.sleep(_STATUS_POLL_INTERVAL) + measured_rpm = await self.request_tachometer() + + async def _wait_for_position( + self, + position: int, + timeout: float, + operation: str, + *, + cancel_on_spin_abort: bool = False, + ) -> int: + """Wait for the encoder to reach ``position`` and return its final value.""" + loop = asyncio.get_running_loop() + deadline = loop.time() + timeout + if cancel_on_spin_abort: + await self._raise_for_spin_faults() + current_position = await self.request_position() + while current_position < position: + if cancel_on_spin_abort: + await self._raise_for_spin_faults() + if self._spin_cancel_requested: + return current_position + if loop.time() >= deadline: + raise TimeoutError( + f"VSpin {operation} did not reach encoder position {position} within " + f"{timeout:.1f} seconds; last position was {current_position}" + ) + await asyncio.sleep(_STATUS_POLL_INTERVAL) + current_position = await self.request_position() + return current_position + + async def _raise_for_spin_faults(self) -> None: + """Raise when a wired safety input makes continued rotor motion unsafe.""" + inputs = await self._request_input_flags() + if inputs & (1 << _nmc.INPUT_AMPLIFIER_FAULT): + raise RuntimeError("VSpin amplifier fault detected during spin") + if inputs & (1 << _nmc.INPUT_IMBALANCE): + raise RuntimeError("VSpin imbalance detected during spin") + if inputs & (1 << _nmc.INPUT_DOOR_OPEN): + raise RuntimeError("VSpin door-open sensor became active during spin") + if inputs & (1 << _nmc.INPUT_DOOR_LOCKED): + raise RuntimeError("VSpin door-lock sensor became inactive during spin") + if inputs & (1 << _nmc.INPUT_BUCKET_UNLOCKED): + raise RuntimeError("VSpin bucket-unlock sensor became inactive during spin") + + async def _command_deceleration(self, deceleration: float) -> None: + """Command a velocity-mode ramp to zero RPM.""" + await self._send_nmc(_nmc.build_set_gain(_nmc.PIC_SERVO_ADDRESS, _VELOCITY_GAINS)) + await self._send_nmc( + _nmc.build_load_trajectory( + _nmc.PIC_SERVO_ADDRESS, + _VELOCITY_TRAJECTORY_MODE, + velocity=0, + acceleration=_nmc.acceleration_to_nmc(deceleration), + ) + ) + + async def _wait_until_stopped(self, initial_rpm: float, deceleration: float) -> None: + """Wait until the controller reports motion complete and zero measured velocity.""" + timeout = _nmc.predicted_ramp_time(initial_rpm, deceleration) + _SPIN_TIMEOUT_MARGIN + loop = asyncio.get_running_loop() + deadline = loop.time() + timeout + await self._raise_for_spin_faults() + status = await self.request_positions_and_tachometer() + while True: + self._raise_on_servo_fault(status, operation="deceleration") + if status.velocity is None: + raise RuntimeError("VSpin velocity was absent while confirming deceleration") + measured_rpm = abs(status.velocity * _TACHOMETER_TO_RPM) + motion_complete = bool(status.status & _nmc.STATUS_MOVE_DONE) + if motion_complete and status.velocity == 0: + return + await self._raise_for_spin_faults() + if loop.time() >= deadline: + raise TimeoutError( + f"VSpin did not finish deceleration within {timeout:.1f} seconds; " + f"last status was 0x{status.status:02x} at {measured_rpm:.1f} RPM" + ) + await asyncio.sleep(_STATUS_POLL_INTERVAL) + status = await self.request_positions_and_tachometer() + + async def stop_spin(self, deceleration: float = 0.8) -> None: + """Safely abort an active spin and wait for measured speed to reach zero.""" + if deceleration <= 0 or deceleration > 1: + raise ValueError("Deceleration must be within 0-1.") + if not self._spin_active: + return + self._spin_cancel_requested = True + measured_rpm = abs(await self.request_tachometer()) + await self._command_deceleration(deceleration) + await self._wait_until_stopped(measured_rpm, deceleration) async def request_home_position(self) -> int: """Changes during a run, but the bucket 1 position relative to it does not.""" - return (await self.request_positions_and_tachometer()).home_position # type: ignore - - async def _request_status(self): - resp = await self.send_command(bytes.fromhex("aa020e10")) - if len(resp) == 0: - raise IOError("Empty status from centrifuge. Is the machine on?") - return resp + home_position = (await self.request_positions_and_tachometer()).home_position + if home_position is None: + raise RuntimeError("VSpin home position was absent from the configured servo status") + return home_position + + async def _request_status(self) -> _nmc.IOStatus: + status_mask = _nmc.SEND_INPUTS | _nmc.SEND_ANALOG_1 + response = await self._send_nmc( + _nmc.build_no_op(_nmc.PIC_IO_ADDRESS), + response_data_length=_nmc.io_status_data_length(status_mask), + ) + return _nmc.decode_io_status(response, status_mask) + + async def _request_input_flags(self) -> int: + inputs = (await self._request_status()).inputs + if inputs is None: + raise RuntimeError("VSpin inputs were absent from the configured IO status") + return inputs + + async def _write_io_output(self, output_word: int) -> None: + await self._send_nmc(_nmc.build_set_output(_nmc.PIC_IO_ADDRESS, output_word)) + self._io_output_word = output_word + + async def _set_io_output_bit(self, bit: int, value: bool) -> None: + if value: + output_word = self._io_output_word | (1 << bit) + else: + output_word = self._io_output_word & ~(1 << bit) + await self._write_io_output(output_word) + + async def _request_io_bit(self, bit: int, *, active_low: bool = False) -> bool: + value = bool(await self._request_input_flags() & (1 << bit)) + return not value if active_low else value + + async def _wait_for_io_bit( + self, + bit: int, + value: bool, + *, + active_low: bool = False, + name: str, + ) -> None: + loop = asyncio.get_running_loop() + deadline = loop.time() + _IO_TRANSITION_TIMEOUT + last_value = await self._request_io_bit(bit, active_low=active_low) + while last_value != value: + if loop.time() >= deadline: + raise TimeoutError( + f"VSpin {name} did not become {value} within {_IO_TRANSITION_TIMEOUT} seconds; " + f"last value was {last_value}" + ) + await asyncio.sleep(_STATUS_POLL_INTERVAL) + last_value = await self._request_io_bit(bit, active_low=active_low) async def request_bucket_locked(self) -> bool: - resp = await self._request_status() - return resp[2] & 0b0001 != 0 # type: ignore + return await self._request_io_bit(_nmc.INPUT_BUCKET_LOCKED, active_low=True) + + async def request_bucket_unlocked(self) -> bool: + return await self._request_io_bit(_nmc.INPUT_BUCKET_UNLOCKED, active_low=True) async def request_door_open(self) -> bool: - resp = await self._request_status() - return resp[2] & 0b0010 != 0 # type: ignore + return await self._request_io_bit(_nmc.INPUT_DOOR_OPEN) async def request_door_locked(self) -> bool: - resp = await self._request_status() - return resp[2] & 0b0100 == 0 # type: ignore + return await self._request_io_bit(_nmc.INPUT_DOOR_LOCKED, active_low=True) + + async def request_amplifier_fault(self) -> bool: + return await self._request_io_bit(_nmc.INPUT_AMPLIFIER_FAULT) + + async def request_imbalance(self) -> bool: + return await self._request_io_bit(_nmc.INPUT_IMBALANCE) + + async def request_spinning(self) -> bool: + return await self._request_io_bit(_nmc.INPUT_SPINNING) # -- bucket calibration -- @@ -321,23 +786,33 @@ async def set_bucket_1_position_to_current(self) -> None: """Set the current position as bucket 1 position and save calibration.""" current_position = await self.request_position() device_id = await self.io.request_serial() - remainder = await self.request_home_position() - current_position - self._bucket_1_remainder = current_position % FULL_ROTATION + home_position = await self.request_home_position() + self._home_position = home_position % FULL_ROTATION + remainder = (home_position - current_position) % FULL_ROTATION + self._bucket_1_remainder = remainder _save_vspin_calibrations(device_id, remainder) async def request_bucket_1_position(self) -> int: """Get the bucket 1 position based on calibration.""" + return await self._request_bucket_position(offset=0) + + async def request_bucket_2_position(self) -> int: + """Get the bucket 2 position based on calibration.""" + return await self._request_bucket_position(offset=FULL_ROTATION // 2) + + async def _request_bucket_position(self, offset: int) -> int: if self._bucket_1_remainder is None: raise bucket_1_not_set_error - home_position = await self.request_home_position() - bucket_1_position_mod_full_rotation = home_position - self.bucket_1_remainder + home_position = self._home_position + if home_position is None: + home_position = await self.request_home_position() + target_remainder = home_position - self.bucket_1_remainder + offset current_position = await self.request_position() - bucket_1_position = ( - FULL_ROTATION - * math.floor((current_position - bucket_1_position_mod_full_rotation) / FULL_ROTATION + 1) - + bucket_1_position_mod_full_rotation + return _nmc.nearest_encoder_position( + current_position, + target_remainder, + counts_per_revolution=FULL_ROTATION, ) - return bucket_1_position # -- CentrifugeBackend interface -- @@ -346,8 +821,12 @@ async def open_door(self): self._door_open = True return logger.info("[vSpin %s] open door", self.device_id) - await self.send_command(bytes.fromhex("aa022600062e")) - await asyncio.sleep(4) + await self._set_io_output_bit(_nmc.OUTPUT_DOOR_CYLINDER, True) + await self._wait_for_io_bit( + _nmc.INPUT_DOOR_OPEN, + True, + name="door-open sensor", + ) self._door_open = True async def close_door(self): @@ -355,8 +834,12 @@ async def close_door(self): self._door_open = False return logger.info("[vSpin %s] close door", self.device_id) - await self.send_command(bytes.fromhex("aa022600042c")) - await asyncio.sleep(2) + await self._set_io_output_bit(_nmc.OUTPUT_DOOR_CYLINDER, False) + await self._wait_for_io_bit( + _nmc.INPUT_DOOR_OPEN, + False, + name="door-open sensor", + ) self._door_open = False async def lock_door(self): @@ -365,58 +848,125 @@ async def lock_door(self): if await self.request_door_locked(): return logger.info("[vSpin %s] lock door", self.device_id) - await self.send_command(bytes.fromhex("aa0226000028")) + await self._set_io_output_bit(_nmc.OUTPUT_DOOR_LOCK_CYLINDER, False) + await self._wait_for_io_bit( + _nmc.INPUT_DOOR_LOCKED, + True, + active_low=True, + name="door-lock sensor", + ) async def unlock_door(self): if not await self.request_door_locked(): return - await self.send_command(bytes.fromhex("aa022600042c")) + await self._set_io_output_bit(_nmc.OUTPUT_DOOR_LOCK_CYLINDER, True) + await self._wait_for_io_bit( + _nmc.INPUT_DOOR_LOCKED, + False, + active_low=True, + name="door-lock sensor", + ) async def lock_bucket(self): if await self.request_bucket_locked(): return - await self.send_command(bytes.fromhex("aa022600072f")) + await self._set_io_output_bit(_nmc.OUTPUT_BUCKET_LOCK_CYLINDER, True) + await self._wait_for_io_bit( + _nmc.INPUT_BUCKET_LOCKED, + True, + active_low=True, + name="bucket-lock sensor", + ) async def unlock_bucket(self): - if not await self.request_bucket_locked(): + if await self.request_bucket_unlocked(): return - await self.send_command(bytes.fromhex("aa022600062e")) + await self._set_io_output_bit(_nmc.OUTPUT_BUCKET_LOCK_CYLINDER, False) + await self._wait_for_io_bit( + _nmc.INPUT_BUCKET_UNLOCKED, + True, + active_low=True, + name="bucket-unlock sensor", + ) async def go_to_bucket1(self): - await self.go_to_position(await self.request_bucket_1_position()) - self._at_bucket = self.bucket1 + await self._go_to_bucket(self.bucket1, await self.request_bucket_1_position()) async def go_to_bucket2(self): - await self.go_to_position(await self.request_bucket_1_position() + FULL_ROTATION // 2) - self._at_bucket = self.bucket2 + await self._go_to_bucket(self.bucket2, await self.request_bucket_2_position()) + + async def _go_to_bucket(self, bucket: ResourceHolder, position: int) -> None: + for attempt in range(_BUCKET_PRESENT_RETRIES + 1): + try: + await self.go_to_position(position) + except _PositionAlignmentError: + if attempt >= _BUCKET_PRESENT_RETRIES: + raise + position += FULL_ROTATION + else: + self._at_bucket = bucket + return async def go_to_position(self, position: int): logger.info("[vSpin %s] go_to_position: position=%d", self.device_id, position) await self.close_door() await self.lock_door() - - position_bytes = position.to_bytes(4, byteorder="little") - byte_string = bytes.fromhex("aa01d497") + position_bytes + bytes.fromhex("c3f52800d71a0000") - sum_byte = (sum(byte_string) - 0xAA) & 0xFF - byte_string += sum_byte.to_bytes(1, byteorder="little") - await self.send_command(bytes.fromhex("aa0226000028")) - await self.send_command(bytes.fromhex("aa0117021a")) - await self.send_command(bytes.fromhex("aa01e6c800b00496000f004b00a00f050007")) - await self.send_command(bytes.fromhex("aa0117041c")) - await self.send_command(bytes.fromhex("aa01170119")) - await self.send_command(bytes.fromhex("aa010b0c")) - await self.send_command(bytes.fromhex("aa01e6c800b00496000f004b00a00f050007")) - await self.send_command(byte_string) - - while abs(await self.request_position() - position) > 10: - await asyncio.sleep(0.1) + await self.unlock_bucket() + + await self._enable_amplifier_and_reset_servo_status() + await self._send_nmc(_nmc.build_set_gain(_nmc.PIC_SERVO_ADDRESS, _POSITION_GAINS)) + try: + trajectory_response = await self._send_nmc( + _nmc.build_load_trajectory( + _nmc.PIC_SERVO_ADDRESS, + _POSITION_TRAJECTORY_MODE, + position=position, + velocity=0x28F5C3, + acceleration=0x1AD7, + ) + ) + motion_started = not bool(trajectory_response.status & _nmc.STATUS_MOVE_DONE) + + loop = asyncio.get_running_loop() + deadline = loop.time() + _MOTION_TIMEOUT + motion_status = await self.request_positions_and_tachometer() + while not motion_status.status & _nmc.STATUS_MOVE_DONE: + motion_started = True + self._raise_on_servo_fault(motion_status, operation=f"move to position {position}") + if loop.time() >= deadline: + raise TimeoutError( + f"VSpin did not complete motion to encoder position {position} within " + f"{_MOTION_TIMEOUT} seconds; last status was 0x{motion_status.status:02x}, " + f"last position was {motion_status.position}" + ) + await asyncio.sleep(_STATUS_POLL_INTERVAL) + motion_status = await self.request_positions_and_tachometer() + self._raise_on_servo_fault(motion_status, operation=f"move to position {position}") + except BaseException: + try: + await asyncio.shield( + self._send_nmc(_nmc.build_stop_motor(_nmc.PIC_SERVO_ADDRESS, _nmc.MOTOR_OFF)) + ) + except Exception: + logger.exception("[vSpin %s] failed to turn off motor after position error", self.device_id) + raise + + await self._disable_servo_after_motion() + if motion_status.position is None: + raise RuntimeError("VSpin completed motion without returning an encoder position") + if abs(motion_status.position - position) > _BUCKET_POSITION_TOLERANCE: + transition = "after motion started" if motion_started else "without reporting motion start" + raise _PositionAlignmentError( + f"VSpin completed move to encoder position {position} {transition}, but settled at " + f"{motion_status.position} (tolerance {_BUCKET_POSITION_TOLERANCE})" + ) + await self.lock_bucket() + await self.unlock_door() await self.open_door() @staticmethod def g_to_rpm(g: float) -> int: - r = 10 - rpm = int((g / (1.118 * 10**-5 * r)) ** 0.5) - return rpm + return int(_nmc.rcf_to_rpm(g)) @evented_operation("centrifuge.spin", _vspin_event_context) async def spin( @@ -461,82 +1011,69 @@ async def spin( deceleration, ) - acceleration_ticks_per_second2 = 12903.2 * acceleration - rounds_per_second = rpm / 60 - ticks_per_second = rounds_per_second * 8000 - distance_during_acceleration = int(0.5 * (ticks_per_second**2) / acceleration_ticks_per_second2) - + ticks_per_second = rpm / 60 * _nmc.COUNTS_PER_REVOLUTION distance_at_speed = ticks_per_second * duration current_position = await self.request_position() - final_position = int(current_position + distance_during_acceleration + distance_at_speed) + final_position = current_position + _nmc.spin_target_distance( + rpm=rpm, + duration=duration, + acceleration=acceleration, + ) - if final_position > 2**32 - 1: + if not -(2**31) <= final_position <= 2**31 - 1: raise NotImplementedError( - "We don't know what happens if the destination position exceeds 2^32-1. " + "The VSpin spin target does not fit in the controller's signed 32-bit position. " "Please report this issue on discuss.pylabrobot.org." ) - position_b = final_position.to_bytes(4, byteorder="little") - rpm_b = int(rpm * 4473.925).to_bytes(4, byteorder="little") - acceleration_b = int(9.15 * 100 * acceleration).to_bytes(4, byteorder="little") - - byte_string = bytes.fromhex("aa01d497") + position_b + rpm_b + acceleration_b - checksum = (sum(byte_string) - 0xAA) & 0xFF - byte_string += checksum.to_bytes(1, byteorder="little") - - await self.send_command(bytes.fromhex("aa0226000028")) - await self.send_command(bytes.fromhex("aa0117021a")) - await self.send_command(bytes.fromhex("aa01e6c800b00496000f004b00a00f050007")) - await self.send_command(bytes.fromhex("aa0117041c")) - await self.send_command(bytes.fromhex("aa01170119")) - await self.send_command(bytes.fromhex("aa010b0c")) - await self.send_command(bytes.fromhex("aa01e60500640000000000fd00803e01000c")) - - await self.send_command(byte_string) + spin_trajectory = _nmc.build_load_trajectory( + _nmc.PIC_SERVO_ADDRESS, + _POSITION_TRAJECTORY_MODE, + position=final_position, + velocity=_nmc.rpm_to_nmc_velocity(rpm), + acceleration=_nmc.acceleration_to_nmc(acceleration), + ) - while ( - await self.request_tachometer() < rpm * 0.95 - and await self.request_position() < final_position - ): - await asyncio.sleep(0.1) - - if await self.request_position() < final_position: - decel_start_position = await self.request_position() + distance_at_speed - - while await self.request_position() < decel_start_position: - await asyncio.sleep(0.1) - - await self.send_command(bytes.fromhex("aa01e60500640000000000fd00803e01000c")) - decc = int(9.15 * 100 * deceleration).to_bytes(2, byteorder="little") - decel_command = bytes.fromhex("aa0194b600000000") + decc + bytes.fromhex("0000") - decel_command += ((sum(decel_command) - 0xAA) & 0xFF).to_bytes(1, byteorder="little") - await self.send_command(decel_command) - - await asyncio.sleep(2) - - async def _reset_to_zero(): - await self.send_command(bytes.fromhex("aa0117021a")) - await self.send_command(bytes.fromhex("aa01e6c800b00496000f004b00a00f050007")) - await self.send_command(bytes.fromhex("aa0117041c")) - await self.send_command(bytes.fromhex("aa01170119")) - await self.send_command(bytes.fromhex("aa010b0c")) - await self.send_command(bytes.fromhex("aa010001")) - await self.send_command(bytes.fromhex("aa01e605006400000000003200e80301006e")) - await self.send_command(bytes.fromhex("aa0194b61283000012010000f3")) - await self.send_command(bytes.fromhex("aa01192842")) - - await _reset_to_zero() - - start = await self.request_home_position() - num_tries = 0 - while await self.request_home_position() == start: - await asyncio.sleep(0.1) - num_tries += 1 - if num_tries % 25 == 0: - await _reset_to_zero() - if num_tries > 100: - raise RuntimeError("Home position did not change after spin.") + await self._enable_amplifier_and_reset_servo_status() + await self._send_nmc(_nmc.build_set_gain(_nmc.PIC_SERVO_ADDRESS, _VELOCITY_GAINS)) + + trajectory_started = False + if self._spin_active: + raise RuntimeError("A VSpin spin is already active") + self._spin_active = True + self._spin_cancel_requested = False + try: + await self._raise_for_spin_faults() + await self._send_nmc(spin_trajectory) + trajectory_started = True + + await self._wait_for_target_speed(rpm, acceleration) + if not self._spin_cancel_requested: + cruise_start_position = await self.request_position() + decel_start_position = int(cruise_start_position + distance_at_speed) + cruise_timeout = duration / _TARGET_SPEED_FRACTION + _SPIN_TIMEOUT_MARGIN + await self._wait_for_position( + decel_start_position, + timeout=cruise_timeout, + operation="at-speed interval", + cancel_on_spin_abort=True, + ) + + if not self._spin_cancel_requested: + await self._command_deceleration(deceleration) + await self._wait_until_stopped(rpm, deceleration) + trajectory_started = False + except BaseException: + if trajectory_started: + try: + await asyncio.shield(self._command_deceleration(deceleration)) + await asyncio.shield(self._wait_until_stopped(rpm, deceleration)) + except Exception: + logger.exception("[vSpin %s] emergency deceleration failed", self.device_id) + raise + finally: + self._spin_active = False # The rotor has moved off whichever bucket was parked at the load position. self._at_bucket = None diff --git a/pylabrobot/agilent/vspin/vspin_tests.py b/pylabrobot/agilent/vspin/vspin_tests.py index b23470aaa99..2e68ac2d346 100644 --- a/pylabrobot/agilent/vspin/vspin_tests.py +++ b/pylabrobot/agilent/vspin/vspin_tests.py @@ -1,12 +1,162 @@ +import dataclasses import unittest -from unittest.mock import AsyncMock, patch +from collections import deque +from unittest.mock import AsyncMock, call, patch +from pylabrobot.agilent.vspin import _nmc, vspin as vspin_module from pylabrobot.agilent.vspin.access2 import Access2 +from pylabrobot.agilent.vspin.errors import CentrifugeDoorError from pylabrobot.agilent.vspin.vspin import VSpin from pylabrobot.events import EventBus, PLREvent, use_event_bus +from pylabrobot.io.binary import Writer from pylabrobot.resources import Coordinate, Resource +_SERVO_STATUS_MASK = ( + _nmc.SEND_POSITION | _nmc.SEND_ANALOG | _nmc.SEND_VELOCITY | _nmc.SEND_AUXILIARY | _nmc.SEND_HOME +) +_IO_STATUS_MASK = _nmc.SEND_INPUTS | _nmc.SEND_ANALOG_1 + + +def _nmc_response(status: int, data: bytes = b"") -> bytes: + return bytes([status]) + data + bytes([(status + sum(data)) & 0xFF]) + + +def _servo_status_data( + *, + position: int = 0, + velocity: int = 0, + home_position: int = 0, +) -> bytes: + return Writer().i32(position).u8(0).i16(velocity).u8(0).i32(home_position).finish() + + +def _io_status_data(*, inputs: int = 0) -> bytes: + return Writer().u16(inputs).u8(0).finish() + + +def _servo_step( + command: bytes, + *, + status: int = _nmc.STATUS_MOVE_DONE, + position: int = 0, + velocity: int = 0, + home_position: int = 0, +) -> "_VSpinScriptStep": + return _VSpinScriptStep( + command, + _nmc_response( + status, + _servo_status_data( + position=position, + velocity=velocity, + home_position=home_position, + ), + ), + ) + + +def _io_step( + command: bytes, + *, + status: int = _nmc.STATUS_MOVE_DONE, + inputs: int = 0, +) -> "_VSpinScriptStep": + return _VSpinScriptStep(command, _nmc_response(status, _io_status_data(inputs=inputs))) + + +def _empty_step( + command: bytes, + *, + status: int = _nmc.STATUS_MOVE_DONE, +) -> "_VSpinScriptStep": + return _VSpinScriptStep(command, _nmc_response(status)) + + +@dataclasses.dataclass(frozen=True) +class _VSpinScriptStep: + command: bytes + response: bytes | None + + +class _ScriptedVSpinFTDI: + """Validate VSpin writes and replay partial NMC responses from a fixed script.""" + + def __init__(self, steps: list[_VSpinScriptStep], max_read_size: int = 3): + self._steps = deque(steps) + self._response = bytearray() + self._max_read_size = max_read_size + self.setup_called = False + self.setup_call_count = 0 + self.stopped = False + self.stop_call_count = 0 + self.writes: list[bytes] = [] + self.latency_timers: list[int] = [] + self.line_properties: list[tuple[int, int, int]] = [] + self.flow_controls: list[int] = [] + self.baudrates: list[int] = [] + self.rts_levels: list[bool] = [] + self.dtr_levels: list[bool] = [] + self.rx_purge_count = 0 + + async def setup(self) -> None: + self.setup_called = True + self.setup_call_count += 1 + self.stopped = False + + async def stop(self) -> None: + self.stopped = True + self.stop_call_count += 1 + # Reopening the real FTDI connection discards replies already buffered on the host side. + self._response.clear() + + async def set_latency_timer(self, latency: int) -> None: + self.latency_timers.append(latency) + + async def set_line_property(self, bits: int, stopbits: int, parity: int) -> None: + self.line_properties.append((bits, stopbits, parity)) + + async def set_flowctrl(self, flowctrl: int) -> None: + self.flow_controls.append(flowctrl) + + async def set_baudrate(self, baudrate: int) -> None: + self.baudrates.append(baudrate) + + async def set_rts(self, level: bool) -> None: + self.rts_levels.append(level) + + async def set_dtr(self, level: bool) -> None: + self.dtr_levels.append(level) + + async def usb_purge_rx_buffer(self) -> None: + self.rx_purge_count += 1 + + async def write(self, data: bytes) -> int: + if self._response: + raise AssertionError(f"VSpin wrote before consuming response {self._response.hex()}") + if not self._steps: + raise AssertionError(f"Unexpected VSpin write: {data.hex()}") + step = self._steps.popleft() + if data != step.command: + raise AssertionError(f"VSpin wrote {data.hex()}, expected {step.command.hex()}") + self.writes.append(data) + if step.response is not None: + self._response.extend(step.response) + return len(data) + + async def read(self, length: int) -> bytes: + count = min(length, self._max_read_size, len(self._response)) + if count == 0: + return b"" + chunk = bytes(self._response[:count]) + del self._response[:count] + return chunk + + def assert_complete(self, test: unittest.TestCase) -> None: + test.assertEqual(list(self._steps), []) + test.assertEqual(bytes(self._response), b"") + + class TestVSpinEvents(unittest.IsolatedAsyncioTestCase): def setUp(self): self.vspin_ftdi = patch("pylabrobot.agilent.vspin.vspin.FTDI", autospec=True) @@ -20,12 +170,19 @@ async def test_spin_emits_loaded_bucket_resources_and_parameters(self): vspin.request_door_open = AsyncMock(return_value=False) # type: ignore[method-assign] vspin.request_door_locked = AsyncMock(return_value=True) # type: ignore[method-assign] vspin.request_bucket_locked = AsyncMock(return_value=False) # type: ignore[method-assign] - vspin.request_tachometer = AsyncMock(return_value=100000) # type: ignore[method-assign] + vspin.request_tachometer = AsyncMock( # type: ignore[method-assign] + return_value=100000 + ) vspin.request_position = AsyncMock( # type: ignore[method-assign] - side_effect=[0, 10000000] + side_effect=[0, 10000000, 20000000] + ) + vspin.request_positions_and_tachometer = AsyncMock( # type: ignore[method-assign] + return_value=_nmc.ServoStatus(status=_nmc.STATUS_MOVE_DONE, velocity=0) + ) + vspin._raise_for_spin_faults = AsyncMock() # type: ignore[method-assign] + vspin._send_nmc = AsyncMock( # type: ignore[method-assign] + return_value=_nmc.NMCResponse(status=0, data=b"") ) - vspin.request_home_position = AsyncMock(side_effect=[0, 1]) # type: ignore[method-assign] - vspin.send_command = AsyncMock(return_value=b"") # type: ignore[method-assign] events: list[PLREvent] = [] event_bus = EventBus() event_bus.subscribe(events.append) @@ -52,6 +209,25 @@ async def test_spin_emits_loaded_bucket_resources_and_parameters(self): self.assertNotIn("relative_centrifugal_force_g", started.data) self.assertNotIn("duration_seconds", started.data) + rpm = VSpin.g_to_rpm(500) + spin_target = _nmc.spin_target_distance(rpm, duration=1, acceleration=0.5) + expected_spin_command = _nmc.build_load_trajectory( + _nmc.PIC_SERVO_ADDRESS, + 0x97, + position=spin_target, + velocity=_nmc.rpm_to_nmc_velocity(rpm), + acceleration=_nmc.acceleration_to_nmc(0.5), + ) + expected_deceleration_command = _nmc.build_load_trajectory( + _nmc.PIC_SERVO_ADDRESS, + 0xB6, + velocity=0, + acceleration=_nmc.acceleration_to_nmc(0.6), + ) + commands = [call.args[0] for call in vspin._send_nmc.await_args_list] + self.assertIn(expected_spin_command, commands) + self.assertIn(expected_deceleration_command, commands) + async def test_spin_failure_emits_requested_parameters(self): vspin = VSpin(name="centrifuge", device_id="test") events: list[PLREvent] = [] @@ -77,12 +253,19 @@ async def test_spin_accepts_positional_parameters_with_event_bus(self): vspin.request_door_open = AsyncMock(return_value=False) # type: ignore[method-assign] vspin.request_door_locked = AsyncMock(return_value=True) # type: ignore[method-assign] vspin.request_bucket_locked = AsyncMock(return_value=False) # type: ignore[method-assign] - vspin.request_tachometer = AsyncMock(return_value=100000) # type: ignore[method-assign] + vspin.request_tachometer = AsyncMock( # type: ignore[method-assign] + return_value=100000 + ) vspin.request_position = AsyncMock( # type: ignore[method-assign] - side_effect=[0, 10000000] + side_effect=[0, 10000000, 20000000] + ) + vspin.request_positions_and_tachometer = AsyncMock( # type: ignore[method-assign] + return_value=_nmc.ServoStatus(status=_nmc.STATUS_MOVE_DONE, velocity=0) + ) + vspin._raise_for_spin_faults = AsyncMock() # type: ignore[method-assign] + vspin._send_nmc = AsyncMock( # type: ignore[method-assign] + return_value=_nmc.NMCResponse(status=0, data=b"") ) - vspin.request_home_position = AsyncMock(side_effect=[0, 1]) # type: ignore[method-assign] - vspin.send_command = AsyncMock(return_value=b"") # type: ignore[method-assign] events: list[PLREvent] = [] event_bus = EventBus() event_bus.subscribe(events.append) @@ -97,6 +280,754 @@ async def test_spin_accepts_positional_parameters_with_event_bus(self): self.assertEqual(started.data["deceleration_fraction"], 0.6) +class TestVSpinProtocol(unittest.IsolatedAsyncioTestCase): + def setUp(self): + self.ftdi_patch = patch("pylabrobot.agilent.vspin.vspin.FTDI", autospec=True) + ftdi_class = self.ftdi_patch.start() + self.addCleanup(self.ftdi_patch.stop) + self.io = ftdi_class.return_value + self.vspin = VSpin(name="centrifuge") + + async def test_position_status_uses_fixed_length_and_checksum(self): + response = bytes.fromhex("11222500004f000018e0050000a4") + self.io.write = AsyncMock(return_value=4) + self.io.read = AsyncMock(side_effect=[response[:5], response[5:]]) + + status = await self.vspin.request_positions_and_tachometer() + + self.assertEqual(status.status, 0x11) + self.assertEqual(status.position, 0x2522) + self.assertEqual(status.velocity, 0) + self.assertEqual(status.home_position, 0x05E0) + self.io.write.assert_awaited_once_with(_nmc.build_no_op(_nmc.PIC_SERVO_ADDRESS)) + self.assertEqual([call.args[0] for call in self.io.read.await_args_list], [14, 9]) + + async def test_position_status_rejects_bad_checksum(self): + response = bytearray.fromhex("11222500004f000018e0050000a4") + response[-1] ^= 0xFF + self.io.write = AsyncMock(return_value=4) + self.io.read = AsyncMock(return_value=bytes(response)) + + with self.assertRaisesRegex( + _nmc.NMCProtocolError, + rf"command {_nmc.build_no_op(_nmc.PIC_SERVO_ADDRESS).hex()}.*" + rf"response {bytes(response).hex()}.*checksum mismatch", + ): + await self.vspin.request_positions_and_tachometer() + + async def test_send_nmc_timeout_includes_command_and_partial_response(self): + command = _nmc.build_no_op(_nmc.PIC_SERVO_ADDRESS) + self.io.write = AsyncMock(return_value=len(command)) + self.io.read = AsyncMock(side_effect=[b"\x01", b""]) + + with self.assertRaisesRegex( + TimeoutError, + rf"command {command.hex()} timed out.*1 of 2 expected.*01", + ): + await self.vspin._send_nmc(command, timeout=0) + + async def test_exact_response_times_out_with_partial_bytes(self): + self.io.read = AsyncMock(side_effect=[b"\x01", b""]) + + with self.assertRaisesRegex(TimeoutError, "1 of 2 expected"): + await self.vspin._read_exact_response(length=2, timeout=0) + + async def test_send_nmc_uses_active_status_mask_length(self): + self.vspin._servo_status_mask = _nmc.SEND_POSITION | _nmc.SEND_VELOCITY + response = bytes.fromhex("0101000000020004") + self.vspin.send_command = AsyncMock(return_value=response) # type: ignore[method-assign] + + parsed = await self.vspin._send_nmc(_nmc.build_no_op(_nmc.PIC_SERVO_ADDRESS)) + + self.assertEqual(parsed, _nmc.NMCResponse(status=1, data=bytes.fromhex("010000000200"))) + self.vspin.send_command.assert_awaited_once_with( # type: ignore[attr-defined] + _nmc.build_no_op(_nmc.PIC_SERVO_ADDRESS), + expected_response_length=8, + read_timeout=0.2, + ) + + async def test_servo_enable_sequence_uses_vendor_transition_delays(self): + self.vspin._send_nmc = AsyncMock() # type: ignore[method-assign] + + with patch("pylabrobot.agilent.vspin.vspin.asyncio.sleep", new=AsyncMock()) as sleep: + await self.vspin._enable_amplifier_and_reset_servo_status() + + sleep.assert_has_awaits( + [ + call(vspin_module._SERVO_TRANSITION_SETTLE_TIME), + call(vspin_module._SERVO_TRANSITION_SETTLE_TIME), + ] + ) + self.vspin._send_nmc.assert_has_awaits( # type: ignore[attr-defined] + [ + call(_nmc.build_stop_motor(_nmc.PIC_SERVO_ADDRESS, _nmc.MOTOR_OFF)), + call(_nmc.build_set_gain(_nmc.PIC_SERVO_ADDRESS, vspin_module._POSITION_GAINS)), + call(_nmc.build_stop_motor(_nmc.PIC_SERVO_ADDRESS, _nmc.STOP_ABRUPT)), + call(_nmc.build_stop_motor(_nmc.PIC_SERVO_ADDRESS, _nmc.AMPLIFIER_ENABLE)), + call(_nmc.build_clear_bits(_nmc.PIC_SERVO_ADDRESS)), + ] + ) + + async def test_servo_disable_sequence_uses_vendor_transition_delays(self): + self.vspin._send_nmc = AsyncMock() # type: ignore[method-assign] + + with patch("pylabrobot.agilent.vspin.vspin.asyncio.sleep", new=AsyncMock()) as sleep: + await self.vspin._disable_servo_after_motion() + + sleep.assert_has_awaits( + [ + call(vspin_module._SERVO_TRANSITION_SETTLE_TIME), + call(vspin_module._SERVO_TRANSITION_SETTLE_TIME), + ] + ) + self.vspin._send_nmc.assert_awaited_once_with( # type: ignore[attr-defined] + _nmc.build_stop_motor(_nmc.PIC_SERVO_ADDRESS, _nmc.MOTOR_OFF) + ) + + async def test_io_sensor_polarities_match_vspin_wiring(self): + self.vspin._request_input_flags = AsyncMock( # type: ignore[method-assign] + return_value=(1 << _nmc.INPUT_DOOR_OPEN) | (1 << _nmc.INPUT_BUCKET_LOCKED) + ) + + self.assertTrue(await self.vspin.request_door_open()) + self.assertTrue(await self.vspin.request_door_locked()) + self.assertFalse(await self.vspin.request_bucket_locked()) + + async def test_io_output_updates_preserve_other_output_bits(self): + self.vspin._io_output_word = 1 << _nmc.OUTPUT_BUCKET_LOCK_CYLINDER + self.vspin._send_nmc = AsyncMock( # type: ignore[method-assign] + return_value=_nmc.NMCResponse(status=0, data=b"") + ) + + await self.vspin._set_io_output_bit(_nmc.OUTPUT_DOOR_LOCK_CYLINDER, True) + + expected_word = (1 << _nmc.OUTPUT_BUCKET_LOCK_CYLINDER) | (1 << _nmc.OUTPUT_DOOR_LOCK_CYLINDER) + self.vspin._send_nmc.assert_awaited_once_with( # type: ignore[attr-defined] + _nmc.build_set_output(_nmc.PIC_IO_ADDRESS, expected_word) + ) + self.assertEqual(self.vspin._io_output_word, expected_word) + + async def test_position_wait_reports_last_position(self): + self.vspin.request_position = AsyncMock(return_value=25) # type: ignore[method-assign] + + with self.assertRaisesRegex(TimeoutError, "last position was 25"): + await self.vspin._wait_for_position(100, timeout=0, operation="test motion") + + async def test_spin_faults_decode_ground_truth_io_bits(self): + self.vspin._request_input_flags = AsyncMock( # type: ignore[method-assign] + return_value=1 << _nmc.INPUT_IMBALANCE + ) + + with self.assertRaisesRegex(RuntimeError, "imbalance"): + await self.vspin._raise_for_spin_faults() + + async def test_spin_rejects_long_run_position_overflow_before_servo_motion(self): + self.vspin.request_door_open = AsyncMock(return_value=False) # type: ignore[method-assign] + self.vspin.request_door_locked = AsyncMock(return_value=True) # type: ignore[method-assign] + self.vspin.request_bucket_locked = AsyncMock(return_value=False) # type: ignore[method-assign] + self.vspin.request_position = AsyncMock(return_value=2**31 - 1) # type: ignore[method-assign] + self.vspin._send_nmc = AsyncMock() # type: ignore[method-assign] + + with self.assertRaisesRegex(NotImplementedError, "signed 32-bit position"): + await self.vspin.spin(g=500, duration=1) + + self.vspin._send_nmc.assert_not_awaited() # type: ignore[attr-defined] + + async def test_stop_spin_commands_deceleration_and_confirms_zero_speed(self): + self.vspin._spin_active = True + self.vspin.request_tachometer = AsyncMock(return_value=1000) # type: ignore[method-assign] + self.vspin.request_positions_and_tachometer = AsyncMock( # type: ignore[method-assign] + side_effect=[ + _nmc.ServoStatus(status=0, velocity=0), + _nmc.ServoStatus(status=_nmc.STATUS_MOVE_DONE, velocity=-1), + _nmc.ServoStatus(status=_nmc.STATUS_MOVE_DONE, velocity=0), + ] + ) + self.vspin._raise_for_spin_faults = AsyncMock() # type: ignore[method-assign] + self.vspin._command_deceleration = AsyncMock() # type: ignore[method-assign] + + await self.vspin.stop_spin(deceleration=0.5) + + self.assertTrue(self.vspin._spin_cancel_requested) + self.vspin._command_deceleration.assert_awaited_once_with(0.5) # type: ignore[attr-defined] + self.assertEqual( # type: ignore[attr-defined] + self.vspin.request_positions_and_tachometer.await_count, + 3, + ) + + async def test_deceleration_timeout_reports_motion_status_and_velocity(self): + self.vspin._raise_for_spin_faults = AsyncMock() # type: ignore[method-assign] + self.vspin.request_positions_and_tachometer = AsyncMock( # type: ignore[method-assign] + return_value=_nmc.ServoStatus(status=_nmc.STATUS_MOVE_DONE, velocity=-1) + ) + + with ( + patch("pylabrobot.agilent.vspin.vspin._SPIN_TIMEOUT_MARGIN", 0), + self.assertRaisesRegex( + TimeoutError, + "last status was 0x01 at 14.7 RPM", + ), + ): + await self.vspin._wait_until_stopped(initial_rpm=0, deceleration=0.5) + + async def test_bucket_calibration_is_normalized_and_saved_consistently(self): + self.vspin.request_position = AsyncMock(return_value=12_345) # type: ignore[method-assign] + self.vspin.request_home_position = AsyncMock(return_value=400) # type: ignore[method-assign] + self.io.request_serial = AsyncMock(return_value="vspin-serial") + + with patch("pylabrobot.agilent.vspin.vspin._save_vspin_calibrations") as save: + await self.vspin.set_bucket_1_position_to_current() + + self.assertEqual(self.vspin.bucket_1_remainder, 4055) + save.assert_called_once_with("vspin-serial", 4055) + + async def test_bucket_targets_use_shortest_path_independently(self): + self.vspin._bucket_1_remainder = 100 + self.vspin.request_home_position = AsyncMock(return_value=500) # type: ignore[method-assign] + self.vspin.request_position = AsyncMock(return_value=7900) # type: ignore[method-assign] + + self.assertEqual(await self.vspin.request_bucket_1_position(), 8400) + self.assertEqual(await self.vspin.request_bucket_2_position(), 4400) + + async def test_bucket_target_uses_saved_home_position_after_spin(self): + self.vspin._bucket_1_remainder = 100 + self.vspin._home_position = 500 + self.vspin.request_home_position = AsyncMock() # type: ignore[method-assign] + self.vspin.request_position = AsyncMock(return_value=7900) # type: ignore[method-assign] + + self.assertEqual(await self.vspin.request_bucket_1_position(), 8400) + self.vspin.request_home_position.assert_not_awaited() # type: ignore[attr-defined] + + async def test_bucket_presentation_retries_alignment_one_revolution_later(self): + self.vspin.request_bucket_1_position = AsyncMock(return_value=8400) # type: ignore[method-assign] + self.vspin.go_to_position = AsyncMock( # type: ignore[method-assign] + side_effect=[vspin_module._PositionAlignmentError("misaligned"), None] + ) + + await self.vspin.go_to_bucket1() + + self.vspin.go_to_position.assert_has_awaits([call(8400), call(16400)]) # type: ignore[attr-defined] + self.assertIs(self.vspin.at_bucket, self.vspin.bucket1) + + +class TestVSpinScriptedFTDI(unittest.IsolatedAsyncioTestCase): + def setUp(self): + self.ftdi_patch = patch("pylabrobot.agilent.vspin.vspin.FTDI", autospec=True) + self.ftdi_patch.start() + self.addCleanup(self.ftdi_patch.stop) + + def _make_vspin(self, steps: list[_VSpinScriptStep]) -> tuple[VSpin, _ScriptedVSpinFTDI]: + vspin = VSpin(name="centrifuge") + io = _ScriptedVSpinFTDI(steps) + vspin.io = io # type: ignore[assignment] + vspin._servo_status_mask = _SERVO_STATUS_MASK + vspin._io_status_mask = _IO_STATUS_MASK + return vspin, io + + @staticmethod + def _bucket_presentation_steps( + current_position: int, target_position: int + ) -> list[_VSpinScriptStep]: + io_status = _nmc.build_no_op(_nmc.PIC_IO_ADDRESS) + servo_status = _nmc.build_no_op(_nmc.PIC_SERVO_ADDRESS) + closed_locked_bucket_unlocked = 1 << _nmc.INPUT_BUCKET_LOCKED + position_trajectory = _nmc.build_load_trajectory( + _nmc.PIC_SERVO_ADDRESS, + vspin_module._POSITION_TRAJECTORY_MODE, + position=target_position, + velocity=0x28F5C3, + acceleration=0x1AD7, + ) + return [ + _servo_step(servo_status, position=current_position), + _io_step(io_status, inputs=closed_locked_bucket_unlocked), + _io_step(io_status, inputs=closed_locked_bucket_unlocked), + _io_step(io_status, inputs=closed_locked_bucket_unlocked), + _io_step(io_status, inputs=closed_locked_bucket_unlocked), + _servo_step(_nmc.build_stop_motor(_nmc.PIC_SERVO_ADDRESS, _nmc.MOTOR_OFF)), + _servo_step(_nmc.build_set_gain(_nmc.PIC_SERVO_ADDRESS, vspin_module._POSITION_GAINS)), + _servo_step(_nmc.build_stop_motor(_nmc.PIC_SERVO_ADDRESS, _nmc.STOP_ABRUPT)), + _servo_step(_nmc.build_stop_motor(_nmc.PIC_SERVO_ADDRESS, _nmc.AMPLIFIER_ENABLE)), + _servo_step(_nmc.build_clear_bits(_nmc.PIC_SERVO_ADDRESS)), + _servo_step(_nmc.build_set_gain(_nmc.PIC_SERVO_ADDRESS, vspin_module._POSITION_GAINS)), + _servo_step(position_trajectory, status=0, position=current_position), + _servo_step(servo_status, position=target_position), + _servo_step(_nmc.build_stop_motor(_nmc.PIC_SERVO_ADDRESS, _nmc.MOTOR_OFF)), + _io_step(io_status, inputs=closed_locked_bucket_unlocked), + _io_step(_nmc.build_set_output(_nmc.PIC_IO_ADDRESS, 0x0100), inputs=0), + _io_step(io_status, inputs=0), + _io_step(io_status, inputs=0), + _io_step( + _nmc.build_set_output(_nmc.PIC_IO_ADDRESS, 0x0500), + inputs=1 << _nmc.INPUT_DOOR_LOCKED, + ), + _io_step(io_status, inputs=1 << _nmc.INPUT_DOOR_LOCKED), + _io_step(io_status, inputs=1 << _nmc.INPUT_DOOR_LOCKED), + _io_step( + _nmc.build_set_output(_nmc.PIC_IO_ADDRESS, 0x0700), + inputs=(1 << _nmc.INPUT_DOOR_LOCKED) | (1 << _nmc.INPUT_DOOR_OPEN), + ), + _io_step( + io_status, + inputs=(1 << _nmc.INPUT_DOOR_LOCKED) | (1 << _nmc.INPUT_DOOR_OPEN), + ), + ] + + @staticmethod + def _network_reset_steps(stale_response: bytes | None = None) -> list[_VSpinScriptStep]: + steps = [_VSpinScriptStep(b"\x00" * 20, None)] + steps.extend( + _VSpinScriptStep( + _nmc.build_no_op(address) + b"\x00" * 8, + None, + ) + for address in range(33) + ) + steps.append(_VSpinScriptStep(_nmc.build_hard_reset(), stale_response)) + return steps + + @classmethod + def _setup_steps(cls) -> list[_VSpinScriptStep]: + steps = cls._network_reset_steps() + steps.extend(cls._network_reset_steps()) + steps.extend( + [ + _empty_step(_nmc.build_set_address(_nmc.PIC_SERVO_ADDRESS)), + _VSpinScriptStep( + _nmc.build_read_status(_nmc.PIC_SERVO_ADDRESS, _nmc.SEND_MODULE_ID), + _nmc_response( + _nmc.STATUS_MOVE_DONE, + bytes([_nmc.PIC_SERVO_MODULE_TYPE, 1]), + ), + ), + _empty_step(_nmc.build_set_address(_nmc.PIC_IO_ADDRESS)), + _VSpinScriptStep( + _nmc.build_read_status(_nmc.PIC_IO_ADDRESS, _nmc.SEND_MODULE_ID), + _nmc_response( + _nmc.STATUS_MOVE_DONE, + bytes([_nmc.PIC_IO_MODULE_TYPE, 1]), + ), + ), + _VSpinScriptStep(_nmc.build_set_address(3), None), + _VSpinScriptStep(_nmc.build_set_baud(57600), None), + _servo_step(_nmc.build_define_status(_nmc.PIC_SERVO_ADDRESS, _SERVO_STATUS_MASK)), + ] + ) + steps.extend( + _empty_step(_nmc.build_set_io_direction(_nmc.PIC_IO_ADDRESS, 0x0FFF)) for _ in range(8) + ) + steps.extend( + _empty_step(_nmc.build_set_io_direction(_nmc.PIC_IO_ADDRESS, direction)) + for direction in (0x0FDF, 0x0EDF, 0x0CDF, 0x08DF) + ) + steps.extend(_empty_step(_nmc.build_set_output(_nmc.PIC_IO_ADDRESS, 0)) for _ in range(4)) + safe_inputs = 1 << _nmc.INPUT_BUCKET_LOCKED + steps.append( + _io_step( + _nmc.build_define_status(_nmc.PIC_IO_ADDRESS, _IO_STATUS_MASK), + inputs=safe_inputs, + ) + ) + for _ in range(5): + steps.extend( + [ + _io_step( + _nmc.build_set_output( + _nmc.PIC_IO_ADDRESS, + 1 << _nmc.OUTPUT_VERSION_TOGGLE, + ), + inputs=safe_inputs, + ), + _io_step( + _nmc.build_set_output(_nmc.PIC_IO_ADDRESS, 0), + inputs=safe_inputs, + ), + ] + ) + io_status = _nmc.build_no_op(_nmc.PIC_IO_ADDRESS) + steps.extend( + [ + _io_step(io_status, inputs=safe_inputs), + _io_step(io_status, inputs=safe_inputs), + _io_step( + _nmc.build_set_output(_nmc.PIC_IO_ADDRESS, 0), + inputs=safe_inputs, + ), + _io_step(io_status, inputs=safe_inputs), + _servo_step(_nmc.build_stop_motor(_nmc.PIC_SERVO_ADDRESS, _nmc.MOTOR_OFF)), + _servo_step(_nmc.build_set_gain(_nmc.PIC_SERVO_ADDRESS, vspin_module._POSITION_GAINS)), + _servo_step(_nmc.build_stop_motor(_nmc.PIC_SERVO_ADDRESS, _nmc.STOP_ABRUPT)), + _servo_step(_nmc.build_stop_motor(_nmc.PIC_SERVO_ADDRESS, _nmc.AMPLIFIER_ENABLE)), + _servo_step(_nmc.build_clear_bits(_nmc.PIC_SERVO_ADDRESS)), + _servo_step(_nmc.build_reset_position(_nmc.PIC_SERVO_ADDRESS)), + _servo_step(_nmc.build_set_gain(_nmc.PIC_SERVO_ADDRESS, vspin_module._HOMING_GAINS)), + _servo_step( + _nmc.build_load_trajectory( + _nmc.PIC_SERVO_ADDRESS, + vspin_module._VELOCITY_TRAJECTORY_MODE, + velocity=0x8312, + acceleration=0x0112, + ), + status=_nmc.STATUS_HOMING_IN_PROGRESS, + ), + _servo_step( + _nmc.build_set_homing(_nmc.PIC_SERVO_ADDRESS, 0x28), + status=_nmc.STATUS_HOMING_IN_PROGRESS, + ), + _servo_step( + _nmc.build_no_op(_nmc.PIC_SERVO_ADDRESS), + status=_nmc.STATUS_HOMING_IN_PROGRESS, + position=100, + ), + _servo_step( + _nmc.build_no_op(_nmc.PIC_SERVO_ADDRESS), + position=200, + home_position=200, + ), + _servo_step(_nmc.build_stop_motor(_nmc.PIC_SERVO_ADDRESS, _nmc.MOTOR_OFF)), + _servo_step(_nmc.build_set_gain(_nmc.PIC_SERVO_ADDRESS, vspin_module._POSITION_GAINS)), + _servo_step(_nmc.build_stop_motor(_nmc.PIC_SERVO_ADDRESS, _nmc.STOP_ABRUPT)), + _servo_step(_nmc.build_stop_motor(_nmc.PIC_SERVO_ADDRESS, _nmc.AMPLIFIER_ENABLE)), + _servo_step(_nmc.build_clear_bits(_nmc.PIC_SERVO_ADDRESS)), + _servo_step(_nmc.build_set_gain(_nmc.PIC_SERVO_ADDRESS, vspin_module._POSITION_GAINS)), + _servo_step( + _nmc.build_load_trajectory( + _nmc.PIC_SERVO_ADDRESS, + vspin_module._POSITION_TRAJECTORY_MODE, + position=0, + velocity=0x28F5C3, + acceleration=0x1AD7, + ), + status=0, + position=200, + home_position=200, + ), + _servo_step( + _nmc.build_no_op(_nmc.PIC_SERVO_ADDRESS), + status=0, + position=50, + home_position=200, + ), + _servo_step( + _nmc.build_no_op(_nmc.PIC_SERVO_ADDRESS), + position=0, + home_position=200, + ), + _servo_step(_nmc.build_stop_motor(_nmc.PIC_SERVO_ADDRESS, _nmc.MOTOR_OFF)), + _io_step(io_status, inputs=safe_inputs), + _io_step(io_status, inputs=safe_inputs), + ] + ) + return steps + + async def test_complete_setup_and_homing_ftdi_transcript(self): + vspin, io = self._make_vspin(self._setup_steps()) + + with ( + patch("pylabrobot.agilent.vspin.vspin.asyncio.sleep", new=AsyncMock()), + patch("pylabrobot.agilent.vspin.vspin._NETWORK_PROBE_TIMEOUT", 0), + ): + await vspin.setup() + + io.assert_complete(self) + self.assertTrue(io.setup_called) + self.assertEqual(io.setup_call_count, 4) + self.assertEqual(io.stop_call_count, 3) + self.assertEqual(io.latency_timers, [16, 16, 16, 16]) + self.assertEqual(io.line_properties, [(8, 1, 0)] * 4) + self.assertEqual(io.flow_controls, [0, 0, 0, 0]) + self.assertEqual(io.baudrates, [19200, 19200, 19200, 19200, 57600]) + self.assertEqual(io.rx_purge_count, 3) + self.assertEqual(io.rts_levels, [True]) + self.assertEqual(io.dtr_levels, [True]) + self.assertEqual(vspin._home_position, 200) + + async def test_network_initialization_probes_the_next_baudrate(self): + steps = self._network_reset_steps() + steps.extend(self._network_reset_steps()) + steps.append(_VSpinScriptStep(_nmc.build_set_address(_nmc.PIC_SERVO_ADDRESS), None)) + steps.extend(self._network_reset_steps()) + steps.extend(self._network_reset_steps()) + steps.extend( + [ + _empty_step(_nmc.build_set_address(_nmc.PIC_SERVO_ADDRESS)), + _VSpinScriptStep( + _nmc.build_read_status(_nmc.PIC_SERVO_ADDRESS, _nmc.SEND_MODULE_ID), + _nmc_response(1, bytes([_nmc.PIC_SERVO_MODULE_TYPE, 1])), + ), + _empty_step(_nmc.build_set_address(_nmc.PIC_IO_ADDRESS)), + _VSpinScriptStep( + _nmc.build_read_status(_nmc.PIC_IO_ADDRESS, _nmc.SEND_MODULE_ID), + _nmc_response(1, bytes([_nmc.PIC_IO_MODULE_TYPE, 1])), + ), + _VSpinScriptStep(_nmc.build_set_address(3), None), + _VSpinScriptStep(_nmc.build_set_baud(57600), None), + ] + ) + vspin, io = self._make_vspin(steps) + + with ( + patch("pylabrobot.agilent.vspin.vspin.asyncio.sleep", new=AsyncMock()), + patch("pylabrobot.agilent.vspin.vspin._NETWORK_PROBE_TIMEOUT", 0), + ): + await vspin._initialize_nmc_network() + + io.assert_complete(self) + self.assertEqual(io.setup_call_count, 5) + self.assertEqual(io.stop_call_count, 5) + self.assertEqual(io.baudrates, [19200, 19200, 19200, 115200, 19200, 19200, 57600]) + self.assertEqual(io.rx_purge_count, 5) + + async def test_network_reopens_to_discard_stale_reset_responses(self): + steps = self._network_reset_steps(stale_response=b"\xfa") + steps.extend(self._network_reset_steps(stale_response=b"\xfb")) + steps.extend( + [ + _empty_step(_nmc.build_set_address(_nmc.PIC_SERVO_ADDRESS)), + _VSpinScriptStep( + _nmc.build_read_status(_nmc.PIC_SERVO_ADDRESS, _nmc.SEND_MODULE_ID), + _nmc_response(1, bytes([_nmc.PIC_SERVO_MODULE_TYPE, 1])), + ), + _empty_step(_nmc.build_set_address(_nmc.PIC_IO_ADDRESS)), + _VSpinScriptStep( + _nmc.build_read_status(_nmc.PIC_IO_ADDRESS, _nmc.SEND_MODULE_ID), + _nmc_response(1, bytes([_nmc.PIC_IO_MODULE_TYPE, 1])), + ), + _VSpinScriptStep(_nmc.build_set_address(3), None), + _VSpinScriptStep(_nmc.build_set_baud(57600), None), + ] + ) + vspin, io = self._make_vspin(steps) + + with ( + patch("pylabrobot.agilent.vspin.vspin.asyncio.sleep", new=AsyncMock()), + patch("pylabrobot.agilent.vspin.vspin._NETWORK_PROBE_TIMEOUT", 0), + ): + await vspin._initialize_nmc_network() + + io.assert_complete(self) + self.assertEqual(io.setup_call_count, 3) + self.assertEqual(io.stop_call_count, 3) + + async def test_network_initialization_rejects_a_third_module(self): + steps = self._network_reset_steps() + steps.extend(self._network_reset_steps()) + steps.extend( + [ + _empty_step(_nmc.build_set_address(_nmc.PIC_SERVO_ADDRESS)), + _VSpinScriptStep( + _nmc.build_read_status(_nmc.PIC_SERVO_ADDRESS, _nmc.SEND_MODULE_ID), + _nmc_response(1, bytes([_nmc.PIC_SERVO_MODULE_TYPE, 1])), + ), + _empty_step(_nmc.build_set_address(_nmc.PIC_IO_ADDRESS)), + _VSpinScriptStep( + _nmc.build_read_status(_nmc.PIC_IO_ADDRESS, _nmc.SEND_MODULE_ID), + _nmc_response(1, bytes([_nmc.PIC_IO_MODULE_TYPE, 1])), + ), + _empty_step(_nmc.build_set_address(3)), + _VSpinScriptStep( + _nmc.build_read_status(3, _nmc.SEND_MODULE_ID), + _nmc_response(1, bytes([_nmc.PIC_SERVO_MODULE_TYPE, 2])), + ), + ] + ) + vspin, io = self._make_vspin(steps) + + with self.assertRaisesRegex(RuntimeError, "unexpected third NMC module"): + await vspin._initialize_nmc_network() + + io.assert_complete(self) + + async def test_complete_spin_ftdi_transcript(self): + g = 500 + duration = 1 + acceleration = 0.5 + deceleration = 0.6 + rpm = VSpin.g_to_rpm(g) + spin_start_position = 0 + cruise_start_position = 100_000 + deceleration_position = int( + cruise_start_position + rpm / 60 * _nmc.COUNTS_PER_REVOLUTION * duration + ) + spin_target = spin_start_position + _nmc.spin_target_distance( + rpm, + duration, + acceleration, + ) + measured_velocity = -int(rpm / abs(vspin_module._TACHOMETER_TO_RPM)) + safe_inputs = 1 << _nmc.INPUT_BUCKET_LOCKED + io_status = _nmc.build_no_op(_nmc.PIC_IO_ADDRESS) + servo_status = _nmc.build_no_op(_nmc.PIC_SERVO_ADDRESS) + spin_trajectory = _nmc.build_load_trajectory( + _nmc.PIC_SERVO_ADDRESS, + vspin_module._POSITION_TRAJECTORY_MODE, + position=spin_target, + velocity=_nmc.rpm_to_nmc_velocity(rpm), + acceleration=_nmc.acceleration_to_nmc(acceleration), + ) + deceleration_trajectory = _nmc.build_load_trajectory( + _nmc.PIC_SERVO_ADDRESS, + vspin_module._VELOCITY_TRAJECTORY_MODE, + velocity=0, + acceleration=_nmc.acceleration_to_nmc(deceleration), + ) + steps = [ + _io_step(io_status, inputs=safe_inputs), + _io_step(io_status, inputs=safe_inputs), + _io_step(io_status, inputs=safe_inputs), + _servo_step(servo_status, position=spin_start_position), + _servo_step(_nmc.build_stop_motor(_nmc.PIC_SERVO_ADDRESS, _nmc.MOTOR_OFF)), + _servo_step(_nmc.build_set_gain(_nmc.PIC_SERVO_ADDRESS, vspin_module._POSITION_GAINS)), + _servo_step(_nmc.build_stop_motor(_nmc.PIC_SERVO_ADDRESS, _nmc.STOP_ABRUPT)), + _servo_step(_nmc.build_stop_motor(_nmc.PIC_SERVO_ADDRESS, _nmc.AMPLIFIER_ENABLE)), + _servo_step(_nmc.build_clear_bits(_nmc.PIC_SERVO_ADDRESS)), + _servo_step(_nmc.build_set_gain(_nmc.PIC_SERVO_ADDRESS, vspin_module._VELOCITY_GAINS)), + _io_step(io_status, inputs=safe_inputs), + _servo_step(spin_trajectory, status=0, position=spin_start_position), + _io_step(io_status, inputs=safe_inputs), + _servo_step( + servo_status, + status=0, + position=50_000, + velocity=measured_velocity, + ), + _servo_step( + servo_status, + status=0, + position=cruise_start_position, + velocity=measured_velocity, + ), + _io_step(io_status, inputs=safe_inputs), + _servo_step( + servo_status, + status=0, + position=deceleration_position, + velocity=measured_velocity, + ), + _servo_step(_nmc.build_set_gain(_nmc.PIC_SERVO_ADDRESS, vspin_module._VELOCITY_GAINS)), + _servo_step(deceleration_trajectory, status=0, position=deceleration_position), + _io_step(io_status, inputs=safe_inputs), + _servo_step( + servo_status, + position=deceleration_position, + velocity=0, + ), + ] + vspin, io = self._make_vspin(steps) + vspin._at_bucket = vspin.bucket1 + + await vspin.spin(g, duration, acceleration, deceleration) + + io.assert_complete(self) + self.assertIsNone(vspin.at_bucket) + self.assertNotIn(_nmc.build_reset_position(_nmc.PIC_SERVO_ADDRESS), io.writes) + self.assertNotIn( + _nmc.build_set_homing(_nmc.PIC_SERVO_ADDRESS, 0x28), + io.writes, + ) + + async def test_complete_abort_ftdi_transcript(self): + initial_velocity = -100 + deceleration = 0.5 + io_status = _nmc.build_no_op(_nmc.PIC_IO_ADDRESS) + servo_status = _nmc.build_no_op(_nmc.PIC_SERVO_ADDRESS) + safe_inputs = 1 << _nmc.INPUT_BUCKET_LOCKED + steps = [ + _servo_step(servo_status, status=0, velocity=initial_velocity), + _servo_step(_nmc.build_set_gain(_nmc.PIC_SERVO_ADDRESS, vspin_module._VELOCITY_GAINS)), + _servo_step( + _nmc.build_load_trajectory( + _nmc.PIC_SERVO_ADDRESS, + vspin_module._VELOCITY_TRAJECTORY_MODE, + velocity=0, + acceleration=_nmc.acceleration_to_nmc(deceleration), + ), + status=0, + velocity=initial_velocity, + ), + _io_step(io_status, inputs=safe_inputs), + _servo_step(servo_status, status=0, velocity=-10), + _io_step(io_status, inputs=safe_inputs), + _servo_step(servo_status, velocity=0), + ] + vspin, io = self._make_vspin(steps) + vspin._spin_active = True + + with patch("pylabrobot.agilent.vspin.vspin.asyncio.sleep", new=AsyncMock()): + await vspin.stop_spin(deceleration) + + io.assert_complete(self) + self.assertTrue(vspin._spin_cancel_requested) + + async def test_complete_bucket_presentation_ftdi_transcripts(self): + current_position = 7900 + cases = ( + ("go_to_bucket1", 8400, "bucket1"), + ("go_to_bucket2", 4400, "bucket2"), + ) + for method_name, target_position, bucket_name in cases: + with self.subTest(bucket=bucket_name): + steps = self._bucket_presentation_steps(current_position, target_position) + vspin, io = self._make_vspin(steps) + vspin._home_position = 500 + vspin._bucket_1_remainder = 100 + + await getattr(vspin, method_name)() + + io.assert_complete(self) + self.assertIs(vspin.at_bucket, getattr(vspin, bucket_name)) + self.assertTrue(vspin.door_open) + + async def test_bucket_motion_fault_prevents_lock_and_door_commands(self): + position = 8400 + steps = self._bucket_presentation_steps(7900, position)[1:13] + steps[-1] = _servo_step( + _nmc.build_no_op(_nmc.PIC_SERVO_ADDRESS), + status=_nmc.STATUS_POSITION_ERROR, + position=7900, + ) + steps.append(_servo_step(_nmc.build_stop_motor(_nmc.PIC_SERVO_ADDRESS, _nmc.MOTOR_OFF))) + vspin, io = self._make_vspin(steps) + + with self.assertRaisesRegex(RuntimeError, "position error.*move to position 8400"): + await vspin.go_to_position(position) + + io.assert_complete(self) + self.assertFalse(vspin.door_open) + + async def test_door_and_lock_operations_are_idempotent(self): + io_status = _nmc.build_no_op(_nmc.PIC_IO_ADDRESS) + steps = [ + _io_step(io_status, inputs=1 << _nmc.INPUT_DOOR_OPEN), + _io_step(io_status, inputs=0), + _io_step(io_status, inputs=0), + _io_step(io_status, inputs=0), + _io_step(io_status, inputs=1 << _nmc.INPUT_DOOR_LOCKED), + _io_step(io_status, inputs=0), + _io_step(io_status, inputs=0), + ] + vspin, io = self._make_vspin(steps) + + await vspin.open_door() + await vspin.close_door() + await vspin.lock_door() + await vspin.unlock_door() + await vspin.lock_bucket() + await vspin.unlock_bucket() + + io.assert_complete(self) + self.assertTrue(all(write == io_status for write in io.writes)) + + async def test_stop_closes_ftdi_without_resetting_the_nmc_network(self): + vspin, io = self._make_vspin([]) + + await vspin.stop() + + io.assert_complete(self) + self.assertTrue(io.stopped) + self.assertEqual(io.writes, []) + + class TestAccess2Events(unittest.IsolatedAsyncioTestCase): def setUp(self): self.vspin_ftdi = patch("pylabrobot.agilent.vspin.vspin.FTDI", autospec=True) @@ -110,6 +1041,9 @@ async def asyncSetUp(self): self.vspin = VSpin(name="centrifuge", device_id="test") self.vspin._door_open = True self.vspin._at_bucket = self.vspin.bucket1 + self.vspin.request_door_open = AsyncMock(return_value=True) # type: ignore[method-assign] + self.vspin.request_bucket_locked = AsyncMock(return_value=True) # type: ignore[method-assign] + self.vspin.request_spinning = AsyncMock(return_value=False) # type: ignore[method-assign] self.loader = Access2(name="loader", device_id="test", vspin=self.vspin) self.loader.driver.load = AsyncMock() # type: ignore[method-assign] self.loader.driver.unload = AsyncMock() # type: ignore[method-assign] @@ -170,3 +1104,37 @@ async def test_unload_failure_emits_bucket_to_loader_transfer(self): self.assertEqual(started.data["source"]["name"], "centrifuge_bucket1") self.assertEqual(started.data["destination"]["name"], "loader") self.assertEqual(failed.data["error_type"], "RuntimeError") + + async def test_load_requires_physical_bucket_lock_before_driver_motion(self): + plate = Resource("plate_1", size_x=1, size_y=1, size_z=1) + self.loader.assign_child_resource(plate, location=Coordinate.zero()) + self.vspin.request_bucket_locked = AsyncMock(return_value=False) # type: ignore[method-assign] + + with self.assertRaisesRegex(RuntimeError, "physically locked"): + await self.loader.load() + + self.loader.driver.load.assert_not_awaited() # type: ignore[attr-defined] + self.assertIs(self.loader.resource, plate) + self.assertIsNone(self.vspin.bucket1.resource) + + async def test_unload_requires_stopped_vspin_before_driver_motion(self): + plate = Resource("plate_1", size_x=1, size_y=1, size_z=1) + self.vspin.bucket1.assign_child_resource(plate, location=Coordinate.zero()) + self.vspin.request_spinning = AsyncMock(return_value=True) # type: ignore[method-assign] + + with self.assertRaisesRegex(RuntimeError, "must be stopped"): + await self.loader.unload() + + self.loader.driver.unload.assert_not_awaited() # type: ignore[attr-defined] + self.assertIs(self.vspin.bucket1.resource, plate) + self.assertIsNone(self.loader.resource) + + async def test_load_requires_physical_door_open_before_driver_motion(self): + plate = Resource("plate_1", size_x=1, size_y=1, size_z=1) + self.loader.assign_child_resource(plate, location=Coordinate.zero()) + self.vspin.request_door_open = AsyncMock(return_value=False) # type: ignore[method-assign] + + with self.assertRaisesRegex(CentrifugeDoorError, "door-open sensor"): + await self.loader.load() + + self.loader.driver.load.assert_not_awaited() # type: ignore[attr-defined]