diff --git a/.github/workflows/docs.yml b/.github/workflows/docs.yml index 90c862f8e22..ad0ea931de5 100644 --- a/.github/workflows/docs.yml +++ b/.github/workflows/docs.yml @@ -30,13 +30,18 @@ jobs: with: python-version: '3.11' + - name: Install Graphviz + run: | + sudo apt-get update + sudo apt-get install -y graphviz + - name: Install dependencies run: pip install -e '.[dev]' - name: Check documentation run: | rm -rf docs/build docs/_autosummary - make docs-check + make docs deploy_docs: name: Build and deploy documentation @@ -55,6 +60,11 @@ jobs: with: python-version: '3.11' + - name: Install Graphviz + run: | + sudo apt-get update + sudo apt-get install -y graphviz + - name: Install dependencies run: pip install -e '.[dev]' diff --git a/docs/_static/devices.json b/docs/_static/devices.json index e1bf47f54a2..fb4c4f4fc38 100644 --- a/docs/_static/devices.json +++ b/docs/_static/devices.json @@ -37,7 +37,7 @@ "capabilities": [ "centrifuging" ], - "status": "mostly", + "status": "full", "api": "pylabrobot.agilent.vspin.VSpin", "api_version": "v1", "code_slug": "agilent/vspin", @@ -53,10 +53,11 @@ "capabilities": [ "centrifuging" ], - "status": "basic", + "status": "full", "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/_static/graphviz.js b/docs/_static/graphviz.js new file mode 100644 index 00000000000..d4a28399d6c --- /dev/null +++ b/docs/_static/graphviz.js @@ -0,0 +1,13 @@ +/** Link each generated SVG to its full-size view without depending on Sphinx's hashed filename. */ +document.addEventListener("DOMContentLoaded", () => { + document.querySelectorAll("object.graphviz").forEach((diagram) => { + const paragraph = document.createElement("p"); + const link = document.createElement("a"); + link.href = diagram.data; + link.textContent = "Open full-size diagram"; + link.target = "_blank"; + link.rel = "noopener"; + paragraph.appendChild(link); + diagram.parentElement.appendChild(paragraph); + }); +}); diff --git a/docs/api/pylabrobot.agilent.rst b/docs/api/pylabrobot.agilent.rst index b67cf81071c..4b0c1bd7b6d 100644 --- a/docs/api/pylabrobot.agilent.rst +++ b/docs/api/pylabrobot.agilent.rst @@ -86,9 +86,14 @@ BioTek Synergy H1 SynergyH1 +.. _vspin-api: + VSpin ----- +For operation states, plate-transfer coordination, and recovery behavior, see the +:doc:`VSpin and Access2 state-machine guide `. + .. currentmodule:: pylabrobot.agilent.vspin .. autosummary:: @@ -99,6 +104,8 @@ VSpin VSpin Access2 Access2Driver + ServoStatus + Access2Status PlateLoc diff --git a/docs/conf.py b/docs/conf.py index af448e26f50..a48922340a2 100644 --- a/docs/conf.py +++ b/docs/conf.py @@ -42,6 +42,7 @@ "sphinx.ext.autosectionlabel", "sphinx.ext.intersphinx", "sphinx.ext.mathjax", + "sphinx.ext.graphviz", "myst_nb", "sphinx_copybutton", "IPython.sphinxext.ipython_console_highlighting", @@ -87,6 +88,9 @@ # html_theme = "pydata_sphinx_theme" +# Render DOT diagrams as scalable images during HTML builds. +graphviz_output_format = "svg" + # Add any paths that contain custom static files (such as style sheets) here, # relative to this directory. They are copied after the builtin static files, # so a file named "default.css" will overwrite the builtin "default.css". @@ -103,6 +107,7 @@ html_css_files.append("plr_cards.css") # served from _static/plr_cards.css html_js_files = list(globals().get("html_js_files", [])) +html_js_files.append("graphviz.js") if "plr_cards.js" not in html_js_files: html_js_files.append("plr_cards.js") # served from _static/plr_cards.js diff --git a/docs/contributor_guide/contributing.md b/docs/contributor_guide/contributing.md index 33d98322908..8ee0d9139b6 100644 --- a/docs/contributor_guide/contributing.md +++ b/docs/contributor_guide/contributing.md @@ -69,6 +69,11 @@ Use PyLabRobot's [default units](../user_guide/getting-started/units.md) in publ It is important that you write documentation for your code. As a rule of thumb, all functions and classes, whether public or private, are required to have a docstring. PyLabRobot uses [Google Style Python Docstrings](https://sphinxcontrib-napoleon.readthedocs.io/en/latest/example_google.html). In addition, PyLabRobot uses [type hints](https://docs.python.org/3/library/typing.html) to document the types of variables. +Install the Graphviz system package before building documentation (`sudo apt-get install graphviz` +on Debian/Ubuntu or `brew install graphviz` on macOS). The `dot` executable must be on `PATH`. +Sphinx renders diagrams from their `.dot` sources during the build; edit the source rather than +saving generated SVG files in the documentation source tree. + To build the documentation, run `make docs` in the root directory. The documentation will be built in `docs/build`. Run `open docs/build/index.html` to open the documentation in your browser. ## Common Tasks diff --git a/docs/contributor_guide/device-driver-guide.md b/docs/contributor_guide/device-driver-guide.md index fa98270f065..e93ae45b791 100644 --- a/docs/contributor_guide/device-driver-guide.md +++ b/docs/contributor_guide/device-driver-guide.md @@ -28,6 +28,8 @@ Keep it small and idiomatic to PyLabRobot. The public surface must expose **no non-idempotent commands.** If the hardware only offers a raw toggle/flip, keep it private (`_toggle_x`) and expose move-to-state methods (`move_x_out` / `move_x_in`) that read current state, act only if needed, then confirm. This keeps the API safe to call repeatedly — the caller states intent ("be open"), not a blind toggle. +Keep connection, calibration, and state on the device; pass operation-specific settings (plate geometry, grip, offsets, speed, duration) as method arguments without carrying them between calls. + ### Unverified drivers If the driver hasn't been checked against real hardware, say so loudly: `setup()` should `logger.warning(...)` that it's untested and invite a change once someone verifies it. Don't quietly present untested code as ready. diff --git a/docs/user_guide/agilent/vspin/hello-world.ipynb b/docs/user_guide/agilent/vspin/hello-world.ipynb index 86c8dbe8075..a7d0d2624a4 100644 --- a/docs/user_guide/agilent/vspin/hello-world.ipynb +++ b/docs/user_guide/agilent/vspin/hello-world.ipynb @@ -9,7 +9,7 @@ "\n", "The Agilent VSpin is a two-bucket microplate centrifuge. This quickstart connects directly to the\n", "centrifuge, calibrates its bucket positions, runs one balanced spin, and disconnects. The optional\n", - "Access2 plate loader is not covered here.\n", + "Access2 plate loader settings are covered at the end.\n", "\n", "| Property | Value |\n", "|---|---|\n", @@ -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", @@ -356,6 +372,86 @@ "source": [ "await vspin.stop()" ] + }, + { + "cell_type": "markdown", + "id": "access2-operation-parameters", + "metadata": {}, + "source": [ + "## Optional Access2 loader parameters\n", + "\n", + "Use these examples before disconnecting, with an initialized Access2 loader named `loader`\n", + "paired with the connected, homed `vspin`.\n", + "Before `load()`, present an empty bucket and place and assign the plate on the loader stage;\n", + "before `unload()`, present the occupied bucket and leave the loader stage empty. See the\n", + "[state-machine guide](state-machine.md#access2-plate-transfers) for transfer preconditions.\n", + "\n", + "Pass plate-specific settings to each `load()` or `unload()` call. They are not stored on\n", + "the loader, and changing a plate resource's dimensions does not change the motion settings.\n", + "All values below are in millimeters:\n", + "\n", + "| Parameter | `load()` default | `unload()` default |\n", + "|---|---:|---:|\n", + "| `plate_height` | 10 | 10 |\n", + "| `source_z_offset` | 3 | 3 |\n", + "| `destination_z_offset` | 3 | 3 |\n", + "| `park_z_offset` | 3 | 0 |\n", + "| `gripper_open_position` | 0 | 0 |\n", + "| `gripper_closed_position` | 5.68 | 5.68 |\n", + "| `gripper_close_threshold` | 1.5 | 1.5 |\n", + "\n", + "`source_z_offset` applies at pickup and `destination_z_offset` at placement: the source\n", + "is the loader stage for `load()` and the presented bucket for `unload()`. `plate_height`\n", + "is passed to the controller for all three teachpoint moves, including return to park.\n", + "Gripper positions are absolute axis coordinates, not plate widths or jaw gaps.\n", + "The gripper can stop on plate contact before its closed target; acceptance requires\n", + "motion completion, the close threshold, and plate detection.\n", + "\n", + "For example, explicitly passing the default settings for a load looks like:\n", + "\n", + "```python\n", + "await loader.load(\n", + " plate_height=10,\n", + " source_z_offset=3,\n", + " destination_z_offset=3,\n", + " park_z_offset=3,\n", + " gripper_open_position=0,\n", + " gripper_closed_position=5.68,\n", + " gripper_close_threshold=1.5,\n", + ")\n", + "```\n", + "\n", + "Use settings established for the plate and teachpoints in your setup. Every value must\n", + "be finite, height must be positive, and gripper settings must satisfy\n", + "`gripper_open_position < gripper_close_threshold <= gripper_closed_position`.\n", + "Invalid settings are rejected before loader actuation.\n", + "\n", + "Each transfer movement also accepts a speed preset: `\"slow\"`, `\"medium\"`, or `\"fast\"`.\n", + "These are controller presets, not velocities in millimeters per second.\n", + "\n", + "| Parameter | Movement | Default for both directions |\n", + "|---|---|---|\n", + "| `source_speed` | Approach the pickup teachpoint | `\"slow\"` |\n", + "| `destination_speed` | Carry the plate to placement | `\"slow\"` |\n", + "| `park_speed` | Return to park after release | `\"slow\"` |\n", + "| `gripper_open_speed` | Open before pickup | `\"fast\"` |\n", + "| `gripper_close_speed` | Close on the plate | `\"slow\"` |\n", + "| `gripper_release_speed` | Open to release the plate | `\"slow\"` |\n", + "\n", + "Standalone loader movements follow the same per-call pattern:\n", + "\n", + "```python\n", + "await loader.driver.park(plate_height=15, z_offset=8, speed=\"slow\")\n", + "await loader.driver.open_gripper(gripper_open_position=0, speed=\"slow\")\n", + "await loader.driver.close_gripper(\n", + " gripper_closed_position=5.68, gripper_close_threshold=1.5, speed=\"slow\"\n", + ")\n", + "```\n", + "\n", + "The examples show method defaults. Standalone `park()` has its own height and offset\n", + "defaults; it does not reuse the preceding transfer's settings. Speed presets are\n", + "validated before actuation, including movements scheduled later in a transfer." + ] } ], "metadata": { diff --git a/docs/user_guide/agilent/vspin/images/access2-transfer.dot b/docs/user_guide/agilent/vspin/images/access2-transfer.dot new file mode 100644 index 00000000000..fcd5751ac1a --- /dev/null +++ b/docs/user_guide/agilent/vspin/images/access2-transfer.dot @@ -0,0 +1,33 @@ +digraph access2_transfer { + graph [bgcolor="#f8fafc", pad=0.2, rankdir=TB, ranksep=0.5, nodesep=0.35, + splines=polyline]; + node [shape=box, style="rounded,filled", fillcolor="#dcfce7", color="#16a34a", + fontcolor="#0f172a", fontname="Helvetica", fontsize=14, margin="0.12,0.09"]; + edge [color="#475569", fontcolor="#475569", fontname="Helvetica", fontsize=12]; + + start [label="PARKED\nIDLE", group="left"]; + approaching [label="APPROACHING\nSOURCE", group="middle"]; + at_source [label="AT SOURCE", group="right"]; + gripping [label="GRIPPING", group="right"]; + holding [label="HOLDING", group="left"]; + moving [label="MOVING TO\nDESTINATION", group="left"]; + at_destination [label="AT DESTINATION", group="middle"]; + releasing [label="RELEASING", group="right"]; + returning [label="RETURNING\nTO PARK", group="right"]; + + {rank=same; start; approaching; at_source;} + {rank=same; holding; gripping;} + {rank=same; moving; at_destination; releasing;} + + + start -> approaching; + approaching -> at_source; + at_source -> gripping [label="plate detected", weight=10]; + // Reverse the drawn arrows on alternate rows to keep the flow compact. + holding -> gripping [dir=back, label="grip verified"]; + holding -> moving [weight=10]; + moving -> at_destination; + at_destination -> releasing; + releasing -> returning [label="gripper open", weight=10]; + returning -> start [label="park confirmed", constraint=false]; +} diff --git a/docs/user_guide/agilent/vspin/images/lifecycle.dot b/docs/user_guide/agilent/vspin/images/lifecycle.dot new file mode 100644 index 00000000000..dd10344ad07 --- /dev/null +++ b/docs/user_guide/agilent/vspin/images/lifecycle.dot @@ -0,0 +1,45 @@ +digraph vspin_lifecycle { + graph [bgcolor="#f8fafc", pad=0.2, rankdir=TB, ranksep=0.35, nodesep=0.4]; + node [shape=box, style="rounded,filled", fillcolor="#e0f2fe", color="#0284c7", + fontcolor="#0f172a", fontname="Helvetica", fontsize=14, margin="0.12,0.09"]; + edge [color="#475569", fontcolor="#475569", fontname="Helvetica", fontsize=12]; + + connection [label="Connection", shape=plain, fontname="Helvetica-Bold"]; + initialization [label="Initialization", shape=plain, fontname="Helvetica-Bold"]; + homing [label="Homing", shape=plain, fontname="Helvetica-Bold"]; + disconnected [label="DISCONNECTED\ninit/home UNKNOWN"]; + connecting [label="CONNECTING"]; + connected [label="CONNECTED"]; + disconnecting [label="DISCONNECTING"]; + init_unknown [label="UNKNOWN"]; + initializing [label="INITIALIZING"]; + initialized [label="INITIALIZED"]; + home_unknown [label="UNKNOWN"]; + home_in_progress [label="HOMING"]; + homed [label="HOMED"]; + + {rank=same; connection; initialization; homing;} + {rank=same; disconnected; init_unknown; home_unknown;} + {rank=same; connecting; initializing; home_in_progress;} + {rank=same; connected; initialized; homed;} + + connection -> disconnected [style=invis]; + initialization -> init_unknown [style=invis]; + homing -> home_unknown [style=invis]; + disconnected -> connecting [label="setup"]; + connecting -> connected [label="transport open"]; + connected -> disconnecting [label="stop"]; + disconnecting -> disconnected [constraint=false, label="closed"]; + init_unknown -> initializing [label="initialize"]; + initializing -> initialized [label="confirmed"]; + home_unknown -> home_in_progress [label="home"]; + home_in_progress -> homed [label="confirmed"]; + + connected -> initializing [label="required", color="#0369a1", fontcolor="#0369a1", style=dashed, constraint=false]; + initialized -> home_in_progress [label="required", color="#0369a1", fontcolor="#0369a1", style=dashed, constraint=false]; + ready [label="Semantic readiness\nIDLE + no recovery required", fillcolor="#dcfce7", color="#16a34a"]; + connected -> ready [style=dashed, color="#0369a1"]; + initialized -> ready [style=dashed, color="#0369a1"]; + homed -> ready [style=dashed, color="#0369a1"]; + disconnecting -> ready [style=invis]; +} diff --git a/docs/user_guide/agilent/vspin/images/state-machine.dot b/docs/user_guide/agilent/vspin/images/state-machine.dot new file mode 100644 index 00000000000..cbe815f8a14 --- /dev/null +++ b/docs/user_guide/agilent/vspin/images/state-machine.dot @@ -0,0 +1,30 @@ +digraph vspin_activity { + graph [bgcolor="#f8fafc", pad=0.2, layout=neato, overlap=true, splines=true, outputorder=edgesfirst]; + node [shape=box, style="rounded,filled", fillcolor="#ede9fe", color="#8b5cf6", + fontcolor="#0f172a", fontname="Helvetica", fontsize=16, pin=true, margin="0.12,0.09"]; + edge [color="#475569", fontcolor="#475569", fontname="Helvetica", fontsize=14]; + + idle [pos="3,6!", label="IDLE", penwidth=2]; + interlocks [pos="0,6!", label="CHANGING\nINTERLOCKS"]; + positioning [pos="0,3.5!", label="POSITIONING"]; + transferring [pos="0,1!", label="TRANSFERRING"]; + preparing [pos="6,6!", label="PREPARING\nTO SPIN"]; + accelerating [pos="6,3.5!", label="ACCELERATING"]; + at_speed [pos="6,1!", label="AT SPEED"]; + decelerating [pos="3,1!", label="DECELERATING"]; + + idle -> interlocks [label="door / lock"]; + interlocks -> idle [label="confirmed", constraint=false]; + idle -> positioning [label="position"]; + positioning -> idle [label="confirmed", constraint=false]; + idle -> transferring [label="load / unload"]; + transferring -> idle [xlabel="loader parked", constraint=false]; + idle -> preparing [label="spin"]; + preparing -> accelerating [label="trajectory starts"]; + accelerating -> at_speed [label="speed verified"]; + at_speed -> decelerating [label="duration /\nstop_spin"]; + decelerating -> idle [label="stopped", constraint=false]; + preparing -> idle [label="stop before motion", color="#0369a1", + fontcolor="#0369a1", constraint=false]; + accelerating -> decelerating [label="stop_spin", color="#0369a1", fontcolor="#0369a1"]; +} diff --git a/docs/user_guide/agilent/vspin/index.md b/docs/user_guide/agilent/vspin/index.md index 95cbc46cac3..358d0b07b32 100644 --- a/docs/user_guide/agilent/vspin/index.md +++ b/docs/user_guide/agilent/vspin/index.md @@ -4,5 +4,6 @@ :maxdepth: 1 hello-world +state-machine events ``` diff --git a/docs/user_guide/agilent/vspin/state-machine.md b/docs/user_guide/agilent/vspin/state-machine.md new file mode 100644 index 00000000000..48c87f26019 --- /dev/null +++ b/docs/user_guide/agilent/vspin/state-machine.md @@ -0,0 +1,178 @@ +# VSpin and Access2 state machine + +The VSpin centrifuge and optional Access2 loader track which operation is in progress, +which positions have been confirmed, and whether a failure requires physical recovery. +Use this page to interpret their state snapshots and coordinate positioning, spins, +and plate transfers. Start with the [VSpin quickstart](hello-world.ipynb) for connection +and calibration instructions. + +## VSpin activity overview + +```{graphviz} images/state-machine.dot +:layout: neato +:alt: VSpin activity cycles sharing a single IDLE state. Interlocks, positioning, transfers, completed spins, and controlled stops all return to IDLE. +:caption: Every successful workflow returns to the same IDLE state. Blue arrows show controlled stops before the normal at-speed interval completes. Failure after actuation retains the activity and sets recovery_required. +``` + +{download}`Download the Graphviz source ` + +`IDLE` is one activity, shared by all workflows. Ordinary commands leaving it require +[connection and readiness](#connection-and-readiness) plus their physical preconditions. +`load()` and `unload()` additionally require a presented bucket and a parked loader. + +Returning to `IDLE` does not reset position information: bucket positioning confirms +`vspin.at_bucket`, a successful transfer preserves it, and rotor motion during a spin +clears it. `PARKED` combines Access2's `IDLE` operation with its confirmed park +teachpoint. `recovery_required` is a separate flag that can accompany any activity +or transfer phase. + +## Read state and physical status + +State snapshots describe what the driver knows about its current session. Physical +conditions such as door position, rotor motion, axis position, and plate contact come +from fresh controller queries. + +```python +# With an existing, connected vspin and its paired loader: +print(vspin.state) +print(vspin.at_bucket) +print(loader.driver.state) + +# Read current physical conditions; these calls do not move the devices. +door_open = await vspin.request_door_open() +door_locked = await vspin.request_door_locked() +bucket_locked = await vspin.request_bucket_locked() +servo = await vspin.request_positions_and_tachometer() +loader_status = await loader.driver.request_status() +``` + +| Information | Where to read it | +|---|---| +| VSpin connection, initialization, homing, activity, and recovery flag | `vspin.state` | +| Bucket confirmed at the load opening | `vspin.at_bucket`: `vspin.bucket1`, `vspin.bucket2`, or `None` | +| Access2 connection, operation or transfer progress, recovery flag, and last confirmed teachpoint | `loader.driver.state` | +| Access2 initialization, homing, faults, axis positions, and optical plate sensor | `await loader.driver.request_status()` | +| Recorded plate locations | `vspin.bucket1.resource`, `vspin.bucket2.resource`, and `loader.resource` | + +A saved snapshot does not update as the device moves; read `.state` again to observe +progress. Recorded plate locations represent PLR's resource assignments. They do not +measure plate mass or prove that a manually placed plate is present. + +## Connection and readiness + +```{graphviz} images/lifecycle.dot +:alt: VSpin connection, initialization, and homing states, with prerequisite arrows from CONNECTED to INITIALIZING and INITIALIZED to HOMING. CONNECTED, INITIALIZED, and HOMED are all required for semantic readiness. +:caption: Dashed arrows are prerequisites. Initialization requires an open connection; homing requires completed initialization. Readiness requires CONNECTED, INITIALIZED, and HOMED together, plus IDLE and no recovery flag. Physical checks still apply. +``` + +{download}`Download the lifecycle diagram source ` + +`setup()` opens the transport, initializes the controller, and homes the device. VSpin +stores these as separate lifecycle dimensions: `CONNECTED` alone does not mean it is +ready to move. Ordinary VSpin commands require initialization and homing to be complete, +`activity == IDLE`, and `recovery_required == False`, followed by the command's physical +checks. + +For Access2, initialization and homing are read from controller status. Its semantic +state records connection, operation, recovery, and the last confirmed teachpoint. + +`stop()` closes the transport and invalidates session-scoped position knowledge. +Use `stop_spin()` to request a controlled rotor stop; `stop()` is the disconnect method. + +## VSpin activities + +| Activity | What is happening | +|---|---| +| `IDLE` | No VSpin workflow owns the device. Readiness and physical preconditions still apply. | +| `CHANGING_INTERLOCKS` | A door or bucket-lock command is waiting for its sensor confirmation. | +| `POSITIONING` | The rotor is moving to a requested position. | +| `TRANSFERRING` | Access2 has reserved the load opening for a plate transfer. | +| `PREPARING_TO_SPIN` | The driver is preparing the interlocks, amplifier, and spin trajectory. | +| `ACCELERATING` | The rotor is accelerating toward the requested speed. | +| `AT_SPEED` | Target speed has been confirmed and the timed interval is in progress. | +| `DECELERATING` | The driver is bringing the rotor to a verified stop. | + +A completed operation normally returns to `IDLE`. A new VSpin workflow is rejected +while another owns the device, including while an Access2 transfer reserves it. +Status queries remain available during a workflow. + +### Present a bucket + +`go_to_bucket1()` and `go_to_bucket2()` coordinate the door and bucket interlocks, +position the rotor, verify its position, and leave the selected bucket locked at the +open load opening. After success, `vspin.at_bucket` identifies that bucket. + +`go_to_position()` accepts an encoder position but does not establish a named bucket +presentation. Use a bucket method before transferring a plate. A completed spin also +clears the presented-bucket reference, so present a bucket again before the next transfer. + +### Spin and stop + +A full spin follows: + +```text +IDLE → PREPARING_TO_SPIN → ACCELERATING → AT_SPEED → DECELERATING → IDLE +``` + +`spin()` coordinates the interlocks and waits for the cycle to finish. `stop_spin()` +can be called from another task during preparation, acceleration, the at-speed interval, +or deceleration. It asks the active spin workflow to stop and waits for that workflow +to finish. A stop requested during preparation can finish before any rotor motion; +a stop requested during acceleration can skip the at-speed interval. Calling +`stop_spin()` outside a spin returns without starting an operation. + +When running a spin in a separate task, retain that task and await it as well so its +exception is observed. Task cancellation is treated as a failure after actuation, even +when the driver's cleanup stops the rotor successfully. + +## Access2 plate transfers + +Use the paired `Access2.load()` and `Access2.unload()` methods to coordinate the two +devices and update plate ownership. The loader driver performs the physical movement; +the paired wrapper reserves VSpin throughout the transfer. + +Before starting a transfer: + +- Present the intended bucket with `go_to_bucket1()` or `go_to_bucket2()`. +- Confirm the rotor is stopped, the door is open, and the bucket is locked. +- Have Access2 initialized, homed, fault-free, and confirmed parked. +- Keep PLR's resource assignments consistent with the physical plates. The source must + contain a plate and the destination must be empty, including the loader stage for an unload. + +Load and unload follow the same phases with opposite source and destination routes: + +```{graphviz} images/access2-transfer.dot +:alt: A closed Access2 transfer cycle from PARKED through pickup, transport, release, and RETURNING TO PARK, with a confirmed return to the same PARKED state. +:caption: Both load and unload return to the same PARKED state after park confirmation. Follow the arrows across alternating rows. A failure after actuation retains the last phase and requires recovery. +``` + +{download}`Download the transfer diagram source ` + +The driver checks source plate presence, grip, destination motion, gripper opening, +and return to park before completing the transfer. On success, Access2 returns to +`IDLE`, its last teachpoint is park, and PLR moves the plate's resource assignment to +the destination. The same VSpin bucket remains presented. + +Your workflow must keep the loader clear before rotor motion, keep transfer destinations +empty, and provide compatible, balanced plates for spinning. The state model and resource +assignments do not establish those physical conditions by themselves. + +## Rejections and recovery + +A rejected precondition before actuation restores the previous operation state. For +example, attempting a paired transfer while the door is closed is rejected before the +loader starts moving. Correct the precondition before retrying. + +A failure or cancellation after actuation sets `recovery_required` and retains the +activity or transfer phase for diagnosis. An uncertain rotor position clears +`vspin.at_bucket`; an uncertain loader position clears its last teachpoint. If a +paired transfer fails after loader actuation, VSpin also requires recovery and blocks +subsequent motion. Resource assignments are updated only after successful transfers, +so after a failure the physical plate may no longer be where its resource is recorded. + +Stop the workflow and establish the actual arm, plate, rotor, and interlock positions +before recovery. Neither `setup()` nor reconnecting the same driver clears the recovery +flag, and there is no public reset method that makes an interrupted transfer safe. + +See the {ref}`VSpin and Access2 API reference ` +for method signatures and the [events page](events.md) for structured operation records. 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..af2f88466cd 100644 --- a/pylabrobot/agilent/vspin/__init__.py +++ b/pylabrobot/agilent/vspin/__init__.py @@ -1,2 +1,4 @@ -from pylabrobot.agilent.vspin.access2 import Access2, Access2Driver +from pylabrobot.agilent.vspin._access2_protocol import Access2Status +from pylabrobot.agilent.vspin._nmc import ServoStatus +from pylabrobot.agilent.vspin.access2 import Access2, Access2Driver, Access2Speed 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..142d944ffed --- /dev/null +++ b/pylabrobot/agilent/vspin/_access2_protocol.py @@ -0,0 +1,394 @@ +"""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 + +# Verified bit in each axis-status byte returned by GET_STATUS. +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 structurally validated Access2 response.""" + + response_id: int + result: 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 raw Access2 axis-status byte 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.""" + _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 = Reader(data) + result = result_reader.u8() + return Access2Reply(response_id=response_id, result=result, data=result_reader.remaining()) + + +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/_state.py b/pylabrobot/agilent/vspin/_state.py new file mode 100644 index 00000000000..db24590aaaa --- /dev/null +++ b/pylabrobot/agilent/vspin/_state.py @@ -0,0 +1,122 @@ +from __future__ import annotations + +import dataclasses +import enum +from typing import Union + + +class ConnectionState(enum.Enum): + """PLR's ownership of a device transport.""" + + DISCONNECTED = enum.auto() + CONNECTING = enum.auto() + CONNECTED = enum.auto() + DISCONNECTING = enum.auto() + + +class VSpinInitializationState(enum.Enum): + """VSpin NMC initialization known to PLR.""" + + UNKNOWN = enum.auto() + INITIALIZING = enum.auto() + INITIALIZED = enum.auto() + + +class VSpinHomingState(enum.Enum): + """VSpin homing known to PLR.""" + + UNKNOWN = enum.auto() + HOMING = enum.auto() + HOMED = enum.auto() + + +class VSpinActivity(enum.Enum): + """The VSpin workflow currently owning the command lock.""" + + IDLE = enum.auto() + CHANGING_INTERLOCKS = enum.auto() + POSITIONING = enum.auto() + TRANSFERRING = enum.auto() + PREPARING_TO_SPIN = enum.auto() + ACCELERATING = enum.auto() + AT_SPEED = enum.auto() + DECELERATING = enum.auto() + + +@dataclasses.dataclass(frozen=True) +class VSpinMachineState: + """VSpin semantic facts not already available from status or resources.""" + + connection: ConnectionState = ConnectionState.DISCONNECTED + initialization: VSpinInitializationState = VSpinInitializationState.UNKNOWN + homing: VSpinHomingState = VSpinHomingState.UNKNOWN + recovery_required: bool = False + activity: VSpinActivity = VSpinActivity.IDLE + + +class Access2Activity(enum.Enum): + """A non-transfer Access2 operation.""" + + IDLE = enum.auto() + INITIALIZING = enum.auto() + HOMING = enum.auto() + MOVING = enum.auto() + + +class TransferDirection(enum.Enum): + """Direction of one plate transfer.""" + + INTO_CENTRIFUGE = enum.auto() + OUT_OF_CENTRIFUGE = enum.auto() + + +class TransferPhase(enum.Enum): + """Last requested or confirmed phase of an active transfer.""" + + APPROACHING_SOURCE = enum.auto() + AT_SOURCE = enum.auto() + GRIPPING = enum.auto() + HOLDING = enum.auto() + MOVING_TO_DESTINATION = enum.auto() + AT_DESTINATION = enum.auto() + RELEASING = enum.auto() + RETURNING_TO_PARK = enum.auto() + + +@dataclasses.dataclass(frozen=True) +class TransferProgress: + """The single active Access2 transfer operation.""" + + direction: TransferDirection + bucket_teachpoint: int + phase: TransferPhase = TransferPhase.APPROACHING_SOURCE + + +Access2Operation = Union[Access2Activity, TransferProgress] + + +@dataclasses.dataclass(frozen=True) +class Access2MachineState: + """Access2 semantic facts not already available in the controller status.""" + + connection: ConnectionState = ConnectionState.DISCONNECTED + recovery_required: bool = False + operation: Access2Operation = Access2Activity.IDLE + last_teachpoint: int | None = None + + +@dataclasses.dataclass +class TransitionToken: + """Record whether a guarded workflow crossed an actuation boundary.""" + + actuated: bool = False + position_uncertain: bool = False + + def mark_actuated(self, *, position_uncertain: bool = False) -> None: + """Record that hardware may change before the awaited command returns.""" + self.actuated = True + self.position_uncertain |= position_uncertain + + def confirm_position(self) -> None: + """Record that the existing motion waiter confirmed the current position.""" + self.position_uncertain = False diff --git a/pylabrobot/agilent/vspin/access2.py b/pylabrobot/agilent/vspin/access2.py index 6f1e2757662..8b8c3f79697 100644 --- a/pylabrobot/agilent/vspin/access2.py +++ b/pylabrobot/agilent/vspin/access2.py @@ -1,33 +1,110 @@ +from __future__ import annotations + import asyncio +import dataclasses import logging -import time +import math +from contextlib import asynccontextmanager +from typing import AsyncIterator, Literal +from pylabrobot.agilent.vspin import _access2_protocol as protocol +from pylabrobot.agilent.vspin._state import ( + Access2Activity, + Access2MachineState, + Access2Operation, + ConnectionState, + TransferDirection, + TransferPhase, + TransferProgress, + TransitionToken, +) from pylabrobot.agilent.vspin.errors import ( BucketHasPlateError, BucketNoPlateError, - CentrifugeDoorError, LoaderNoPlateError, NotAtBucketError, ) from pylabrobot.agilent.vspin.vspin import VSpin from pylabrobot.events import evented_operation, resource_reference -from pylabrobot.io.ftdi import FTDI +from pylabrobot.io.ftdi import FTDI, is_ftdi_transport_error from pylabrobot.resources import Coordinate, ResourceHolder logger = logging.getLogger(__name__) +_MOTION_POLL_INTERVAL = 0.1 +_AXIS_POSITION_TOLERANCE = 0.1 +_DEFAULT_GRIPPER_OPEN_POSITION = 0.0 +_DEFAULT_GRIPPER_CLOSE_THRESHOLD = 1.5 +_DEFAULT_GRIPPER_CLOSED_POSITION = 5.68 +_AXIS_NAMES: dict[int, str] = { + protocol.AXIS_GRIPPER: "gripper", + protocol.AXIS_Y: "Y", + protocol.AXIS_Z: "Z", +} + +Access2Speed = Literal["slow", "medium", "fast"] + + +def _speed_code(speed: Access2Speed) -> int: + """Translate a named controller speed preset before starting an operation.""" + speeds = { + "slow": protocol.SPEED_SLOW, + "medium": protocol.SPEED_MEDIUM, + "fast": protocol.SPEED_FAST, + } + try: + return speeds[speed] + except (KeyError, TypeError) as error: + raise ValueError("Access2 speed must be 'slow', 'medium', or 'fast'") from error + + +@dataclasses.dataclass(frozen=True) +class TransferRoute: + """Teachpoints for one transfer direction.""" + + source_teachpoint: int + destination_teachpoint: int + source_name: str + -def _loader_load_event_context(self: "Access2") -> dict: +def _transfer_route( + direction: TransferDirection, + bucket_teachpoint: int, +) -> TransferRoute: + """Build one transfer route from existing Access2 protocol constants.""" + if bucket_teachpoint not in ( + protocol.TEACHPOINT_BUCKET_1, + protocol.TEACHPOINT_BUCKET_2, + ): + raise ValueError(f"Invalid bucket teachpoint: {bucket_teachpoint}") + + if direction is TransferDirection.INTO_CENTRIFUGE: + return TransferRoute( + source_teachpoint=protocol.TEACHPOINT_PICK, + destination_teachpoint=bucket_teachpoint, + source_name="stage", + ) + if direction is TransferDirection.OUT_OF_CENTRIFUGE: + return TransferRoute( + source_teachpoint=bucket_teachpoint, + destination_teachpoint=protocol.TEACHPOINT_PICK, + source_name="centrifuge", + ) + raise ValueError(f"Invalid transfer direction: {direction}") + + +def _loader_load_event_context(self: "Access2", **parameters: float | str) -> dict: plate = self.resource return { "device": resource_reference(self), "resources": [] if plate is None else [resource_reference(plate)], "source": resource_reference(self), "destination": resource_reference(self._vspin.at_bucket), + "parameters": parameters, } -def _loader_unload_event_context(self: "Access2") -> dict: +def _loader_unload_event_context(self: "Access2", **parameters: float | str) -> dict: bucket = self._vspin.at_bucket plate = None if bucket is None else bucket.resource return { @@ -35,112 +112,827 @@ def _loader_unload_event_context(self: "Access2") -> dict: "resources": [] if plate is None else [resource_reference(plate)], "source": resource_reference(bucket), "destination": resource_reference(self), + "parameters": parameters, } 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, + ): """ Args: device_id: The libftdi id for the loader. Find using `python3 -m pylibftdi.examples.list_devices` + timeout: Communication and operation timeout in seconds. """ super().__init__() self.io = FTDI(human_readable_device_name="Agilent Access2 Loader", device_id=device_id) self.timeout = timeout + self._command_lock = asyncio.Lock() + self._operation_lock = asyncio.Lock() + self._state = Access2MachineState() + + @property + def state(self) -> Access2MachineState: + """Return the current Access2 semantic-state snapshot.""" + return self._state + + def _mark_recovery_required(self, *, position_uncertain: bool) -> None: + """Make recovery sticky and invalidate only an uncertain arm teachpoint.""" + self._state = dataclasses.replace( + self._state, + recovery_required=True, + last_teachpoint=None if position_uncertain else self._state.last_teachpoint, + ) + + def _record_disconnected(self) -> None: + """Record transport closure without copying controller status facts.""" + self._state = dataclasses.replace( + self._state, + connection=ConnectionState.DISCONNECTED, + last_teachpoint=None, + ) + + @asynccontextmanager + async def _operation_scope( + self, + operation: Access2Operation, + ) -> AsyncIterator[TransitionToken]: + """Own the operation lock and apply actuation-aware state changes.""" + async with self._operation_lock: + previous_operation = self._state.operation + transition = TransitionToken() + self._state = dataclasses.replace(self._state, operation=operation) + try: + yield transition + except BaseException as error: + if is_ftdi_transport_error(error): + self._record_disconnected() + if transition.actuated: + self._mark_recovery_required(position_uncertain=transition.position_uncertain) + else: + self._state = dataclasses.replace(self._state, operation=previous_operation) + raise + else: + self._state = dataclasses.replace(self._state, operation=Access2Activity.IDLE) + + def _set_transfer_phase(self, phase: TransferPhase) -> None: + """Advance the active transfer while retaining its direction and bucket.""" + transfer = self._state.operation + if not isinstance(transfer, TransferProgress): + raise RuntimeError("No Access2 transfer is active") + self._state = dataclasses.replace( + self._state, + operation=dataclasses.replace(transfer, phase=phase), + ) - 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() - - async def setup(self): + 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, raise_on_error: bool = True + ) -> protocol.Access2Reply: + """Send one transport-independent command through the FTDI envelope.""" + frame = protocol.build_ftdi_frame(command) + logger.debug("[loader] Sending %s", frame.hex()) + try: + 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() + except BaseException as error: + if is_ftdi_transport_error(error): + self._record_disconnected() + raise + logger.debug("[loader] Received %s", response_frame.hex()) + response = protocol.parse_ftdi_reply(response_frame, request_id=command[0]) + if raise_on_error and response.result != 0: + raise protocol.Access2ProtocolError( + f"Access2 command 0x{command[0]:02x} returned result 0x{response.result:02x}; " + f"response data: {response.data.hex()}" + ) + return response + + async def setup(self) -> None: + """Connect, initialize, home, open, and park the Access2 loader.""" logger.debug("[loader] setup") + async with self._operation_scope(Access2Activity.INITIALIZING) as transition: + if self._state.recovery_required: + raise RuntimeError("Access2 requires recovery during setup") + self._state = dataclasses.replace(self._state, connection=ConnectionState.CONNECTING) + try: + await self.io.setup() + except BaseException: + self._record_disconnected() + raise + self._state = dataclasses.replace(self._state, connection=ConnectionState.CONNECTED) + await self.io.set_baudrate(115384) - await self.io.setup() - await self.io.set_baudrate(115384) + self._raise_on_fault(await self.request_status(), operation="setup precondition") - 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 def stop(self): + await self.send_command(protocol.build_ping()) + transition.mark_actuated() + await self.send_command(protocol.build_initialize()) + self._state = dataclasses.replace( + self._state, + operation=Access2Activity.HOMING, + last_teachpoint=None, + ) + transition.mark_actuated(position_uncertain=True) + await self._home() + transition.confirm_position() + self._state = dataclasses.replace(self._state, operation=Access2Activity.MOVING) + transition.mark_actuated() + await self._move_axis_to_position( + protocol.AXIS_GRIPPER, + _DEFAULT_GRIPPER_OPEN_POSITION, + profile=protocol.PROFILE_DYNAMIC_EMPTY, + speed=protocol.SPEED_FAST, + ) + self._state = dataclasses.replace(self._state, last_teachpoint=None) + transition.mark_actuated(position_uncertain=True) + await self._move_to_teachpoint( + protocol.TEACHPOINT_PARK, + 0, + 15, + profile=protocol.PROFILE_DYNAMIC_EMPTY, + speed=protocol.SPEED_FAST, + ) + self._state = dataclasses.replace( + self._state, + last_teachpoint=protocol.TEACHPOINT_PARK, + ) + transition.confirm_position() + await self._require_ready(operation="setup postcondition") + + async def stop(self) -> None: + """Close the Access2 transport and invalidate session-scoped state.""" logger.debug("[loader] stop") - await self.io.stop() + async with self._operation_lock: + self._state = dataclasses.replace(self._state, connection=ConnectionState.DISCONNECTING) + try: + await self.io.stop() + finally: + self._record_disconnected() - 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 _require_ready( + self, + operation: str = "readiness check", + ) -> protocol.Access2Status: + """Return fresh ready status after semantic and controller checks.""" + if self._state.connection is not ConnectionState.CONNECTED: + raise RuntimeError(f"Access2 is not connected during {operation}") + if self._state.recovery_required: + raise RuntimeError(f"Access2 requires recovery during {operation}") + 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 home(self) -> None: + """Home all Access2 axes and wait for the controller to confirm completion.""" + async with self._operation_scope(Access2Activity.HOMING) as transition: + if self._state.connection is not ConnectionState.CONNECTED: + raise RuntimeError("Access2 is not connected during home") + if self._state.recovery_required: + raise RuntimeError("Access2 requires recovery during home") + self._state = dataclasses.replace(self._state, last_teachpoint=None) + transition.mark_actuated(position_uncertain=True) + status = await self._home() + if not status.initialized: + raise RuntimeError( + f"Access2 lost initialized state during home: status 0x{status.access2_status:02x}" + ) + transition.confirm_position() + + 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 + ) + + @staticmethod + def _gripper_is_closed( + status: protocol.Access2Status, + *, + gripper_closed_position: float, + gripper_close_threshold: float, + ) -> bool: + at_closed_position = ( + status.gripper_position is not None + and abs(status.gripper_position - gripper_closed_position) <= _AXIS_POSITION_TOLERANCE + ) + 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 status.gripper_position >= gripper_close_threshold + and (at_closed_position or status.optical_plate_sensor) + ) + + 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) - async def park(self): + 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 _close_gripper( + self, + *, + gripper_closed_position: float = _DEFAULT_GRIPPER_CLOSED_POSITION, + gripper_close_threshold: float = _DEFAULT_GRIPPER_CLOSE_THRESHOLD, + speed: int = protocol.SPEED_SLOW, + ) -> protocol.Access2Status: + """Close until the configured threshold, allowing normal plate contact. + + The FTDI controller can return a nonzero result when a plate stops the + gripper before its unobstructed target. The result is retained for + diagnostics. Full controller status must confirm completion and either the + unobstructed close position or plate-sensor-backed contact past the close + threshold. + """ + response = await self.send_command( + protocol.build_move_axis_to_position( + protocol.AXIS_GRIPPER, + gripper_closed_position, + protocol.PROFILE_DYNAMIC_EMPTY, + speed, + ), + raise_on_error=False, + ) + status = await self._wait_until_motion_complete( + (protocol.AXIS_GRIPPER,), + operation="close gripper", + ) + if not self._gripper_is_closed( + status, + gripper_closed_position=gripper_closed_position, + gripper_close_threshold=gripper_close_threshold, + ) or (response.result != 0 and not status.optical_plate_sensor): + assert status.gripper_position is not None + assert status.gripper_status is not None + error = ( + "Access2 did not close the gripper: " + f"position {status.gripper_position:.3f}, threshold {gripper_close_threshold:.3f}, " + f"axis status 0x{status.gripper_status:02x}, " + f"optical plate sensor {status.optical_plate_sensor}, " + f"command result 0x{response.result:02x}" + ) + if response.result != 0: + raise protocol.Access2ProtocolError(error) + raise RuntimeError(error) + if response.result != 0: + logger.debug( + "[loader] Gripper-close command returned 0x%02x; full status confirmed closed", + response.result, + ) + return status + + async def park( + self, *, plate_height: float = 15, z_offset: float = 8, speed: Access2Speed = "slow" + ) -> None: + """Move the Access2 arm to its park teachpoint. + + Args: + plate_height: Plate height sent to the controller, in millimeters. + z_offset: Z offset at the park teachpoint, in millimeters. + speed: Controller speed preset: ``slow``, ``medium``, or ``fast``. + """ + speed_code = _speed_code(speed) + if not math.isfinite(plate_height) or plate_height <= 0: + raise ValueError("Plate height must be finite and positive") + if not math.isfinite(z_offset): + raise ValueError("Park Z offset must be finite") logger.debug("[loader] park") - await self.send_command(bytes.fromhex("1105000e00440b0000000000410000704103007539")) + async with self._operation_scope(Access2Activity.MOVING) as transition: + await self._require_ready(operation="park precondition") + self._state = dataclasses.replace(self._state, last_teachpoint=None) + transition.mark_actuated(position_uncertain=True) + await self._move_to_teachpoint( + protocol.TEACHPOINT_PARK, + z_offset, + plate_height, + profile=protocol.PROFILE_DYNAMIC_FULL, + speed=speed_code, + ) + self._state = dataclasses.replace( + self._state, + last_teachpoint=protocol.TEACHPOINT_PARK, + ) + transition.confirm_position() + await self._require_ready(operation="park postcondition") - async def close(self): - logger.debug("[loader] close") - await self.send_command(bytes.fromhex("1105000a00420700010000803f02008c64")) + async def close_gripper( + self, + *, + gripper_closed_position: float = _DEFAULT_GRIPPER_CLOSED_POSITION, + gripper_close_threshold: float = _DEFAULT_GRIPPER_CLOSE_THRESHOLD, + speed: Access2Speed = "slow", + ) -> None: + """Close the gripper with per-call position, contact threshold, and speed. - async def open(self): - logger.debug("[loader] open") - await self.send_command(bytes.fromhex("1105000a0042070001000080bf0200b73e")) + Args: + gripper_closed_position: Absolute gripper-axis target in millimeters. + gripper_close_threshold: Minimum axis position accepted as contact, in millimeters. + speed: Controller speed preset: ``slow``, ``medium``, or ``fast``. + """ + speed_code = _speed_code(speed) + if not ( + math.isfinite(gripper_closed_position) + and math.isfinite(gripper_close_threshold) + and 0 < gripper_close_threshold <= gripper_closed_position + ): + raise ValueError("Gripper positions must be finite with 0 < threshold <= closed position") + logger.debug("[loader] close gripper") + async with self._operation_scope(Access2Activity.MOVING) as transition: + status = await self._require_ready(operation="gripper-close precondition") + if self._gripper_is_closed( + status, + gripper_closed_position=gripper_closed_position, + gripper_close_threshold=gripper_close_threshold, + ): + return + transition.mark_actuated() + await self._close_gripper( + gripper_closed_position=gripper_closed_position, + gripper_close_threshold=gripper_close_threshold, + speed=speed_code, + ) + await self._require_ready(operation="gripper-close postcondition") - async def load(self): - """Only tested for 1cm plate, 3mm pickup height.""" - logger.debug("[loader] load") + async def open_gripper( + self, + *, + gripper_open_position: float = _DEFAULT_GRIPPER_OPEN_POSITION, + speed: Access2Speed = "slow", + ) -> None: + """Open the gripper with per-call position and speed. - await self.send_command(bytes.fromhex("1105000a004607000100000000020235bf")) - await self.send_command(bytes.fromhex("1105000e00440b000100004040000020410200a5cb")) + Args: + gripper_open_position: Absolute gripper-axis target in millimeters. + speed: Controller speed preset: ``slow``, ``medium``, or ``fast``. + """ + speed_code = _speed_code(speed) + if not math.isfinite(gripper_open_position): + raise ValueError("Gripper open position must be finite") + logger.debug("[loader] open gripper") + async with self._operation_scope(Access2Activity.MOVING) as transition: + status = await self._require_ready(operation="gripper-open precondition") + if self._gripper_is_at_position(status, gripper_open_position): + return + transition.mark_actuated() + await self._move_axis_to_position( + protocol.AXIS_GRIPPER, + gripper_open_position, + profile=protocol.PROFILE_DYNAMIC_EMPTY, + speed=speed_code, + ) + await self._require_ready(operation="gripper-open postcondition") - r = await self.send_command(bytes.fromhex("1105000300500000b3dc")) - if r == bytes.fromhex("1105000800510500000300000079f1"): - raise RuntimeError("no plate found on stage") + async def _transfer( + self, + direction: TransferDirection, + bucket_teachpoint: int, + *, + plate_height: float, + source_z_offset: float, + destination_z_offset: float, + park_z_offset: float, + gripper_open_position: float, + gripper_closed_position: float, + gripper_close_threshold: float, + source_speed: Access2Speed, + destination_speed: Access2Speed, + park_speed: Access2Speed, + gripper_open_speed: Access2Speed, + gripper_close_speed: Access2Speed, + gripper_release_speed: Access2Speed, + ) -> None: + """Run the load/unload hardware sequence through one stateful path.""" + if not all( + math.isfinite(value) + for value in ( + plate_height, + source_z_offset, + destination_z_offset, + park_z_offset, + gripper_open_position, + gripper_closed_position, + gripper_close_threshold, + ) + ): + raise ValueError("Transfer parameters must be finite") + if plate_height <= 0: + raise ValueError("Plate height must be positive") + if not gripper_open_position < gripper_close_threshold <= gripper_closed_position: + raise ValueError( + "Gripper positions must satisfy open position < close threshold <= closed position" + ) + source_speed_code = _speed_code(source_speed) + destination_speed_code = _speed_code(destination_speed) + park_speed_code = _speed_code(park_speed) + gripper_open_speed_code = _speed_code(gripper_open_speed) + gripper_close_speed_code = _speed_code(gripper_close_speed) + gripper_release_speed_code = _speed_code(gripper_release_speed) + route = _transfer_route(direction, bucket_teachpoint) + progress = TransferProgress( + direction=direction, + bucket_teachpoint=bucket_teachpoint, + ) + async with self._operation_scope(progress) as transition: + await self._require_ready(operation="transfer precondition") + if self._state.last_teachpoint != protocol.TEACHPOINT_PARK: + raise RuntimeError("Access2 must be confirmed parked before a transfer") - 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")) + transition.mark_actuated() + await self._move_axis_to_position( + protocol.AXIS_GRIPPER, + gripper_open_position, + profile=protocol.PROFILE_DYNAMIC_EMPTY, + speed=gripper_open_speed_code, + ) - async def unload(self): - """Only tested for 1cm plate, 3mm pickup height.""" - logger.debug("[loader] unload") + self._state = dataclasses.replace(self._state, last_teachpoint=None) + transition.mark_actuated(position_uncertain=True) + await self._move_to_teachpoint( + route.source_teachpoint, source_z_offset, plate_height, speed=source_speed_code + ) + self._state = dataclasses.replace( + self._state, + last_teachpoint=route.source_teachpoint, + ) + transition.confirm_position() + self._set_transfer_phase(TransferPhase.AT_SOURCE) - await self.send_command(bytes.fromhex("1105000a004607000100000000020235bf")) - await self.send_command(bytes.fromhex("1105000e00440b000200004040000020410200dd31")) + sensor_values = await self.request_sensor_values() + if not sensor_values & protocol.STATUS_OPTICAL_PLATE_SENSOR: + raise RuntimeError(f"no plate found on {route.source_name}") - r = await self.send_command(bytes.fromhex("1105000300500000b3dc")) - if r == bytes.fromhex("1105000800510500000300000079f1"): - raise RuntimeError("no plate found in centrifuge") + self._set_transfer_phase(TransferPhase.GRIPPING) + transition.mark_actuated() + await self._close_gripper( + gripper_closed_position=gripper_closed_position, + gripper_close_threshold=gripper_close_threshold, + speed=gripper_close_speed_code, + ) + self._set_transfer_phase(TransferPhase.HOLDING) - 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")) + self._set_transfer_phase(TransferPhase.MOVING_TO_DESTINATION) + self._state = dataclasses.replace(self._state, last_teachpoint=None) + transition.mark_actuated(position_uncertain=True) + await self._move_to_teachpoint( + route.destination_teachpoint, + destination_z_offset, + plate_height, + profile=protocol.PROFILE_DYNAMIC_FULL, + speed=destination_speed_code, + ) + self._state = dataclasses.replace( + self._state, + last_teachpoint=route.destination_teachpoint, + ) + transition.confirm_position() + self._set_transfer_phase(TransferPhase.AT_DESTINATION) + + self._set_transfer_phase(TransferPhase.RELEASING) + transition.mark_actuated() + await self._move_axis_to_position( + protocol.AXIS_GRIPPER, + gripper_open_position, + profile=protocol.PROFILE_DYNAMIC_EMPTY, + speed=gripper_release_speed_code, + ) + + self._set_transfer_phase(TransferPhase.RETURNING_TO_PARK) + self._state = dataclasses.replace(self._state, last_teachpoint=None) + transition.mark_actuated(position_uncertain=True) + await self._move_to_teachpoint( + protocol.TEACHPOINT_PARK, + park_z_offset, + plate_height, + speed=park_speed_code, + ) + self._state = dataclasses.replace( + self._state, + last_teachpoint=protocol.TEACHPOINT_PARK, + ) + transition.confirm_position() + await self._require_ready(operation="transfer postcondition") + + async def load( + self, + bucket_teachpoint: int = protocol.TEACHPOINT_BUCKET_1, + *, + plate_height: float = 10, + source_z_offset: float = 3, + destination_z_offset: float = 3, + park_z_offset: float = 3, + gripper_open_position: float = _DEFAULT_GRIPPER_OPEN_POSITION, + gripper_closed_position: float = _DEFAULT_GRIPPER_CLOSED_POSITION, + gripper_close_threshold: float = _DEFAULT_GRIPPER_CLOSE_THRESHOLD, + source_speed: Access2Speed = "slow", + destination_speed: Access2Speed = "slow", + park_speed: Access2Speed = "slow", + gripper_open_speed: Access2Speed = "fast", + gripper_close_speed: Access2Speed = "slow", + gripper_release_speed: Access2Speed = "slow", + ) -> None: + """Move a plate from the stage into the selected bucket. + + Args: + bucket_teachpoint: Controller teachpoint for the target bucket. + plate_height: Plate height sent to the controller, in millimeters. + source_z_offset: Z offset at the pickup teachpoint, in millimeters. + destination_z_offset: Z offset at the placement teachpoint, in millimeters. + park_z_offset: Z offset when returning to park, in millimeters. + gripper_open_position: Absolute gripper-axis position when opening, in millimeters. + gripper_closed_position: Absolute gripper-axis target when closing, in millimeters. + gripper_close_threshold: Minimum axis position accepted as plate contact, in millimeters. + source_speed: Approach to the pickup teachpoint; ``slow``, ``medium``, or ``fast``. + destination_speed: Move carrying the plate to placement; ``slow``, ``medium``, or ``fast``. + park_speed: Return to park after release; ``slow``, ``medium``, or ``fast``. + gripper_open_speed: Initial opening before pickup; ``slow``, ``medium``, or ``fast``. + gripper_close_speed: Closing on the plate; ``slow``, ``medium``, or ``fast``. + gripper_release_speed: Opening to release the plate; ``slow``, ``medium``, or ``fast``. + """ + logger.debug("[loader] load") + await self._transfer( + TransferDirection.INTO_CENTRIFUGE, + bucket_teachpoint, + plate_height=plate_height, + source_z_offset=source_z_offset, + destination_z_offset=destination_z_offset, + park_z_offset=park_z_offset, + gripper_open_position=gripper_open_position, + gripper_closed_position=gripper_closed_position, + gripper_close_threshold=gripper_close_threshold, + source_speed=source_speed, + destination_speed=destination_speed, + park_speed=park_speed, + gripper_open_speed=gripper_open_speed, + gripper_close_speed=gripper_close_speed, + gripper_release_speed=gripper_release_speed, + ) + + async def unload( + self, + bucket_teachpoint: int = protocol.TEACHPOINT_BUCKET_1, + *, + plate_height: float = 10, + source_z_offset: float = 3, + destination_z_offset: float = 3, + park_z_offset: float = 0, + gripper_open_position: float = _DEFAULT_GRIPPER_OPEN_POSITION, + gripper_closed_position: float = _DEFAULT_GRIPPER_CLOSED_POSITION, + gripper_close_threshold: float = _DEFAULT_GRIPPER_CLOSE_THRESHOLD, + source_speed: Access2Speed = "slow", + destination_speed: Access2Speed = "slow", + park_speed: Access2Speed = "slow", + gripper_open_speed: Access2Speed = "fast", + gripper_close_speed: Access2Speed = "slow", + gripper_release_speed: Access2Speed = "slow", + ) -> None: + """Move a plate from the selected bucket onto the stage. + + Args: + bucket_teachpoint: Controller teachpoint for the target bucket. + plate_height: Plate height sent to the controller, in millimeters. + source_z_offset: Z offset at the pickup teachpoint, in millimeters. + destination_z_offset: Z offset at the placement teachpoint, in millimeters. + park_z_offset: Z offset when returning to park, in millimeters. + gripper_open_position: Absolute gripper-axis position when opening, in millimeters. + gripper_closed_position: Absolute gripper-axis target when closing, in millimeters. + gripper_close_threshold: Minimum axis position accepted as plate contact, in millimeters. + source_speed: Approach to the pickup teachpoint; ``slow``, ``medium``, or ``fast``. + destination_speed: Move carrying the plate to placement; ``slow``, ``medium``, or ``fast``. + park_speed: Return to park after release; ``slow``, ``medium``, or ``fast``. + gripper_open_speed: Initial opening before pickup; ``slow``, ``medium``, or ``fast``. + gripper_close_speed: Closing on the plate; ``slow``, ``medium``, or ``fast``. + gripper_release_speed: Opening to release the plate; ``slow``, ``medium``, or ``fast``. + """ + logger.debug("[loader] unload") + await self._transfer( + TransferDirection.OUT_OF_CENTRIFUGE, + bucket_teachpoint, + plate_height=plate_height, + source_z_offset=source_z_offset, + destination_z_offset=destination_z_offset, + park_z_offset=park_z_offset, + gripper_open_position=gripper_open_position, + gripper_closed_position=gripper_closed_position, + gripper_close_threshold=gripper_close_threshold, + source_speed=source_speed, + destination_speed=destination_speed, + park_speed=park_speed, + gripper_open_speed=gripper_open_speed, + gripper_close_speed=gripper_close_speed, + gripper_release_speed=gripper_release_speed, + ) class Access2(ResourceHolder): @@ -155,7 +947,19 @@ def __init__( size_y: float = 0.0, size_z: float = 0.0, ): - driver = Access2Driver(device_id=device_id) + """Create an Access2 loader paired with a VSpin centrifuge. + + 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. + """ + driver = Access2Driver( + device_id=device_id, + ) ResourceHolder.__init__( self, name=name, @@ -169,36 +973,210 @@ def __init__( self.driver: Access2Driver = driver self._vspin = vspin + def _teachpoint_for_bucket(self, bucket: ResourceHolder) -> int: + """Map a VSpin bucket resource to its Access2 protocol teachpoint.""" + if bucket is self._vspin.bucket1: + return protocol.TEACHPOINT_BUCKET_1 + if bucket is self._vspin.bucket2: + return protocol.TEACHPOINT_BUCKET_2 + raise NotAtBucketError("Unknown VSpin bucket") + + async def _run_driver_transfer( + self, + direction: TransferDirection, + bucket: ResourceHolder, + *, + plate_height: float, + source_z_offset: float, + destination_z_offset: float, + park_z_offset: float, + gripper_open_position: float, + gripper_closed_position: float, + gripper_close_threshold: float, + source_speed: Access2Speed, + destination_speed: Access2Speed, + park_speed: Access2Speed, + gripper_open_speed: Access2Speed, + gripper_close_speed: Access2Speed, + gripper_release_speed: Access2Speed, + ) -> None: + """Reserve VSpin and ask Access2Driver to perform one physical transfer.""" + if self.driver.state.recovery_required: + raise RuntimeError("Access2 requires recovery") + bucket_teachpoint = self._teachpoint_for_bucket(bucket) + async with self._vspin.reserve_transfer(bucket) as vspin_transition: + try: + if direction is TransferDirection.INTO_CENTRIFUGE: + await self.driver.load( + bucket_teachpoint, + plate_height=plate_height, + source_z_offset=source_z_offset, + destination_z_offset=destination_z_offset, + park_z_offset=park_z_offset, + gripper_open_position=gripper_open_position, + gripper_closed_position=gripper_closed_position, + gripper_close_threshold=gripper_close_threshold, + source_speed=source_speed, + destination_speed=destination_speed, + park_speed=park_speed, + gripper_open_speed=gripper_open_speed, + gripper_close_speed=gripper_close_speed, + gripper_release_speed=gripper_release_speed, + ) + else: + await self.driver.unload( + bucket_teachpoint, + plate_height=plate_height, + source_z_offset=source_z_offset, + destination_z_offset=destination_z_offset, + park_z_offset=park_z_offset, + gripper_open_position=gripper_open_position, + gripper_closed_position=gripper_closed_position, + gripper_close_threshold=gripper_close_threshold, + source_speed=source_speed, + destination_speed=destination_speed, + park_speed=park_speed, + gripper_open_speed=gripper_open_speed, + gripper_close_speed=gripper_close_speed, + gripper_release_speed=gripper_release_speed, + ) + except BaseException: + if self.driver.state.recovery_required: + vspin_transition.mark_actuated() + raise + @evented_operation("centrifuge_loader.load", _loader_load_event_context) - async def load(self) -> None: - if not self._vspin.door_open: - raise CentrifugeDoorError("Centrifuge door must be open to load a plate.") - if self._vspin.at_bucket is None: + async def load( + self, + *, + plate_height: float = 10, + source_z_offset: float = 3, + destination_z_offset: float = 3, + park_z_offset: float = 3, + gripper_open_position: float = _DEFAULT_GRIPPER_OPEN_POSITION, + gripper_closed_position: float = _DEFAULT_GRIPPER_CLOSED_POSITION, + gripper_close_threshold: float = _DEFAULT_GRIPPER_CLOSE_THRESHOLD, + source_speed: Access2Speed = "slow", + destination_speed: Access2Speed = "slow", + park_speed: Access2Speed = "slow", + gripper_open_speed: Access2Speed = "fast", + gripper_close_speed: Access2Speed = "slow", + gripper_release_speed: Access2Speed = "slow", + ) -> None: + """Move the loader's plate into the currently presented VSpin bucket. + + Settings apply only to this transfer; plate dimensions are not inferred from the resource. + Gripper positions describe axis travel, not plate width or jaw separation. + + Args: + plate_height: Plate height sent to the controller, in millimeters. + source_z_offset: Z offset at the pickup teachpoint, in millimeters. + destination_z_offset: Z offset at the placement teachpoint, in millimeters. + park_z_offset: Z offset when returning to park, in millimeters. + gripper_open_position: Absolute gripper-axis position when opening, in millimeters. + gripper_closed_position: Absolute gripper-axis target when closing, in millimeters. + gripper_close_threshold: Minimum axis position accepted as plate contact, in millimeters. + source_speed: Approach to the pickup teachpoint; ``slow``, ``medium``, or ``fast``. + destination_speed: Move carrying the plate to placement; ``slow``, ``medium``, or ``fast``. + park_speed: Return to park after release; ``slow``, ``medium``, or ``fast``. + gripper_open_speed: Initial opening before pickup; ``slow``, ``medium``, or ``fast``. + gripper_close_speed: Closing on the plate; ``slow``, ``medium``, or ``fast``. + gripper_release_speed: Opening to release the plate; ``slow``, ``medium``, or ``fast``. + """ + bucket = self._vspin.at_bucket + if bucket is None: raise NotAtBucketError( "Centrifuge must be at a bucket to load a plate. " "Use vspin.go_to_bucket1() or vspin.go_to_bucket2()." ) if self.resource is None: raise LoaderNoPlateError("Loader must have a plate to load.") - if self._vspin.at_bucket.resource is not None: + if bucket.resource is not None: raise BucketHasPlateError("Bucket must be empty to load a plate.") - await self.driver.load() + await self._run_driver_transfer( + TransferDirection.INTO_CENTRIFUGE, + bucket, + plate_height=plate_height, + source_z_offset=source_z_offset, + destination_z_offset=destination_z_offset, + park_z_offset=park_z_offset, + gripper_open_position=gripper_open_position, + gripper_closed_position=gripper_closed_position, + gripper_close_threshold=gripper_close_threshold, + source_speed=source_speed, + destination_speed=destination_speed, + park_speed=park_speed, + gripper_open_speed=gripper_open_speed, + gripper_close_speed=gripper_close_speed, + gripper_release_speed=gripper_release_speed, + ) - self._vspin.at_bucket.assign_child_resource(self.resource, location=Coordinate.zero()) + bucket.assign_child_resource(self.resource, location=Coordinate.zero()) @evented_operation("centrifuge_loader.unload", _loader_unload_event_context) - async def unload(self) -> None: - if not self._vspin.door_open: - raise CentrifugeDoorError("Centrifuge door must be open to unload a plate.") - if self._vspin.at_bucket is None: + async def unload( + self, + *, + plate_height: float = 10, + source_z_offset: float = 3, + destination_z_offset: float = 3, + park_z_offset: float = 0, + gripper_open_position: float = _DEFAULT_GRIPPER_OPEN_POSITION, + gripper_closed_position: float = _DEFAULT_GRIPPER_CLOSED_POSITION, + gripper_close_threshold: float = _DEFAULT_GRIPPER_CLOSE_THRESHOLD, + source_speed: Access2Speed = "slow", + destination_speed: Access2Speed = "slow", + park_speed: Access2Speed = "slow", + gripper_open_speed: Access2Speed = "fast", + gripper_close_speed: Access2Speed = "slow", + gripper_release_speed: Access2Speed = "slow", + ) -> None: + """Move the presented VSpin bucket's plate onto the loader. + + Settings apply only to this transfer; plate dimensions are not inferred from the resource. + Gripper positions describe axis travel, not plate width or jaw separation. + + Args: + plate_height: Plate height sent to the controller, in millimeters. + source_z_offset: Z offset at the pickup teachpoint, in millimeters. + destination_z_offset: Z offset at the placement teachpoint, in millimeters. + park_z_offset: Z offset when returning to park, in millimeters. + gripper_open_position: Absolute gripper-axis position when opening, in millimeters. + gripper_closed_position: Absolute gripper-axis target when closing, in millimeters. + gripper_close_threshold: Minimum axis position accepted as plate contact, in millimeters. + source_speed: Approach to the pickup teachpoint; ``slow``, ``medium``, or ``fast``. + destination_speed: Move carrying the plate to placement; ``slow``, ``medium``, or ``fast``. + park_speed: Return to park after release; ``slow``, ``medium``, or ``fast``. + gripper_open_speed: Initial opening before pickup; ``slow``, ``medium``, or ``fast``. + gripper_close_speed: Closing on the plate; ``slow``, ``medium``, or ``fast``. + gripper_release_speed: Opening to release the plate; ``slow``, ``medium``, or ``fast``. + """ + bucket = self._vspin.at_bucket + if bucket is None: raise NotAtBucketError( "Centrifuge must be at a bucket to unload a plate. " "Use vspin.go_to_bucket1() or vspin.go_to_bucket2()." ) - if self._vspin.at_bucket.resource is None: + if bucket.resource is None: raise BucketNoPlateError("Bucket must have a plate to unload.") - await self.driver.unload() + await self._run_driver_transfer( + TransferDirection.OUT_OF_CENTRIFUGE, + bucket, + plate_height=plate_height, + source_z_offset=source_z_offset, + destination_z_offset=destination_z_offset, + park_z_offset=park_z_offset, + gripper_open_position=gripper_open_position, + gripper_closed_position=gripper_closed_position, + gripper_close_threshold=gripper_close_threshold, + source_speed=source_speed, + destination_speed=destination_speed, + park_speed=park_speed, + gripper_open_speed=gripper_open_speed, + gripper_close_speed=gripper_close_speed, + gripper_release_speed=gripper_release_speed, + ) - self.assign_child_resource(self._vspin.at_bucket.resource) + self.assign_child_resource(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..0ffb4cc2648 --- /dev/null +++ b/pylabrobot/agilent/vspin/access2_protocol_tests.py @@ -0,0 +1,234 @@ +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(response.result, 0) + 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_preserves_nonzero_command_result(self): + inner = Writer().u8(0x51).u16(1).u8(7).finish() + + response = protocol.parse_reply(inner, protocol.GET_SENSOR_VALUES) + + self.assertEqual(response.result, 7) + self.assertEqual(response.data, b"") + + 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..418bc2dd88b --- /dev/null +++ b/pylabrobot/agilent/vspin/access2_tests.py @@ -0,0 +1,1119 @@ +from __future__ import annotations + +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._state import ( + Access2Activity, + ConnectionState, + TransferDirection, + TransferPhase, + TransferProgress, +) +from pylabrobot.agilent.vspin.access2 import Access2Driver +from pylabrobot.io.binary import Writer + +_READY_FLAGS = protocol.STATUS_INITIALIZED | protocol.STATUS_HOMED + + +def _mark_driver_ready(driver: Access2Driver) -> None: + """Put a mock-backed driver at the verified lifecycle boundary under test.""" + driver._state = dataclasses.replace( + driver.state, + connection=ConnectionState.CONNECTED, + last_teachpoint=protocol.TEACHPOINT_PARK, + ) + + +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() + + async def test_connected_transport_does_not_imply_controller_readiness(self): + self.driver._state = dataclasses.replace( + self.driver.state, + connection=ConnectionState.CONNECTED, + ) + self.driver.request_status = AsyncMock( # type: ignore[method-assign] + return_value=_status(flags=0) + ) + + with self.assertRaisesRegex(RuntimeError, "not initialized and homed"): + await self.driver._require_ready() + + async def test_ftdi_error_invalidates_connection_state(self): + _mark_driver_ready(self.driver) + self.driver.io.write = AsyncMock(side_effect=RuntimeError("transport lost")) # type: ignore[method-assign] + + with ( + patch("pylabrobot.agilent.vspin.access2.is_ftdi_transport_error", return_value=True), + self.assertRaisesRegex(RuntimeError, "transport lost"), + ): + await self.driver.open_gripper() + + self.assertEqual(self.driver.state.connection, ConnectionState.DISCONNECTED) + self.assertEqual(self.driver.state.operation, Access2Activity.IDLE) + self.assertIsNone(self.driver.state.last_teachpoint) + + +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] + _mark_driver_ready(driver) + 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_DYNAMIC_EMPTY, + 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) + self.assertEqual(driver.state.connection, ConnectionState.CONNECTED) + self.assertEqual(driver.state.operation, Access2Activity.IDLE) + self.assertEqual(driver.state.last_teachpoint, protocol.TEACHPOINT_PARK) + + 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_DYNAMIC_EMPTY, + 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(0x03 | protocol.STATUS_OPTICAL_PLATE_SENSOR).finish(), + ), + _ScriptStep( + protocol.build_move_axis_to_position( + protocol.AXIS_GRIPPER, + 5.68, + protocol.PROFILE_DYNAMIC_EMPTY, + ) + ), + _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_DYNAMIC_EMPTY, + ) + ), + _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_DYNAMIC_EMPTY, + 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(0x03 | protocol.STATUS_OPTICAL_PLATE_SENSOR).finish(), + ), + _ScriptStep( + protocol.build_move_axis_to_position( + protocol.AXIS_GRIPPER, + 5.68, + protocol.PROFILE_DYNAMIC_EMPTY, + ), + result=0x51, + ), + _ScriptStep( + protocol.build_get_status(), + _full_status_data( + flags=_READY_FLAGS | protocol.STATUS_OPTICAL_PLATE_SENSOR, + gripper_position=1.94, + ), + ), + _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_DYNAMIC_EMPTY, + ) + ), + _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_DYNAMIC_EMPTY, + 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_nonzero_result_stops_an_exact_axis_move(self): + command = protocol.build_move_axis_to_position(protocol.AXIS_Y, 100) + driver, io = self._make_driver([_ScriptStep(command, result=7)]) + + with self.assertRaisesRegex(protocol.Access2ProtocolError, "result 0x07"): + await driver._move_axis_to_position(protocol.AXIS_Y, 100) + + io.assert_complete(self) + + async def test_gripper_close_requires_the_configured_threshold(self): + command = protocol.build_move_axis_to_position(protocol.AXIS_GRIPPER, 5.68) + steps = [ + _ScriptStep(command, result=0x51), + _ScriptStep( + protocol.build_get_status(), + _full_status_data( + flags=_READY_FLAGS | protocol.STATUS_OPTICAL_PLATE_SENSOR, + gripper_position=1.4, + ), + ), + ] + driver, io = self._make_driver(steps) + + with self.assertRaisesRegex( + protocol.Access2ProtocolError, + "position 1.400, threshold 1.500, axis status 0x01, " + "optical plate sensor True, command result 0x51", + ): + await driver._close_gripper() + + io.assert_complete(self) + + async def test_gripper_contact_uses_each_calls_target_and_threshold(self): + for threshold, accepted in ((1.8, True), (2.0, False)): + with self.subTest(threshold=threshold): + driver, io = self._make_driver( + [ + _ScriptStep( + protocol.build_move_axis_to_position( + protocol.AXIS_GRIPPER, 4.75, speed=protocol.SPEED_FAST + ), + result=0x51, + ), + _ScriptStep( + protocol.build_get_status(), + _full_status_data( + flags=_READY_FLAGS | protocol.STATUS_OPTICAL_PLATE_SENSOR, + gripper_position=1.94, + ), + ), + ] + ) + if accepted: + await driver._close_gripper( + gripper_closed_position=4.75, + gripper_close_threshold=threshold, + speed=protocol.SPEED_FAST, + ) + else: + with self.assertRaisesRegex(protocol.Access2ProtocolError, "threshold 2.000"): + await driver._close_gripper( + gripper_closed_position=4.75, + gripper_close_threshold=threshold, + speed=protocol.SPEED_FAST, + ) + io.assert_complete(self) + + async def test_gripper_close_rejects_a_non_contact_error(self): + command = protocol.build_move_axis_to_position(protocol.AXIS_GRIPPER, 5.68) + steps = [ + _ScriptStep(command, result=0x07), + _ScriptStep( + protocol.build_get_status(), + _full_status_data(gripper_position=5.68), + ), + ] + driver, io = self._make_driver(steps) + + with self.assertRaisesRegex(protocol.Access2ProtocolError, "command result 0x07"): + await driver._close_gripper() + + 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") + _mark_driver_ready(self.driver) + self.driver._move_axis_to_position = AsyncMock() # type: ignore[method-assign] + self.driver._move_to_teachpoint = AsyncMock() # type: ignore[method-assign] + self.driver._close_gripper = AsyncMock() # type: ignore[method-assign] + + async def test_park_parameters_are_per_call(self): + self.driver.request_status = AsyncMock( # type: ignore[method-assign] + return_value=_status(flags=_READY_FLAGS) + ) + await self.driver.park(plate_height=22, z_offset=4, speed="medium") + await self.driver.park() + self.driver._move_to_teachpoint.assert_has_awaits( # type: ignore[attr-defined] + [ + call( + protocol.TEACHPOINT_PARK, + 4, + 22, + profile=protocol.PROFILE_DYNAMIC_FULL, + speed=protocol.SPEED_MEDIUM, + ), + call( + protocol.TEACHPOINT_PARK, + 8, + 15, + profile=protocol.PROFILE_DYNAMIC_FULL, + speed=protocol.SPEED_SLOW, + ), + ] + ) + self.assertEqual(self.driver.state.operation, Access2Activity.IDLE) + self.assertEqual(self.driver.state.last_teachpoint, protocol.TEACHPOINT_PARK) + + async def test_invalid_motion_settings_fail_before_io(self): + self.driver.request_status = AsyncMock() # type: ignore[method-assign] + before = self.driver.state + for transfer in (self.driver.load, self.driver.unload): + for parameter in ( + "source_speed", + "destination_speed", + "park_speed", + "gripper_open_speed", + "gripper_close_speed", + "gripper_release_speed", + ): + with self.subTest(operation=transfer.__name__, parameter=parameter): + with self.assertRaisesRegex(ValueError, "Access2 speed"): + await transfer(**{parameter: "invalid"}) # type: ignore[arg-type] + self.assertEqual(self.driver.state, before) + for operation in (self.driver.park, self.driver.open_gripper, self.driver.close_gripper): + with self.subTest(operation=operation.__name__): + with self.assertRaisesRegex(ValueError, "Access2 speed"): + await operation(speed="invalid") # type: ignore[arg-type] + self.assertEqual(self.driver.state, before) + for parameters in ( + {"plate_height": 0}, + {"plate_height": float("nan")}, + {"z_offset": float("inf")}, + ): + with self.subTest(parameters=parameters): + with self.assertRaises(ValueError): + await self.driver.park(**parameters) + self.assertEqual(self.driver.state, before) + self.driver.request_status.assert_not_awaited() # type: ignore[attr-defined] + 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_standalone_gripper_speeds_are_per_call(self): + self.driver.request_status = AsyncMock( # type: ignore[method-assign] + return_value=_status(flags=_READY_FLAGS) + ) + await self.driver.open_gripper(speed="medium") + await self.driver.open_gripper() + self.driver._move_axis_to_position.assert_has_awaits( # type: ignore[attr-defined] + [ + call( + protocol.AXIS_GRIPPER, + 0, + profile=protocol.PROFILE_DYNAMIC_EMPTY, + speed=protocol.SPEED_MEDIUM, + ), + call( + protocol.AXIS_GRIPPER, + 0, + profile=protocol.PROFILE_DYNAMIC_EMPTY, + speed=protocol.SPEED_SLOW, + ), + ] + ) + await self.driver.close_gripper(speed="fast") + await self.driver.close_gripper() + self.driver._close_gripper.assert_has_awaits( # type: ignore[attr-defined] + [ + call(gripper_closed_position=5.68, gripper_close_threshold=1.5, speed=protocol.SPEED_FAST), + call(gripper_closed_position=5.68, gripper_close_threshold=1.5, speed=protocol.SPEED_SLOW), + ] + ) + + async def test_transfer_speeds_are_per_call_in_both_directions(self): + self.driver.request_status = AsyncMock( # type: ignore[method-assign] + return_value=_status(flags=_READY_FLAGS) + ) + self.driver.request_sensor_values = AsyncMock( # type: ignore[method-assign] + return_value=protocol.STATUS_OPTICAL_PLATE_SENSOR + ) + for transfer in (self.driver.load, self.driver.unload): + with self.subTest(direction=transfer.__name__): + await transfer( + source_speed="medium", + destination_speed="fast", + park_speed="medium", + gripper_open_speed="slow", + gripper_close_speed="fast", + gripper_release_speed="medium", + ) + source, destination = ( + (protocol.TEACHPOINT_PICK, protocol.TEACHPOINT_BUCKET_1) + if transfer == self.driver.load + else (protocol.TEACHPOINT_BUCKET_1, protocol.TEACHPOINT_PICK) + ) + self.driver._move_to_teachpoint.assert_has_awaits( # type: ignore[attr-defined] + [ + call(source, 3, 10, speed=protocol.SPEED_MEDIUM), + call( + destination, 3, 10, profile=protocol.PROFILE_DYNAMIC_FULL, speed=protocol.SPEED_FAST + ), + call( + protocol.TEACHPOINT_PARK, + 3 if transfer == self.driver.load else 0, + 10, + speed=protocol.SPEED_MEDIUM, + ), + ] + ) + self.driver._move_axis_to_position.assert_has_awaits( # type: ignore[attr-defined] + [ + call( + protocol.AXIS_GRIPPER, + 0, + profile=protocol.PROFILE_DYNAMIC_EMPTY, + speed=protocol.SPEED_SLOW, + ), + call( + protocol.AXIS_GRIPPER, + 0, + profile=protocol.PROFILE_DYNAMIC_EMPTY, + speed=protocol.SPEED_MEDIUM, + ), + ] + ) + self.driver._close_gripper.assert_awaited_with( # type: ignore[attr-defined] + gripper_closed_position=5.68, gripper_close_threshold=1.5, speed=protocol.SPEED_FAST + ) + await transfer() + self.driver._close_gripper.assert_awaited_with( # type: ignore[attr-defined] + gripper_closed_position=5.68, gripper_close_threshold=1.5, speed=protocol.SPEED_SLOW + ) + + async def test_transfer_parameters_are_per_call_in_both_directions(self): + self.driver.request_status = AsyncMock( # type: ignore[method-assign] + return_value=_status(flags=_READY_FLAGS) + ) + self.driver.request_sensor_values = AsyncMock( # type: ignore[method-assign] + return_value=protocol.STATUS_OPTICAL_PLATE_SENSOR + ) + for transfer in (self.driver.load, self.driver.unload): + with self.subTest(direction=transfer.__name__): + await transfer( + protocol.TEACHPOINT_BUCKET_2, + plate_height=22, + source_z_offset=4, + destination_z_offset=2, + park_z_offset=1, + gripper_open_position=0.25, + gripper_closed_position=4.75, + gripper_close_threshold=1.8, + ) + source, destination = ( + (protocol.TEACHPOINT_PICK, protocol.TEACHPOINT_BUCKET_2) + if transfer == self.driver.load + else (protocol.TEACHPOINT_BUCKET_2, protocol.TEACHPOINT_PICK) + ) + self.driver._move_to_teachpoint.assert_has_awaits( # type: ignore[attr-defined] + [ + call(source, 4, 22, speed=protocol.SPEED_SLOW), + call( + destination, 2, 22, profile=protocol.PROFILE_DYNAMIC_FULL, speed=protocol.SPEED_SLOW + ), + call(protocol.TEACHPOINT_PARK, 1, 22, speed=protocol.SPEED_SLOW), + ] + ) + self.driver._close_gripper.assert_awaited_with( # type: ignore[attr-defined] + gripper_closed_position=4.75, gripper_close_threshold=1.8, speed=protocol.SPEED_SLOW + ) + self.driver._move_axis_to_position.assert_awaited_with( # type: ignore[attr-defined] + protocol.AXIS_GRIPPER, + 0.25, + profile=protocol.PROFILE_DYNAMIC_EMPTY, + speed=protocol.SPEED_SLOW, + ) + self.assertEqual(self.driver.state.operation, Access2Activity.IDLE) + await transfer() + self.driver._close_gripper.assert_awaited_with( # type: ignore[attr-defined] + gripper_closed_position=5.68, gripper_close_threshold=1.5, speed=protocol.SPEED_SLOW + ) + self.driver._move_axis_to_position.assert_awaited_with( # type: ignore[attr-defined] + protocol.AXIS_GRIPPER, + 0, + profile=protocol.PROFILE_DYNAMIC_EMPTY, + speed=protocol.SPEED_SLOW, + ) + self.driver._move_to_teachpoint.assert_awaited_with( # type: ignore[attr-defined] + protocol.TEACHPOINT_PARK, + 3 if transfer == self.driver.load else 0, + 10, + speed=protocol.SPEED_SLOW, + ) + + async def test_invalid_transfer_parameters_fail_before_io(self): + self.driver.request_status = AsyncMock() # type: ignore[method-assign] + before = self.driver.state + for transfer in (self.driver.load, self.driver.unload): + for parameters in ( + {"gripper_close_threshold": 0}, + {"gripper_closed_position": 1}, + {"gripper_open_position": float("nan")}, + {"gripper_closed_position": float("inf")}, + {"plate_height": 0}, + {"plate_height": -1}, + {"plate_height": float("nan")}, + {"source_z_offset": float("inf")}, + {"destination_z_offset": float("nan")}, + {"park_z_offset": float("inf")}, + ): + with self.subTest(direction=transfer.__name__, parameters=parameters): + with self.assertRaises(ValueError): + await transfer(**parameters) + self.assertEqual(self.driver.state, before) + self.driver.request_status.assert_not_awaited() # type: ignore[attr-defined] + 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_gripper_state_methods_use_absolute_positions(self): + self.driver.request_status = AsyncMock( # type: ignore[method-assign] + return_value=_status(flags=_READY_FLAGS) + ) + + await self.driver.close_gripper() + await self.driver.open_gripper() + + self.driver._close_gripper.assert_awaited_once_with( # type: ignore[attr-defined] + gripper_closed_position=5.68, gripper_close_threshold=1.5, speed=protocol.SPEED_SLOW + ) + self.driver._move_axis_to_position.assert_awaited_once_with( # type: ignore[attr-defined] + protocol.AXIS_GRIPPER, + 0.0, + profile=protocol.PROFILE_DYNAMIC_EMPTY, + speed=protocol.SPEED_SLOW, + ) + + async def test_failure_before_actuation_restores_operation(self): + before = self.driver.state + + with self.assertRaisesRegex(RuntimeError, "precondition failed"): + async with self.driver._operation_scope(Access2Activity.MOVING): + raise RuntimeError("precondition failed") + + self.assertEqual(self.driver.state, before) + + async def test_actuated_failure_retains_transfer_phase_and_requires_recovery(self): + progress = TransferProgress( + direction=TransferDirection.INTO_CENTRIFUGE, + bucket_teachpoint=protocol.TEACHPOINT_BUCKET_1, + ) + + with self.assertRaisesRegex(RuntimeError, "motion failed"): + async with self.driver._operation_scope(progress) as transition: + self.driver._set_transfer_phase(TransferPhase.MOVING_TO_DESTINATION) + transition.mark_actuated(position_uncertain=True) + raise RuntimeError("motion failed") + + self.assertTrue(self.driver.state.recovery_required) + self.assertIsNone(self.driver.state.last_teachpoint) + self.assertIsInstance(self.driver.state.operation, TransferProgress) + assert isinstance(self.driver.state.operation, TransferProgress) + self.assertEqual( + self.driver.state.operation.phase, + TransferPhase.MOVING_TO_DESTINATION, + ) + + async def test_stop_invalidates_connection_and_teachpoint(self): + self.driver.io.stop = AsyncMock() # type: ignore[method-assign] + + await self.driver.stop() + + self.assertEqual(self.driver.state.connection, ConnectionState.DISCONNECTED) + self.assertIsNone(self.driver.state.last_teachpoint) + + async def test_setup_opens_gripper_with_default_position_and_profile(self): + driver = Access2Driver(device_id="test") + 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.0, + profile=protocol.PROFILE_DYNAMIC_EMPTY, + 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=0x03, + gripper_position=5.671, + ) + self.driver.request_status = AsyncMock(return_value=closed) # type: ignore[method-assign] + + await self.driver.close_gripper() + + self.driver._close_gripper.assert_not_awaited() # type: ignore[attr-defined] + + async def test_close_gripper_is_idempotent_at_contact_position(self): + closed = protocol.Access2Status( + access2_status=_READY_FLAGS | protocol.STATUS_OPTICAL_PLATE_SENSOR, + vspin_status=0, + gripper_status=protocol.AXIS_STATUS_MOVE_DONE, + gripper_position=1.94, + ) + self.driver.request_status = AsyncMock(return_value=closed) # type: ignore[method-assign] + + await self.driver.close_gripper() + + self.driver._close_gripper.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( # type: ignore[method-assign] + return_value=0x03 | protocol.STATUS_OPTICAL_PLATE_SENSOR + ) + + 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_DYNAMIC_EMPTY, + speed=protocol.SPEED_FAST, + ), + call( + protocol.AXIS_GRIPPER, + 0, + profile=protocol.PROFILE_DYNAMIC_EMPTY, + speed=protocol.SPEED_SLOW, + ), + ] + ) + self.driver._close_gripper.assert_awaited_once_with( # type: ignore[attr-defined] + gripper_closed_position=5.68, gripper_close_threshold=1.5, speed=protocol.SPEED_SLOW + ) + self.driver._move_to_teachpoint.assert_has_awaits( # type: ignore[attr-defined] + [ + call(protocol.TEACHPOINT_PICK, 3, 10, speed=protocol.SPEED_SLOW), + call( + protocol.TEACHPOINT_BUCKET_1, + 3, + 10, + profile=protocol.PROFILE_DYNAMIC_FULL, + speed=protocol.SPEED_SLOW, + ), + call(protocol.TEACHPOINT_PARK, 3, 10, speed=protocol.SPEED_SLOW), + ] + ) + self.assertEqual(self.driver.state.operation, Access2Activity.IDLE) + self.assertEqual(self.driver.state.last_teachpoint, protocol.TEACHPOINT_PARK) + + async def test_load_uses_selected_bucket_teachpoint(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( # type: ignore[method-assign] + return_value=protocol.STATUS_OPTICAL_PLATE_SENSOR + ) + + await self.driver.load(protocol.TEACHPOINT_BUCKET_2) + + self.driver._move_to_teachpoint.assert_any_await( # type: ignore[attr-defined] + protocol.TEACHPOINT_BUCKET_2, + 3, + 10, + profile=protocol.PROFILE_DYNAMIC_FULL, + speed=protocol.SPEED_SLOW, + ) + + async def test_load_reports_each_transfer_phase_at_its_actuation_boundary(self): + ready = _status(flags=_READY_FLAGS) + observed: list[TransferPhase] = [] + + def record_phase() -> None: + operation = self.driver.state.operation + assert isinstance(operation, TransferProgress) + observed.append(operation.phase) + + async def move_axis(*args: object, **kwargs: object) -> None: + del args, kwargs + record_phase() + + async def move_to_teachpoint(*args: object, **kwargs: object) -> None: + del args, kwargs + record_phase() + + async def sense_plate() -> int: + record_phase() + return protocol.STATUS_OPTICAL_PLATE_SENSOR + + async def close_gripper(**parameters: float) -> protocol.Access2Status: + record_phase() + return ready + + self.driver.request_status = AsyncMock(side_effect=[ready, ready]) # type: ignore[method-assign] + self.driver.request_sensor_values = AsyncMock(side_effect=sense_plate) # type: ignore[method-assign] + self.driver._move_axis_to_position = AsyncMock(side_effect=move_axis) # type: ignore[method-assign] + self.driver._move_to_teachpoint = AsyncMock(side_effect=move_to_teachpoint) # type: ignore[method-assign] + self.driver._close_gripper = AsyncMock(side_effect=close_gripper) # type: ignore[method-assign] + + await self.driver.load() + + self.assertEqual( + observed, + [ + TransferPhase.APPROACHING_SOURCE, + TransferPhase.APPROACHING_SOURCE, + TransferPhase.AT_SOURCE, + TransferPhase.GRIPPING, + TransferPhase.MOVING_TO_DESTINATION, + TransferPhase.RELEASING, + TransferPhase.RETURNING_TO_PARK, + ], + ) + + 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._close_gripper.assert_not_awaited() # type: ignore[attr-defined] + self.driver._move_to_teachpoint.assert_awaited_once_with( # type: ignore[attr-defined] + protocol.TEACHPOINT_PICK, 3, 10, speed=protocol.SPEED_SLOW + ) + + 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._close_gripper.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._close_gripper.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..6af49bf6cd6 100644 --- a/pylabrobot/agilent/vspin/vspin.py +++ b/pylabrobot/agilent/vspin/vspin.py @@ -1,15 +1,26 @@ +from __future__ import annotations + import asyncio -import ctypes +import dataclasses import json import logging -import math import os -import time import warnings -from typing import Optional - +from contextlib import asynccontextmanager +from typing import AsyncIterator, Optional + +from pylabrobot.agilent.vspin import _nmc +from pylabrobot.agilent.vspin._state import ( + ConnectionState, + TransitionToken, + VSpinActivity, + VSpinHomingState, + VSpinInitializationState, + VSpinMachineState, +) +from pylabrobot.agilent.vspin.errors import CentrifugeDoorError from pylabrobot.events import device_reference, evented_operation, resource_reference -from pylabrobot.io.ftdi import FTDI +from pylabrobot.io.ftdi import FTDI, is_ftdi_transport_error from pylabrobot.resources import Coordinate, ResourceHolder logger = logging.getLogger(__name__) @@ -31,10 +42,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 +60,76 @@ 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 +_SERVO_STOP_CONFIRMATION_SAMPLES = 3 +_TACHOMETER_TO_RPM = -14.69320388 +_NETWORK_PROBE_TIMEOUT = 0.2 +_NETWORK_INPUT_QUIET_TIME = 0.1 +_NETWORK_INPUT_DRAIN_TIMEOUT = 1.0 +_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 +138,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 +190,18 @@ 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._nmc_lock = asyncio.Lock() + self._command_lock = asyncio.Lock() + self._state = VSpinMachineState() + self._spin_cancel_requested = False + self._spin_stop_deceleration: float | None = None + self._spin_completion_event = asyncio.Event() + self._spin_completion_event.set() 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) @@ -122,192 +220,791 @@ def __init__(self, name: str, device_id: Optional[str] = None): child_location=Coordinate.zero(), ) - # Door and rotor state, tracked from the commands we issue: the controller has no query for - # which bucket is parked at the load position. - self._door_open = False + # The controller has no query for which bucket is parked at the load position. self._at_bucket: Optional[ResourceHolder] = None @property - def door_open(self) -> bool: - """Whether the door was left open by the last door command.""" - return self._door_open + def state(self) -> VSpinMachineState: + """Return the current VSpin semantic-state snapshot.""" + return self._state @property def at_bucket(self) -> Optional[ResourceHolder]: """The bucket parked at the load position, or None if the rotor is elsewhere.""" return self._at_bucket - async def setup(self): + def _set_activity(self, activity: VSpinActivity) -> None: + """Replace only the VSpin activity dimension.""" + self._state = dataclasses.replace(self._state, activity=activity) + + def _mark_recovery_required(self, *, position_uncertain: bool) -> None: + """Make recovery sticky and invalidate an uncertain rotor presentation.""" + self._state = dataclasses.replace(self._state, recovery_required=True) + if position_uncertain: + self._at_bucket = None + + def _record_disconnected(self) -> None: + """Record transport closure without guessing retained controller state.""" + self._state = dataclasses.replace( + self._state, + connection=ConnectionState.DISCONNECTED, + initialization=VSpinInitializationState.UNKNOWN, + homing=VSpinHomingState.UNKNOWN, + ) + self._at_bucket = None + + def _require_operational_state(self) -> None: + """Reject ordinary motion unless the semantic lifecycle is ready.""" + if self._state.connection is not ConnectionState.CONNECTED: + raise RuntimeError("VSpin is not connected") + if self._state.initialization is not VSpinInitializationState.INITIALIZED: + raise RuntimeError("VSpin is not initialized") + if self._state.homing is not VSpinHomingState.HOMED: + raise RuntimeError("VSpin is not homed") + if self._state.recovery_required: + raise RuntimeError("VSpin requires recovery") + if self._state.activity is not VSpinActivity.IDLE: + raise RuntimeError("Another VSpin operation is active") + + @asynccontextmanager + async def _command_scope( + self, + name: str, + *, + activity: VSpinActivity | None = None, + require_ready: bool = True, + ) -> AsyncIterator[TransitionToken]: + """Own the command lock and apply actuation-aware state changes.""" + if self._command_lock.locked(): + raise RuntimeError(f"Cannot {name} while another VSpin command is active") + async with self._command_lock: + previous = self._state + transition = TransitionToken() + try: + if require_ready: + self._require_operational_state() + if activity is not None: + self._set_activity(activity) + yield transition + except BaseException as error: + transport_failed = is_ftdi_transport_error(error) + if transport_failed: + self._record_disconnected() + if transition.actuated: + self._mark_recovery_required(position_uncertain=transition.position_uncertain) + elif activity is not None: + if transport_failed: + self._set_activity(previous.activity) + else: + self._state = previous + raise + else: + if activity is not None: + self._set_activity(VSpinActivity.IDLE) + + async def setup(self) -> None: + """Connect, initialize, home, and place the VSpin in its safe setup position.""" + async with self._command_scope("set up VSpin", require_ready=False) as transition: + try: + await self._setup(transition=transition) + except BaseException: + if transition.position_uncertain: + 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 setup error", self.device_id + ) + raise + + async def _setup(self, *, transition: TransitionToken) -> None: + """Run the VSpin setup sequence while recording verified lifecycle checkpoints.""" + if self._state.recovery_required: + raise RuntimeError("VSpin requires recovery during setup") 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) + self._state = dataclasses.replace( + self._state, + connection=ConnectionState.CONNECTING, + initialization=VSpinInitializationState.UNKNOWN, + homing=VSpinHomingState.UNKNOWN, + ) + try: + await self.io.setup() + except BaseException: + self._record_disconnected() + raise + self._state = dataclasses.replace(self._state, connection=ConnectionState.CONNECTED) + await self._configure_ftdi() + self._state = dataclasses.replace( + self._state, + initialization=VSpinInitializationState.INITIALIZING, + ) + transition.mark_actuated() + 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.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(1 << _nmc.OUTPUT_VERSION_TOGGLE) + await self._write_io_output(0x0000) + self._state = dataclasses.replace( + self._state, + initialization=VSpinInitializationState.INITIALIZED, + ) + await self._lock_door(transition=transition) + + 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 + self._state = dataclasses.replace(self._state, homing=VSpinHomingState.HOMING) + transition.mark_actuated(position_uncertain=True) + self._at_bucket = None + 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")) - - await self.lock_door() - - async def stop(self): + 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") + transition.confirm_position() + self._state = dataclasses.replace(self._state, homing=VSpinHomingState.HOMED) + + await self._disable_servo_after_motion() + + await self._lock_door(transition=transition) + + async def stop(self) -> None: + """Close the VSpin transport and invalidate session-scoped state.""" + async with self._command_scope("stop VSpin", require_ready=False): + await self._stop() + + async def _stop(self) -> None: + """Close the VSpin transport and always record it as disconnected.""" logger.info("[vSpin %s] disconnected", self.device_id) - await self.configure_and_initialize() - await self.io.stop() + self._state = dataclasses.replace(self._state, connection=ConnectionState.DISCONNECTING) + try: + await self.io.stop() + finally: + self._record_disconnected() + + 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.""" + try: + async with self._nmc_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) + except BaseException as error: + if is_ftdi_transport_error(error): + self._record_disconnected() + raise + + 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._nmc_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 + + 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() + await self._drain_nmc_input() + + 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], + ) + + 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() + await self._drain_nmc_input() + return - async def configure_and_initialize(self): - await self.set_configuration_data() - await self.initialize() + 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 set_configuration_data(self): - """Set the device configuration data.""" + 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) + + async def _drain_nmc_input(self) -> None: + """Discard reset-time replies until the FTDI receive path remains quiet.""" + loop = asyncio.get_running_loop() + deadline = loop.time() + _NETWORK_INPUT_DRAIN_TIMEOUT + quiet_since: float | None = None + discarded = bytearray() + + while True: + chunk = await self.io.read(64) + now = loop.time() + if chunk: + discarded.extend(chunk) + quiet_since = None + elif quiet_since is None: + quiet_since = now + elif now - quiet_since >= _NETWORK_INPUT_QUIET_TIME: + break + + if now >= deadline: + raise TimeoutError( + "VSpin NMC receive path did not become quiet after reset; " + f"discarded {len(discarded)} bytes: {bytes(discarded).hex()}" + ) + await asyncio.sleep(0.01) + + if discarded: + logger.debug("Discarded reset-time NMC input: %s", bytes(discarded).hex()) # -- 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 for servo motion to finish and both rotor-stop signals to agree.""" + 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() + stopped_samples = 0 + 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: + stopped_samples += 1 + if stopped_samples >= _SERVO_STOP_CONFIRMATION_SAMPLES: + break + else: + stopped_samples = 0 + 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() + await self._wait_until_rotor_safe_to_access() + + async def stop_spin(self, deceleration: float = 0.8) -> None: + """Ask the active spin owner to decelerate, then wait for its completion.""" + if deceleration <= 0 or deceleration > 1: + raise ValueError("Deceleration must be within 0-1.") + activity = self._state.activity + if activity not in ( + VSpinActivity.PREPARING_TO_SPIN, + VSpinActivity.ACCELERATING, + VSpinActivity.AT_SPEED, + VSpinActivity.DECELERATING, + ): + return + if not self._spin_cancel_requested and activity is not VSpinActivity.DECELERATING: + self._spin_stop_deceleration = deceleration + self._spin_cancel_requested = True + await self._spin_completion_event.wait() + if self._state.recovery_required: + raise RuntimeError("VSpin failed while stopping and requires recovery") 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) + + async def _request_rotor_safe_to_access(self) -> bool: + """Return whether the servo and spinning input both confirm a stopped rotor.""" + status = await self.request_positions_and_tachometer() + if status.velocity is None: + raise RuntimeError("VSpin velocity was absent while checking whether the rotor is stopped") + servo_stopped = bool(status.status & _nmc.STATUS_MOVE_DONE) and status.velocity == 0 + return servo_stopped and not await self.request_spinning() + + async def _wait_until_rotor_safe_to_access(self) -> None: + """Wait for independent servo and spinning-input stop confirmation.""" + loop = asyncio.get_running_loop() + deadline = loop.time() + _IO_TRANSITION_TIMEOUT + while not await self._request_rotor_safe_to_access(): + if loop.time() >= deadline: + raise TimeoutError( + f"VSpin rotor did not become safe to access within {_IO_TRANSITION_TIMEOUT} seconds" + ) + await asyncio.sleep(_STATUS_POLL_INTERVAL) + + @asynccontextmanager + async def reserve_transfer( + self, + bucket: ResourceHolder, + ) -> AsyncIterator[TransitionToken]: + """Reserve the load opening while a paired loader may enter the rotor.""" + async with self._command_scope( + "transfer a plate", + activity=VSpinActivity.TRANSFERRING, + ) as transition: + if self._at_bucket is not bucket: + raise RuntimeError("Requested bucket is not confirmed at the load opening") + if not await self.request_door_open(): + raise CentrifugeDoorError("Centrifuge door-open sensor must be active") + if not await self.request_bucket_locked(): + raise RuntimeError("Centrifuge bucket must be physically locked") + if not await self._request_rotor_safe_to_access(): + raise RuntimeError("Centrifuge rotor must be stopped") + yield transition # -- bucket calibration -- @@ -319,104 +1016,291 @@ def bucket_1_remainder(self) -> int: async def set_bucket_1_position_to_current(self) -> None: """Set the current position as bucket 1 position and save calibration.""" + async with self._command_scope("set the bucket 1 position"): + await self._set_bucket_1_position_to_current() + + async def _set_bucket_1_position_to_current(self) -> None: 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 -- - async def open_door(self): + async def open_door(self) -> None: + """Open the centrifuge door and wait for its physical sensor.""" + async with self._command_scope( + "open the door", + activity=VSpinActivity.CHANGING_INTERLOCKS, + ) as transition: + await self._open_door(transition=transition) + + async def _open_door(self, *, transition: TransitionToken) -> None: + """Open the door within an existing VSpin command scope.""" if await self.request_door_open(): - self._door_open = True return + await self._wait_until_rotor_safe_to_access() logger.info("[vSpin %s] open door", self.device_id) - await self.send_command(bytes.fromhex("aa022600062e")) - await asyncio.sleep(4) - self._door_open = True + transition.mark_actuated() + 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", + ) + + async def close_door(self) -> None: + """Close the centrifuge door and wait for its physical sensor.""" + async with self._command_scope( + "close the door", + activity=VSpinActivity.CHANGING_INTERLOCKS, + ) as transition: + await self._close_door(transition=transition) - async def close_door(self): + async def _close_door(self, *, transition: TransitionToken) -> None: + """Close the door within an existing VSpin command scope.""" if not (await self.request_door_open()): - 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) - self._door_open = False + transition.mark_actuated() + 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", + ) + + async def lock_door(self) -> None: + """Lock the centrifuge door and wait for its physical sensor.""" + async with self._command_scope( + "lock the door", + activity=VSpinActivity.CHANGING_INTERLOCKS, + ) as transition: + await self._lock_door(transition=transition) - async def lock_door(self): + async def _lock_door(self, *, transition: TransitionToken) -> None: + """Lock the door within an existing VSpin command scope.""" if await self.request_door_open(): raise RuntimeError("Cannot lock door while it is open.") if await self.request_door_locked(): return logger.info("[vSpin %s] lock door", self.device_id) - await self.send_command(bytes.fromhex("aa0226000028")) + transition.mark_actuated() + 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) -> None: + """Unlock the centrifuge door and wait for its physical sensor.""" + async with self._command_scope( + "unlock the door", + activity=VSpinActivity.CHANGING_INTERLOCKS, + ) as transition: + await self._unlock_door(transition=transition) - async def unlock_door(self): + async def _unlock_door(self, *, transition: TransitionToken) -> None: + """Unlock the door within an existing VSpin command scope.""" if not await self.request_door_locked(): return - await self.send_command(bytes.fromhex("aa022600042c")) + await self._wait_until_rotor_safe_to_access() + transition.mark_actuated() + 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) -> None: + """Lock the presented bucket and wait for its physical sensor.""" + async with self._command_scope( + "lock the bucket", + activity=VSpinActivity.CHANGING_INTERLOCKS, + ) as transition: + await self._lock_bucket(transition=transition) - async def lock_bucket(self): + async def _lock_bucket(self, *, transition: TransitionToken) -> None: + """Lock the bucket within an existing VSpin command scope.""" if await self.request_bucket_locked(): return - await self.send_command(bytes.fromhex("aa022600072f")) + await self._wait_until_rotor_safe_to_access() + transition.mark_actuated() + 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(): + async def unlock_bucket(self) -> None: + """Unlock the presented bucket and wait for its physical sensor.""" + async with self._command_scope( + "unlock the bucket", + activity=VSpinActivity.CHANGING_INTERLOCKS, + ) as transition: + await self._unlock_bucket(transition=transition) + + async def _unlock_bucket(self, *, transition: TransitionToken) -> None: + """Unlock the bucket within an existing VSpin command scope.""" + if await self.request_bucket_unlocked(): return - await self.send_command(bytes.fromhex("aa022600062e")) + transition.mark_actuated() + 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 + async def go_to_bucket1(self) -> None: + """Present bucket 1 at the load opening.""" + async with self._command_scope( + "move to bucket 1", + activity=VSpinActivity.POSITIONING, + ) as transition: + await self._go_to_bucket( + self.bucket1, + await self.request_bucket_1_position(), + transition=transition, + ) - 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 + async def go_to_bucket2(self) -> None: + """Present bucket 2 at the load opening.""" + async with self._command_scope( + "move to bucket 2", + activity=VSpinActivity.POSITIONING, + ) as transition: + await self._go_to_bucket( + self.bucket2, + await self.request_bucket_2_position(), + transition=transition, + ) - async def go_to_position(self, position: int): + async def _go_to_bucket( + self, + bucket: ResourceHolder, + position: int, + *, + transition: TransitionToken, + ) -> None: + """Run the existing positioning retry and record the confirmed bucket.""" + for attempt in range(_BUCKET_PRESENT_RETRIES + 1): + try: + await self._go_to_position(position, transition=transition) + 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) -> None: + """Move to an absolute encoder position without claiming a presented bucket.""" + async with self._command_scope( + "move to a position", + activity=VSpinActivity.POSITIONING, + ) as transition: + await self._go_to_position(position, transition=transition) + + async def _go_to_position(self, position: int, *, transition: TransitionToken) -> None: + """Move the rotor within an existing VSpin command scope.""" 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.open_door() + await self._close_door(transition=transition) + await self._lock_door(transition=transition) + await self._unlock_bucket(transition=transition) + + await self._enable_amplifier_and_reset_servo_status() + await self._send_nmc(_nmc.build_set_gain(_nmc.PIC_SERVO_ADDRESS, _POSITION_GAINS)) + try: + self._at_bucket = None + transition.mark_actuated(position_uncertain=True) + 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: + motion_context = ( + "after motion started" if motion_started else "without reporting motion start" + ) + raise _PositionAlignmentError( + f"VSpin completed move to encoder position {position} {motion_context}, but settled at " + f"{motion_status.position} (tolerance {_BUCKET_POSITION_TOLERANCE})" + ) + transition.confirm_position() + await self._lock_bucket(transition=transition) + await self._unlock_door(transition=transition) + await self._open_door(transition=transition) @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( @@ -443,12 +1327,46 @@ async def spin( if duration < 1: raise ValueError("Spin time must be at least 1 second") + owns_completion_event = False + try: + async with self._command_scope( + "start a spin", + activity=VSpinActivity.PREPARING_TO_SPIN, + ) as transition: + self._spin_completion_event.clear() + self._spin_cancel_requested = False + self._spin_stop_deceleration = None + owns_completion_event = True + await self._run_spin_cycle( + g, + duration, + acceleration, + deceleration, + transition=transition, + ) + finally: + if owns_completion_event: + self._spin_completion_event.set() + + async def _run_spin_cycle( + self, + g: float, + duration: float, + acceleration: float, + deceleration: float, + *, + transition: TransitionToken, + ) -> None: + """Run the spin algorithm while recording its confirmed phase boundaries.""" if await self.request_door_open(): - await self.close_door() + await self._close_door(transition=transition) if not await self.request_door_locked(): - await self.lock_door() + await self._lock_door(transition=transition) if await self.request_bucket_locked(): - await self.unlock_bucket() + await self._unlock_bucket(transition=transition) + + if self._spin_cancel_requested: + return rpm = VSpin.g_to_rpm(g) logger.info( @@ -461,82 +1379,94 @@ 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")) + if self._spin_cancel_requested: + return - 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.") + trajectory_may_be_active = False + try: + transition.mark_actuated() + await self._enable_amplifier_and_reset_servo_status() + if self._spin_cancel_requested: + await self._disable_servo_after_motion() + return + await self._send_nmc(_nmc.build_set_gain(_nmc.PIC_SERVO_ADDRESS, _VELOCITY_GAINS)) + if self._spin_cancel_requested: + await self._disable_servo_after_motion() + return + + await self._raise_for_spin_faults() + if self._spin_cancel_requested: + await self._disable_servo_after_motion() + return + self._at_bucket = None + self._set_activity(VSpinActivity.ACCELERATING) + transition.mark_actuated(position_uncertain=True) + # The controller may start moving before its reply is received. + trajectory_may_be_active = True + await self._send_nmc(spin_trajectory) + + await self._wait_for_target_speed(rpm, acceleration) + if not self._spin_cancel_requested: + self._set_activity(VSpinActivity.AT_SPEED) + 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, + ) + + self._set_activity(VSpinActivity.DECELERATING) + active_deceleration = self._spin_stop_deceleration or deceleration + await self._command_deceleration(active_deceleration) + await self._wait_until_stopped(rpm, active_deceleration) + trajectory_may_be_active = False + transition.confirm_position() + except BaseException: + if trajectory_may_be_active: + self._set_activity(VSpinActivity.DECELERATING) + active_deceleration = self._spin_stop_deceleration or deceleration + try: + await asyncio.shield(self._command_deceleration(active_deceleration)) + await asyncio.shield(self._wait_until_stopped(rpm, active_deceleration)) + except Exception: + logger.exception("[vSpin %s] emergency deceleration failed", self.device_id) + else: + 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 spin preparation error", self.device_id + ) + raise # 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..9f050bbca73 100644 --- a/pylabrobot/agilent/vspin/vspin_tests.py +++ b/pylabrobot/agilent/vspin/vspin_tests.py @@ -1,11 +1,183 @@ +from __future__ import annotations + +import asyncio +import dataclasses import unittest -from unittest.mock import AsyncMock, patch +from collections import deque +from typing import Awaitable, Callable +from unittest.mock import ANY, AsyncMock, call, patch +from pylabrobot.agilent.vspin import _access2_protocol as protocol +from pylabrobot.agilent.vspin import _nmc +from pylabrobot.agilent.vspin import vspin as vspin_module +from pylabrobot.agilent.vspin._state import ( + ConnectionState, + TransitionToken, + VSpinActivity, + VSpinHomingState, + VSpinInitializationState, +) 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 _mark_vspin_ready(vspin: VSpin) -> None: + """Put a mock-backed VSpin at the verified lifecycle boundary under test.""" + vspin._state = dataclasses.replace( + vspin.state, + connection=ConnectionState.CONNECTED, + initialization=VSpinInitializationState.INITIALIZED, + homing=VSpinHomingState.HOMED, + ) + + +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): @@ -15,17 +187,26 @@ def setUp(self): async def test_spin_emits_loaded_bucket_resources_and_parameters(self): vspin = VSpin(name="centrifuge", device_id="test") + _mark_vspin_ready(vspin) plate = Resource("plate_1", size_x=1, size_y=1, size_z=1) vspin.bucket1.assign_child_resource(plate, location=Coordinate.zero()) 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.request_spinning = AsyncMock(return_value=False) # type: ignore[method-assign] + 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,8 +233,28 @@ 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") + _mark_vspin_ready(vspin) events: list[PLREvent] = [] event_bus = EventBus() event_bus.subscribe(events.append) @@ -74,15 +275,24 @@ async def test_spin_failure_emits_requested_parameters(self): async def test_spin_accepts_positional_parameters_with_event_bus(self): vspin = VSpin(name="centrifuge", device_id="test") + _mark_vspin_ready(vspin) 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.request_spinning = AsyncMock(return_value=False) # type: ignore[method-assign] + 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 +307,1454 @@ 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") + _mark_vspin_ready(self.vspin) + + async def test_connection_does_not_imply_initialization_or_homing(self): + vspin = VSpin(name="fresh centrifuge") + vspin._state = dataclasses.replace( + vspin.state, + connection=ConnectionState.CONNECTED, + ) + + with self.assertRaisesRegex(RuntimeError, "not initialized"): + await vspin.open_door() + + async def test_failure_before_actuation_restores_activity(self): + before = self.vspin.state + + with self.assertRaisesRegex(RuntimeError, "precondition failed"): + async with self.vspin._command_scope( + "test transition", + activity=VSpinActivity.POSITIONING, + ): + raise RuntimeError("precondition failed") + + self.assertEqual(self.vspin.state, before) + + async def test_setup_network_failure_does_not_send_motor_commands(self) -> None: + """A failure before setup motion must not send commands to an uninitialized servo.""" + self.vspin._initialize_nmc_network = AsyncMock( # type: ignore[method-assign] + side_effect=TimeoutError("network initialization failed") + ) + send_nmc = AsyncMock() + self.vspin._send_nmc = send_nmc # type: ignore[method-assign] + + with self.assertRaisesRegex(TimeoutError, "network initialization failed"): + await self.vspin.setup() + + send_nmc.assert_not_awaited() + self.assertTrue(self.vspin.state.recovery_required) + self.assertFalse(self.vspin._command_lock.locked()) + + async def test_actuated_position_failure_invalidates_presentation(self): + self.vspin._at_bucket = self.vspin.bucket1 + + with self.assertRaisesRegex(RuntimeError, "motion failed"): + async with self.vspin._command_scope( + "test transition", + activity=VSpinActivity.POSITIONING, + ) as transition: + transition.mark_actuated(position_uncertain=True) + raise RuntimeError("motion failed") + + self.assertTrue(self.vspin.state.recovery_required) + self.assertEqual(self.vspin.state.activity, VSpinActivity.POSITIONING) + self.assertIsNone(self.vspin.at_bucket) + + async def test_transfer_reservation_owns_vspin_command_state(self): + self.vspin._at_bucket = self.vspin.bucket2 + 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_rotor_safe_to_access = AsyncMock( # type: ignore[method-assign] + return_value=True + ) + + async with self.vspin.reserve_transfer(self.vspin.bucket2): + self.assertTrue(self.vspin._command_lock.locked()) + self.assertEqual(self.vspin.state.activity, VSpinActivity.TRANSFERRING) + + self.assertEqual(self.vspin.state.activity, VSpinActivity.IDLE) + + async def test_spin_reports_acceleration_cruise_and_deceleration_phases(self): + observed: list[tuple[str, VSpinActivity]] = [] + 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(side_effect=[0, 100]) # type: ignore[method-assign] + self.vspin._enable_amplifier_and_reset_servo_status = AsyncMock() # type: ignore[method-assign] + self.vspin._send_nmc = AsyncMock( # type: ignore[method-assign] + return_value=_nmc.NMCResponse(status=0, data=b"") + ) + self.vspin._raise_for_spin_faults = AsyncMock() # type: ignore[method-assign] + + async def wait_for_target_speed(rpm: float, acceleration: float) -> None: + del rpm, acceleration + observed.append(("acceleration", self.vspin.state.activity)) + + async def wait_for_position( + position: int, + timeout: float, + operation: str, + *, + cancel_on_spin_abort: bool = False, + ) -> int: + del timeout, operation, cancel_on_spin_abort + observed.append(("cruise", self.vspin.state.activity)) + return position + + async def command_deceleration(deceleration: float) -> None: + del deceleration + observed.append(("deceleration command", self.vspin.state.activity)) + + async def wait_until_stopped(initial_rpm: float, deceleration: float) -> None: + del initial_rpm, deceleration + observed.append(("stopped waiter", self.vspin.state.activity)) + + self.vspin._wait_for_target_speed = AsyncMock(side_effect=wait_for_target_speed) # type: ignore[method-assign] + self.vspin._wait_for_position = AsyncMock(side_effect=wait_for_position) # type: ignore[method-assign] + self.vspin._command_deceleration = AsyncMock(side_effect=command_deceleration) # type: ignore[method-assign] + self.vspin._wait_until_stopped = AsyncMock(side_effect=wait_until_stopped) # type: ignore[method-assign] + + await self.vspin.spin(g=500, duration=1, acceleration=0.5, deceleration=0.5) + + self.assertEqual( + observed, + [ + ("acceleration", VSpinActivity.ACCELERATING), + ("cruise", VSpinActivity.AT_SPEED), + ("deceleration command", VSpinActivity.DECELERATING), + ("stopped waiter", VSpinActivity.DECELERATING), + ], + ) + self.assertEqual(self.vspin.state.activity, VSpinActivity.IDLE) + + async def test_stop_invalidates_session_state_and_bucket_presentation(self): + self.vspin._at_bucket = self.vspin.bucket1 + self.io.stop = AsyncMock() + + await self.vspin.stop() + + self.assertEqual(self.vspin.state.connection, ConnectionState.DISCONNECTED) + self.assertEqual(self.vspin.state.initialization, VSpinInitializationState.UNKNOWN) + self.assertEqual(self.vspin.state.homing, VSpinHomingState.UNKNOWN) + self.assertIsNone(self.vspin.at_bucket) + + async def test_ftdi_error_invalidates_lifecycle_and_bucket_presentation(self): + self.vspin._at_bucket = self.vspin.bucket1 + self.io.write = AsyncMock(side_effect=RuntimeError("transport lost")) + + with ( + patch("pylabrobot.agilent.vspin.vspin.is_ftdi_transport_error", return_value=True), + self.assertRaisesRegex(RuntimeError, "transport lost"), + ): + await self.vspin.open_door() + + self.assertEqual(self.vspin.state.connection, ConnectionState.DISCONNECTED) + self.assertEqual(self.vspin.state.initialization, VSpinInitializationState.UNKNOWN) + self.assertEqual(self.vspin.state.homing, VSpinHomingState.UNKNOWN) + self.assertEqual(self.vspin.state.activity, VSpinActivity.IDLE) + self.assertIsNone(self.vspin.at_bucket) + + 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_nmc_input_drain_discards_bytes_until_quiet(self): + self.io.read = AsyncMock(side_effect=[b"\x00\x89", b"", b""]) + + with patch("pylabrobot.agilent.vspin.vspin._NETWORK_INPUT_QUIET_TIME", 0): + await self.vspin._drain_nmc_input() + + self.assertEqual(self.io.read.await_count, 3) + + async def test_nmc_input_drain_times_out_when_bytes_do_not_stop(self): + self.io.read = AsyncMock(return_value=b"\x00\x89") + + with ( + patch("pylabrobot.agilent.vspin.vspin._NETWORK_INPUT_DRAIN_TIMEOUT", 0), + self.assertRaisesRegex(TimeoutError, "discarded 2 bytes: 0089"), + ): + await self.vspin._drain_nmc_input() + + 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_command_lock_rejects_state_changing_commands_before_hardware_access(self): + self.vspin.request_door_open = AsyncMock() # type: ignore[method-assign] + operations = ( + ("setup", self.vspin.setup), + ("stop", self.vspin.stop), + ("set bucket 1 position", self.vspin.set_bucket_1_position_to_current), + ("open door", self.vspin.open_door), + ("close door", self.vspin.close_door), + ("lock door", self.vspin.lock_door), + ("unlock door", self.vspin.unlock_door), + ("lock bucket", self.vspin.lock_bucket), + ("unlock bucket", self.vspin.unlock_bucket), + ("go to bucket 1", self.vspin.go_to_bucket1), + ("go to bucket 2", self.vspin.go_to_bucket2), + ("go to position", lambda: self.vspin.go_to_position(0)), + ("spin", lambda: self.vspin.spin(g=500, duration=1)), + ) + await self.vspin._command_lock.acquire() + try: + for name, operation in operations: + with self.subTest(operation=name): + with self.assertRaisesRegex(RuntimeError, "another VSpin command is active"): + await operation() + finally: + self.vspin._command_lock.release() + + self.vspin.request_door_open.assert_not_awaited() # type: ignore[attr-defined] + + async def test_command_lock_is_released_after_preparation_failure(self): + self.vspin.request_door_open = AsyncMock( # type: ignore[method-assign] + side_effect=RuntimeError("door status failed") + ) + + with self.assertRaisesRegex(RuntimeError, "door status failed"): + await self.vspin.spin(g=500, duration=1) + + self.assertFalse(self.vspin._command_lock.locked()) + + async def test_status_queries_remain_available_during_a_command(self): + self.vspin._request_input_flags = AsyncMock(return_value=0) # type: ignore[method-assign] + + await self.vspin._command_lock.acquire() + try: + self.assertFalse(await self.vspin.request_door_open()) + finally: + self.vspin._command_lock.release() + + async def _check_spin_reply_failure(self, *, cancel: bool) -> None: + """Fail a reply after writing the spin trajectory and require controlled stopping.""" + self.vspin._at_bucket = self.vspin.bucket1 + 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=0) # type: ignore[method-assign] + self.vspin._enable_amplifier_and_reset_servo_status = AsyncMock() # type: ignore[method-assign] + self.vspin._raise_for_spin_faults = AsyncMock() # type: ignore[method-assign] + + rpm = VSpin.g_to_rpm(500) + spin_trajectory = _nmc.build_load_trajectory( + _nmc.PIC_SERVO_ADDRESS, + vspin_module._POSITION_TRAJECTORY_MODE, + position=_nmc.spin_target_distance(rpm, duration=1, acceleration=0.5), + velocity=_nmc.rpm_to_nmc_velocity(rpm), + acceleration=_nmc.acceleration_to_nmc(0.5), + ) + deceleration_trajectory = _nmc.build_load_trajectory( + _nmc.PIC_SERVO_ADDRESS, + vspin_module._VELOCITY_TRAJECTORY_MODE, + velocity=0, + acceleration=_nmc.acceleration_to_nmc(0.6), + ) + reply_started = asyncio.Event() + fail_reply = asyncio.Event() + + async def read(length: int) -> bytes: + """Leave the trajectory reply pending until the test times out or cancels it.""" + self.assertEqual(length, 2) + if self.io.write.call_args.args[0] == spin_trajectory: + reply_started.set() + await fail_reply.wait() + raise TimeoutError("spin acknowledgement timed out") + return _nmc_response(_nmc.STATUS_MOVE_DONE) + + async def confirm_stop(initial_rpm: float, deceleration: float) -> None: + """Check that the spin owns its lock and completion event until stop confirmation.""" + self.assertEqual((initial_rpm, deceleration), (rpm, 0.6)) + self.assertTrue(self.vspin._command_lock.locked()) + self.assertFalse(self.vspin._spin_completion_event.is_set()) + + self.io.write = AsyncMock(side_effect=len) + self.io.read = AsyncMock(side_effect=read) + wait_until_stopped = AsyncMock(side_effect=confirm_stop) + self.vspin._wait_until_stopped = wait_until_stopped # type: ignore[method-assign] + + spin_task = asyncio.ensure_future(self.vspin.spin(500, 1, 0.5, 0.6)) + await asyncio.wait_for(reply_started.wait(), timeout=1) + if cancel: + spin_task.cancel() + with self.assertRaises(asyncio.CancelledError): + await spin_task + else: + fail_reply.set() + with self.assertRaisesRegex(TimeoutError, "spin acknowledgement timed out"): + await spin_task + + gain_command = _nmc.build_set_gain(_nmc.PIC_SERVO_ADDRESS, vspin_module._VELOCITY_GAINS) + self.assertEqual( + self.io.write.await_args_list, + [ + call(gain_command), + call(spin_trajectory), + call(gain_command), + call(deceleration_trajectory), + ], + ) + wait_until_stopped.assert_awaited_once_with(rpm, 0.6) + self.assertTrue(self.vspin.state.recovery_required) + self.assertIsNone(self.vspin.at_bucket) + self.assertTrue(self.vspin._spin_completion_event.is_set()) + self.assertFalse(self.vspin._command_lock.locked()) + + async def test_spin_reply_timeout_decelerates_before_reraising(self) -> None: + """A timeout after the motion write must still stop the rotor before returning.""" + await self._check_spin_reply_failure(cancel=False) + + async def test_cancelled_spin_reply_decelerates_before_reraising(self) -> None: + """Cancelling a task awaiting the motion reply must still stop the rotor.""" + await self._check_spin_reply_failure(cancel=True) + + async def test_stop_spin_requests_owner_deceleration_and_waits_for_completion(self): + owner_started = asyncio.Event() + allow_completion = asyncio.Event() + requested_decelerations: list[float | None] = [] + + async def run_spin_cycle( + g: float, + duration: float, + acceleration: float, + deceleration: float, + *, + transition: TransitionToken, + ) -> None: + del g, duration, acceleration, deceleration + transition.mark_actuated(position_uncertain=True) + self.vspin._set_activity(VSpinActivity.AT_SPEED) + owner_started.set() + while not self.vspin._spin_cancel_requested: + await asyncio.sleep(0) + requested_decelerations.append(self.vspin._spin_stop_deceleration) + self.vspin._set_activity(VSpinActivity.DECELERATING) + await allow_completion.wait() + transition.confirm_position() + + self.vspin._run_spin_cycle = AsyncMock( # type: ignore[method-assign] + side_effect=run_spin_cycle + ) + spin_task: asyncio.Future[None] = asyncio.ensure_future(self.vspin.spin(g=500, duration=1)) + await owner_started.wait() + + stop_task = asyncio.create_task(self.vspin.stop_spin(deceleration=0.5)) + while not self.vspin._spin_cancel_requested: + await asyncio.sleep(0) + while not requested_decelerations: + await asyncio.sleep(0) + + self.assertFalse(stop_task.done()) + self.assertEqual(requested_decelerations, [0.5]) + self.assertTrue(self.vspin._command_lock.locked()) + + allow_completion.set() + await asyncio.gather(spin_task, stop_task) + + self.assertEqual(self.vspin.state.activity, VSpinActivity.IDLE) + self.assertFalse(self.vspin.state.recovery_required) + self.assertTrue(self.vspin._spin_completion_event.is_set()) + + async def test_stop_spin_during_preparation_prevents_servo_motion(self): + position_request_started = asyncio.Event() + allow_position_response = asyncio.Event() + + 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] + + async def request_position() -> int: + position_request_started.set() + await allow_position_response.wait() + return 0 + + self.vspin.request_position = AsyncMock(side_effect=request_position) # type: ignore[method-assign] + self.vspin._enable_amplifier_and_reset_servo_status = AsyncMock() # type: ignore[method-assign] + self.vspin._send_nmc = AsyncMock() # type: ignore[method-assign] + + spin_task: asyncio.Future[None] = asyncio.ensure_future(self.vspin.spin(g=500, duration=1)) + await position_request_started.wait() + stop_task = asyncio.create_task(self.vspin.stop_spin(deceleration=0.5)) + while not self.vspin._spin_cancel_requested: + await asyncio.sleep(0) + + self.assertEqual(self.vspin.state.activity, VSpinActivity.PREPARING_TO_SPIN) + self.assertFalse(stop_task.done()) + + allow_position_response.set() + await asyncio.gather(spin_task, stop_task) + + self.vspin._enable_amplifier_and_reset_servo_status.assert_not_awaited() # type: ignore[attr-defined] + self.vspin._send_nmc.assert_not_awaited() # type: ignore[attr-defined] + self.assertEqual(self.vspin.state.activity, VSpinActivity.IDLE) + self.assertTrue(self.vspin._spin_completion_event.is_set()) + + cancellation_seen_by_next_spin: list[bool] = [] + + async def next_spin_cycle( + g: float, + duration: float, + acceleration: float, + deceleration: float, + *, + transition: TransitionToken, + ) -> None: + del g, duration, acceleration, deceleration, transition + cancellation_seen_by_next_spin.append(self.vspin._spin_cancel_requested) + + self.vspin._run_spin_cycle = AsyncMock(side_effect=next_spin_cycle) # type: ignore[method-assign] + + await self.vspin.spin(g=500, duration=1) + + self.assertEqual(cancellation_seen_by_next_spin, [False]) + + async def test_stop_spin_at_end_of_preparation_turns_off_the_servo(self): + final_safety_check_started = asyncio.Event() + allow_safety_check = asyncio.Event() + + 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=0) # type: ignore[method-assign] + self.vspin._enable_amplifier_and_reset_servo_status = AsyncMock() # type: ignore[method-assign] + self.vspin._disable_servo_after_motion = AsyncMock() # type: ignore[method-assign] + self.vspin._send_nmc = AsyncMock( # type: ignore[method-assign] + return_value=_nmc.NMCResponse(status=0, data=b"") + ) + + async def final_safety_check() -> None: + final_safety_check_started.set() + await allow_safety_check.wait() + + self.vspin._raise_for_spin_faults = AsyncMock( # type: ignore[method-assign] + side_effect=final_safety_check + ) + + spin_task: asyncio.Future[None] = asyncio.ensure_future(self.vspin.spin(g=500, duration=1)) + await final_safety_check_started.wait() + stop_task = asyncio.create_task(self.vspin.stop_spin(deceleration=0.5)) + while not self.vspin._spin_cancel_requested: + await asyncio.sleep(0) + + self.assertFalse(stop_task.done()) + + allow_safety_check.set() + await asyncio.gather(spin_task, stop_task) + + self.vspin._enable_amplifier_and_reset_servo_status.assert_awaited_once() # type: ignore[attr-defined] + self.vspin._disable_servo_after_motion.assert_awaited_once() # type: ignore[attr-defined] + self.vspin._send_nmc.assert_awaited_once_with( # type: ignore[attr-defined] + _nmc.build_set_gain(_nmc.PIC_SERVO_ADDRESS, vspin_module._VELOCITY_GAINS) + ) + self.assertEqual(self.vspin.state.activity, VSpinActivity.IDLE) + self.assertFalse(self.vspin.state.recovery_required) + + async def test_spin_preparation_failures_turn_motor_off(self) -> None: + """Preparation errors and task cancellation disable the servo and require recovery.""" + motor_off = _nmc.build_stop_motor(_nmc.PIC_SERVO_ADDRESS, _nmc.MOTOR_OFF) + boundaries = ( + _nmc.build_stop_motor(_nmc.PIC_SERVO_ADDRESS, _nmc.AMPLIFIER_ENABLE), + _nmc.build_set_gain(_nmc.PIC_SERVO_ADDRESS, vspin_module._VELOCITY_GAINS), + None, # The final safety check after amplifier and gain configuration. + ) + for boundary in boundaries: + for cancel, cleanup_fails in ((False, False), (True, False), (False, True)): + with self.subTest(boundary=boundary, cancel=cancel, cleanup_fails=cleanup_fails): + vspin = VSpin(name="centrifuge") + _mark_vspin_ready(vspin) + 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_position = AsyncMock(return_value=0) # type: ignore[method-assign] + boundary_reached = asyncio.Event() + release_boundary = asyncio.Event() + failure = RuntimeError("spin preparation failed") + + async def fail_preparation() -> None: + """Pause preparation until the test fails or cancels the spin task.""" + boundary_reached.set() + await release_boundary.wait() + raise failure + + async def send_nmc(command: bytes) -> _nmc.NMCResponse: + """Fail the selected preparation command or the motor-off cleanup.""" + if command == boundary: + await fail_preparation() + if command == motor_off and boundary_reached.is_set() and cleanup_fails: + raise RuntimeError("motor-off reply failed") + return _nmc.NMCResponse(status=_nmc.STATUS_MOVE_DONE, data=b"") + + send = AsyncMock(side_effect=send_nmc) + vspin._send_nmc = send # type: ignore[method-assign] + vspin._raise_for_spin_faults = AsyncMock( # type: ignore[method-assign] + side_effect=fail_preparation if boundary is None else None + ) + + with ( + patch("pylabrobot.agilent.vspin.vspin._SERVO_TRANSITION_SETTLE_TIME", 0), + patch.object(vspin_module.logger, "exception") as log_exception, + ): + spin_task: asyncio.Future[None] = asyncio.ensure_future(vspin.spin(g=500, duration=1)) + await asyncio.wait_for(boundary_reached.wait(), timeout=1) + if cancel: + spin_task.cancel() + with self.assertRaises(asyncio.CancelledError): + await spin_task + else: + release_boundary.set() + with self.assertRaises(RuntimeError) as raised: + await spin_task + self.assertIs(raised.exception, failure) + + self.assertEqual(send.await_args_list[-1], call(motor_off)) + self.assertFalse( + any(c.args[0][2] & 0x0F == _nmc.CMD_LOAD_TRAJECTORY for c in send.await_args_list) + ) + self.assertTrue(vspin.state.recovery_required) + self.assertEqual(vspin.state.activity, VSpinActivity.PREPARING_TO_SPIN) + self.assertFalse(vspin._command_lock.locked()) + self.assertTrue(vspin._spin_completion_event.is_set()) + with self.assertRaisesRegex(RuntimeError, "requires recovery"): + vspin._require_operational_state() + if cleanup_fails: + log_exception.assert_called_once() + else: + log_exception.assert_not_called() + + async def test_stop_spin_reports_owner_failure_after_recovery_is_recorded(self): + owner_started = asyncio.Event() + + async def fail_spin_cycle( + g: float, + duration: float, + acceleration: float, + deceleration: float, + *, + transition: TransitionToken, + ) -> None: + del g, duration, acceleration, deceleration + transition.mark_actuated(position_uncertain=True) + self.vspin._set_activity(VSpinActivity.AT_SPEED) + owner_started.set() + while not self.vspin._spin_cancel_requested: + await asyncio.sleep(0) + self.vspin._set_activity(VSpinActivity.DECELERATING) + raise RuntimeError("owner stop failed") + + self.vspin._run_spin_cycle = AsyncMock( # type: ignore[method-assign] + side_effect=fail_spin_cycle + ) + spin_task: asyncio.Future[None] = asyncio.ensure_future(self.vspin.spin(g=500, duration=1)) + await owner_started.wait() + stop_task = asyncio.create_task(self.vspin.stop_spin()) + + with self.assertRaisesRegex(RuntimeError, "owner stop failed"): + await spin_task + with self.assertRaisesRegex(RuntimeError, "requires recovery"): + await stop_task + + self.assertTrue(self.vspin.state.recovery_required) + self.assertEqual(self.vspin.state.activity, VSpinActivity.DECELERATING) + + 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_deceleration_ignores_transient_stopped_status_before_motion_starts(self): + self.vspin._raise_for_spin_faults = AsyncMock() # type: ignore[method-assign] + self.vspin.request_positions_and_tachometer = AsyncMock( # type: ignore[method-assign] + side_effect=[ + _nmc.ServoStatus(status=_nmc.STATUS_MOVE_DONE, velocity=0), + _nmc.ServoStatus(status=0, velocity=-1), + _nmc.ServoStatus(status=_nmc.STATUS_MOVE_DONE, velocity=0), + _nmc.ServoStatus(status=_nmc.STATUS_MOVE_DONE, velocity=0), + _nmc.ServoStatus(status=_nmc.STATUS_MOVE_DONE, velocity=0), + ] + ) + self.vspin._wait_until_rotor_safe_to_access = AsyncMock() # type: ignore[method-assign] + + with patch("pylabrobot.agilent.vspin.vspin.asyncio.sleep", new=AsyncMock()): + await self.vspin._wait_until_stopped(initial_rpm=100, deceleration=0.5) + + self.assertEqual( # type: ignore[attr-defined] + self.vspin.request_positions_and_tachometer.await_count, + 5, + ) + self.vspin._wait_until_rotor_safe_to_access.assert_awaited_once() # type: ignore[attr-defined] + + async def test_rotor_access_requires_servo_stop_and_cleared_spinning_input(self): + self.vspin.request_positions_and_tachometer = AsyncMock( # type: ignore[method-assign] + side_effect=[ + _nmc.ServoStatus(status=_nmc.STATUS_MOVE_DONE, velocity=0), + _nmc.ServoStatus(status=_nmc.STATUS_MOVE_DONE, velocity=0), + _nmc.ServoStatus(status=0, velocity=0), + _nmc.ServoStatus(status=_nmc.STATUS_MOVE_DONE, velocity=-1), + ] + ) + self.vspin.request_spinning = AsyncMock( # type: ignore[method-assign] + side_effect=[False, True] + ) + + self.assertTrue(await self.vspin._request_rotor_safe_to_access()) + self.assertFalse(await self.vspin._request_rotor_safe_to_access()) + self.assertFalse(await self.vspin._request_rotor_safe_to_access()) + self.assertFalse(await self.vspin._request_rotor_safe_to_access()) + self.assertEqual(self.vspin.request_spinning.await_count, 2) # type: ignore[attr-defined] + + async def test_rotor_access_confirmation_has_a_bounded_wait(self): + self.vspin.request_positions_and_tachometer = AsyncMock( # type: ignore[method-assign] + return_value=_nmc.ServoStatus(status=_nmc.STATUS_MOVE_DONE, velocity=0) + ) + self.vspin.request_spinning = AsyncMock(return_value=True) # type: ignore[method-assign] + + with ( + patch("pylabrobot.agilent.vspin.vspin._IO_TRANSITION_TIMEOUT", 0), + self.assertRaisesRegex(TimeoutError, "did not become safe to access"), + ): + await self.vspin._wait_until_rotor_safe_to_access() + + async def test_rotor_access_actuators_wait_for_stop_confirmation(self): + async def assert_guarded(operation: Callable[[], Awaitable[None]]) -> None: + self.vspin._wait_until_rotor_safe_to_access = AsyncMock( # type: ignore[method-assign] + side_effect=TimeoutError("rotor not stopped") + ) + self.vspin._set_io_output_bit = AsyncMock() # type: ignore[method-assign] + + with self.assertRaisesRegex(TimeoutError, "rotor not stopped"): + await operation() + + self.vspin._wait_until_rotor_safe_to_access.assert_awaited_once() # type: ignore[attr-defined] + self.vspin._set_io_output_bit.assert_not_awaited() # type: ignore[attr-defined] + self.assertEqual(self.vspin.state.activity, VSpinActivity.IDLE) + self.assertFalse(self.vspin.state.recovery_required) + + self.vspin.request_door_open = AsyncMock(return_value=False) # type: ignore[method-assign] + await assert_guarded(self.vspin.open_door) + + self.vspin.request_door_locked = AsyncMock(return_value=True) # type: ignore[method-assign] + await assert_guarded(self.vspin.unlock_door) + + self.vspin.request_bucket_locked = AsyncMock(return_value=False) # type: ignore[method-assign] + await assert_guarded(self.vspin.lock_bucket) + + 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( # type: ignore[attr-defined] + [call(8400, transition=ANY), call(16400, transition=ANY)] + ) + 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) + self.quiet_time_patch = patch("pylabrobot.agilent.vspin.vspin._NETWORK_INPUT_QUIET_TIME", 0) + self.quiet_time_patch.start() + self.addCleanup(self.quiet_time_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 + _mark_vspin_ready(vspin) + 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), + _servo_step(servo_status, position=target_position), + _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), + _servo_step(servo_status, position=target_position), + _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), + _servo_step(servo_status, position=target_position), + _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) + self.assertEqual(vspin.state.connection, ConnectionState.CONNECTED) + self.assertEqual(vspin.state.initialization, VSpinInitializationState.INITIALIZED) + self.assertEqual(vspin.state.homing, VSpinHomingState.HOMED) + + async def test_setup_homing_timeout_turns_motor_off(self) -> None: + """A homing timeout must turn off the motor before propagating the failure.""" + steps = self._setup_steps() + home_command = _nmc.build_set_homing(_nmc.PIC_SERVO_ADDRESS, 0x28) + home_index = next(i for i, step in enumerate(steps) if step.command == home_command) + motor_off = _nmc.build_stop_motor(_nmc.PIC_SERVO_ADDRESS, _nmc.MOTOR_OFF) + vspin, io = self._make_vspin(steps[: home_index + 2] + [_servo_step(motor_off)]) + + with ( + patch("pylabrobot.agilent.vspin.vspin.asyncio.sleep", new=AsyncMock()), + patch("pylabrobot.agilent.vspin.vspin._NETWORK_PROBE_TIMEOUT", 0), + patch("pylabrobot.agilent.vspin.vspin._MOTION_TIMEOUT", 0), + self.assertRaisesRegex(TimeoutError, "homing did not finish"), + ): + await vspin.setup() + + io.assert_complete(self) + self.assertEqual(io.writes[-1], motor_off) + self.assertTrue(vspin.state.recovery_required) + self.assertEqual(vspin.state.homing, VSpinHomingState.HOMING) + self.assertIsNone(vspin.at_bucket) + self.assertFalse(vspin._command_lock.locked()) + + async def test_setup_motor_off_failure_preserves_homing_timeout(self) -> None: + """A failed motor-off acknowledgement must not hide the original homing error.""" + steps = self._setup_steps() + home_command = _nmc.build_set_homing(_nmc.PIC_SERVO_ADDRESS, 0x28) + home_index = next(i for i, step in enumerate(steps) if step.command == home_command) + motor_off = _nmc.build_stop_motor(_nmc.PIC_SERVO_ADDRESS, _nmc.MOTOR_OFF) + invalid_reply = _nmc_response(0, _servo_status_data())[:-1] + b"\x01" + vspin, io = self._make_vspin( + steps[: home_index + 2] + [_VSpinScriptStep(motor_off, invalid_reply)] + ) + + with ( + patch("pylabrobot.agilent.vspin.vspin.asyncio.sleep", new=AsyncMock()), + patch("pylabrobot.agilent.vspin.vspin._NETWORK_PROBE_TIMEOUT", 0), + patch("pylabrobot.agilent.vspin.vspin._MOTION_TIMEOUT", 0), + self.assertLogs(vspin_module.logger, level="ERROR") as logs, + self.assertRaisesRegex(TimeoutError, "homing did not finish"), + ): + await vspin.setup() + + io.assert_complete(self) + self.assertIn("failed to turn off motor after setup error", logs.output[0]) + self.assertTrue(vspin.state.recovery_required) + self.assertFalse(vspin._command_lock.locked()) + + async def test_cancelled_setup_homing_turns_motor_off(self) -> None: + """Task cancellation during homing must wait for motor-off cleanup.""" + steps = self._setup_steps() + home_command = _nmc.build_set_homing(_nmc.PIC_SERVO_ADDRESS, 0x28) + home_index = next(i for i, step in enumerate(steps) if step.command == home_command) + motor_off = _nmc.build_stop_motor(_nmc.PIC_SERVO_ADDRESS, _nmc.MOTOR_OFF) + vspin, io = self._make_vspin(steps[: home_index + 2] + [_servo_step(motor_off)]) + homing_started = asyncio.Event() + allow_status = asyncio.Event() + request_status = vspin.request_positions_and_tachometer + + async def wait_for_homing_status() -> _nmc.ServoStatus: + """Pause setup with homing active and the status reply fully consumed.""" + status = await request_status() + homing_started.set() + await allow_status.wait() + return status + + with ( + patch("pylabrobot.agilent.vspin.vspin.asyncio.sleep", new=AsyncMock()), + patch("pylabrobot.agilent.vspin.vspin._NETWORK_PROBE_TIMEOUT", 0), + patch.object(vspin, "request_positions_and_tachometer", side_effect=wait_for_homing_status), + ): + setup_task = asyncio.create_task(vspin.setup()) + await asyncio.wait_for(homing_started.wait(), timeout=1) + setup_task.cancel() + with self.assertRaises(asyncio.CancelledError): + await setup_task + + io.assert_complete(self) + self.assertEqual(io.writes[-1], motor_off) + self.assertTrue(vspin.state.recovery_required) + self.assertEqual(vspin.state.homing, VSpinHomingState.HOMING) + self.assertIsNone(vspin.at_bucket) + self.assertFalse(vspin._command_lock.locked()) + + 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, + ), + _io_step(io_status, inputs=safe_inputs), + _servo_step( + servo_status, + position=deceleration_position, + velocity=0, + ), + _io_step(io_status, inputs=safe_inputs), + _servo_step( + servo_status, + position=deceleration_position, + velocity=0, + ), + _servo_step( + servo_status, + position=deceleration_position, + velocity=0, + ), + _io_step(io_status, inputs=safe_inputs), + ] + 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_stop_spin_does_not_send_commands_in_parallel_with_spin_owner(self): + vspin, io = self._make_vspin([]) + vspin._set_activity(VSpinActivity.AT_SPEED) + vspin._spin_completion_event.clear() + + stop_task = asyncio.create_task(vspin.stop_spin(deceleration=0.5)) + while not vspin._spin_cancel_requested: + await asyncio.sleep(0) + + self.assertFalse(stop_task.done()) + self.assertEqual(vspin._spin_stop_deceleration, 0.5) + io.assert_complete(self) + + vspin._set_activity(VSpinActivity.IDLE) + vspin._spin_completion_event.set() + await stop_task + + io.assert_complete(self) + + 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.assertEqual(vspin.state.activity, VSpinActivity.IDLE) + + 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.assertTrue(vspin.state.recovery_required) + self.assertIsNone(vspin.at_bucket) + + 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) @@ -108,8 +1766,13 @@ def setUp(self): async def asyncSetUp(self): self.vspin = VSpin(name="centrifuge", device_id="test") - self.vspin._door_open = True + _mark_vspin_ready(self.vspin) 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_rotor_safe_to_access = AsyncMock( # type: ignore[method-assign] + return_value=True + ) 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] @@ -140,6 +1803,127 @@ async def test_load_emits_loader_to_bucket_transfer(self): self.assertEqual(started.data["source"]["name"], "loader") self.assertEqual(started.data["destination"]["name"], "centrifuge_bucket1") self.assertIs(self.vspin.bucket1.resource, plate) + self.loader.driver.load.assert_awaited_once_with( # type: ignore[attr-defined] + protocol.TEACHPOINT_BUCKET_1, + plate_height=10, + source_z_offset=3, + destination_z_offset=3, + park_z_offset=3, + gripper_open_position=0, + gripper_closed_position=5.68, + gripper_close_threshold=1.5, + source_speed="slow", + destination_speed="slow", + park_speed="slow", + gripper_open_speed="fast", + gripper_close_speed="slow", + gripper_release_speed="slow", + ) + + async def test_load_maps_presented_bucket_2_to_its_teachpoint(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._at_bucket = self.vspin.bucket2 + + await self.loader.load() + + self.loader.driver.load.assert_awaited_once_with( # type: ignore[attr-defined] + protocol.TEACHPOINT_BUCKET_2, + plate_height=10, + source_z_offset=3, + destination_z_offset=3, + park_z_offset=3, + gripper_open_position=0, + gripper_closed_position=5.68, + gripper_close_threshold=1.5, + source_speed="slow", + destination_speed="slow", + park_speed="slow", + gripper_open_speed="fast", + gripper_close_speed="slow", + gripper_release_speed="slow", + ) + self.assertIs(self.vspin.bucket2.resource, plate) + + async def test_transfer_parameters_reach_driver_and_events(self): + plate = Resource("plate_1", size_x=127, size_y=85, size_z=22) + self.loader.assign_child_resource(plate, location=Coordinate.zero()) + parameters = { + "plate_height": 22.0, + "source_z_offset": 4.0, + "destination_z_offset": 2.0, + "park_z_offset": 1.0, + "gripper_open_position": 0.25, + "gripper_closed_position": 4.75, + "gripper_close_threshold": 1.8, + "source_speed": "medium", + "destination_speed": "fast", + "park_speed": "medium", + "gripper_open_speed": "slow", + "gripper_close_speed": "medium", + "gripper_release_speed": "fast", + } + events: list[PLREvent] = [] + event_bus = EventBus() + event_bus.subscribe(events.append) + + with use_event_bus(event_bus): + await self.loader.load(**parameters) + self.assertIs(self.vspin.bucket1.resource, plate) + self.assertIsNone(self.loader.resource) + await self.loader.unload(**parameters) + + self.loader.driver.load.assert_awaited_once_with( # type: ignore[attr-defined] + protocol.TEACHPOINT_BUCKET_1, **parameters + ) + self.loader.driver.unload.assert_awaited_once_with( # type: ignore[attr-defined] + protocol.TEACHPOINT_BUCKET_1, **parameters + ) + self.assertIs(self.loader.resource, plate) + self.assertIsNone(self.vspin.bucket1.resource) + started = [ + event + for event in events + if event.name in ("centrifuge_loader.load.started", "centrifuge_loader.unload.started") + ] + self.assertEqual(len(started), 2) + for event in started: + self.assertEqual(event.data["parameters"], parameters) + + async def test_invalid_transfer_settings_release_vspin_without_recovery(self): + plate = Resource("plate_1", size_x=127, size_y=85, size_z=22) + # Use the real transfer entry points; FTDI remains mocked by the fixture. + self.loader = Access2(name="loader", device_id="test", vspin=self.vspin) + self.loader.assign_child_resource(plate, location=Coordinate.zero()) + before = self.vspin.state + loader_before = self.loader.driver.state + + with self.assertRaisesRegex(ValueError, "open position < close threshold"): + await self.loader.load(gripper_open_position=2, gripper_close_threshold=1) + + self.assertEqual(self.vspin.state, before) + self.assertEqual(self.loader.driver.state, loader_before) + self.assertIs(self.loader.resource, plate) + self.assertIsNone(self.vspin.bucket1.resource) + + async def test_actuated_loader_failure_blocks_vspin_motion(self): + plate = Resource("plate_1", size_x=1, size_y=1, size_z=1) + self.loader.assign_child_resource(plate, location=Coordinate.zero()) + + async def fail_after_actuation(bucket_teachpoint: int, **parameters: float | str) -> None: + self.assertEqual(bucket_teachpoint, protocol.TEACHPOINT_BUCKET_1) + self.loader.driver._mark_recovery_required(position_uncertain=False) + raise RuntimeError("loader motion failed") + + self.loader.driver.load = AsyncMock(side_effect=fail_after_actuation) # type: ignore[method-assign] + + with self.assertRaisesRegex(RuntimeError, "loader motion failed"): + await self.loader.load() + + self.assertTrue(self.vspin.state.recovery_required) + self.assertEqual(self.vspin.state.activity, VSpinActivity.TRANSFERRING) + self.assertIs(self.loader.resource, plate) + self.assertIsNone(self.vspin.bucket1.resource) async def test_unload_failure_emits_bucket_to_loader_transfer(self): plate = Resource("plate_1", size_x=1, size_y=1, size_z=1) @@ -170,3 +1954,39 @@ 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_rotor_safe_to_access = AsyncMock( # type: ignore[method-assign] + return_value=False + ) + + 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] diff --git a/pylabrobot/io/ftdi.py b/pylabrobot/io/ftdi.py index b07470c302d..17c121464ae 100644 --- a/pylabrobot/io/ftdi.py +++ b/pylabrobot/io/ftdi.py @@ -47,6 +47,13 @@ def _open_device(self) -> int: logger = logging.getLogger(__name__) +def is_ftdi_transport_error(error: BaseException) -> bool: + """Return whether an exception was raised by the optional pylibftdi transport.""" + if not HAS_PYLIBFTDI: + return False + return isinstance(error, FtdiError) + + def _parse_usb_address(address: str) -> Tuple[int, Tuple[int, ...]]: """Parse a USB topology path '-[....]' into its bus and ports.""" bus_str, sep, port_str = address.partition("-")