diff --git a/src/openflexure_microscope_server/things/stage/sangaboard.py b/src/openflexure_microscope_server/things/stage/sangaboard.py index 7429aea3..43edae8b 100644 --- a/src/openflexure_microscope_server/things/stage/sangaboard.py +++ b/src/openflexure_microscope_server/things/stage/sangaboard.py @@ -3,8 +3,11 @@ from __future__ import annotations import threading +import time +from collections.abc import Sequence from contextlib import contextmanager from copy import copy +from queue import Queue from types import TracebackType from typing import Any, Iterator, Literal, Optional, Self @@ -19,6 +22,32 @@ REQUIRED_VERSION = semver.Version.parse("1.0.0") RECOMMENDED_VERSION = semver.Version.parse("1.0.4") +class JogCommand: + """A base class for jog operations.""" + + def __init__(self) -> None: + """Initialise a JogCommand.""" + super().__init__() + self.received = threading.Event() + self.finished = threading.Event() + + +class JogMoveCommand(JogCommand): + """A command to make a jog move.""" + + def __init__(self, displacement: Sequence[int]) -> None: + """Initialise a JogMoveCommand. + + :param displacement: The displacement for the jog move, in hardware coordinates. + """ + super().__init__() + self.displacement = displacement + + +class JogStopCommand(JogCommand): + """A command to stop a jog move.""" + + class SangaboardThing(BaseStage): """A Thing to manage a Sangaboard motor controller. @@ -49,6 +78,11 @@ class SangaboardThing(BaseStage): self.sangaboard_kwargs = copy(kwargs) self.sangaboard_kwargs["port"] = port self._sangaboard_lock = threading.RLock() + + self._jog_lock = threading.Lock() + self._jog_queue: Queue[JogCommand] = Queue[JogCommand](maxsize=1) + self._jog_thread: Optional[threading.Thread] = None + super().__init__(thing_server_interface, **kwargs) def __enter__(self) -> Self: @@ -118,6 +152,46 @@ class SangaboardThing(BaseStage): f"{RECOMMENDED_VERSION}." ) + def _hardware_start_move_relative(self, displacement: Sequence[int]) -> None: + """Start a relative move. + + This starts the stage moving, but does not wait for the move to complete. It + sets ``self.moving`` to `True`: resetting it is the responsibility of the calling code. + """ + with self.sangaboard() as sb: + self.moving = True + sb.move_rel(displacement) + + def _hardware_stop(self) -> None: + """Stop any motion of the stage as soon as possible.""" + with self.sangaboard() as sb: + sb.query("stop") + self.moving = False + + def _move_duration(self, displacement: Sequence[int]) -> float: + """Calculate the expected duration of a move with the given displacement.""" + max_displacement = max(abs(d) for d in displacement) + return max_displacement * 0.001 # This does not yet check the board's speed. + + def _poll_moving(self) -> bool: + """Determine if the stage is still moving. + + This also sets `self.moving` if the status has changed. + + :return: whether the stage is still moving. + """ + with self.sangaboard() as sb: + moving = sb.query("moving?") == "true" + if self.moving != moving: + self.moving = moving + return moving + + def _poll_until_stopped(self) -> None: + """Poll the stage until it reports that it has stopped moving.""" + with self.sangaboard(): + while self._poll_moving(): + lt.cancellable_sleep(0.1) + def _hardware_move_relative( self, block_cancellation: bool = False, @@ -125,20 +199,24 @@ class SangaboardThing(BaseStage): ) -> None: """Make a relative move in the coordinate system used by the sangaboard.""" displacement = [kwargs.get(axis, 0) for axis in self.axis_names] + duration = self._move_duration(displacement) with self.sangaboard() as sb: - self.moving = True try: sb.move_rel(displacement) if block_cancellation: sb.query("notify_on_stop") else: + if duration > 0.2: + lt.cancellable_sleep( + duration - 0.1 + ) # Avoid unnecessary polling while sb.query("moving?") == "true": lt.cancellable_sleep(0.1) except lt.exceptions.InvocationCancelledError as e: # If the move has been cancelled, stop it but don't handle the exception. # We need the exception to propagate in order to stop any calling tasks, # and to mark the invocation as "cancelled" rather than stopped. - sb.query("stop") + self._hardware_stop() raise e finally: self.moving = False @@ -198,3 +276,108 @@ class SangaboardThing(BaseStage): sb.query(f"{led_command} {on_brightness}") else: sb.query(f"{led_command} 0") + + @lt.action + def jog(self, stop: bool = False, **kwargs: int) -> None: + r"""Make a relative move that may be interrupted by a future ``jog``\ . + + This action makes a relative move. If another ``jog`` action is called while + a ``jog`` is already in progress, the first will be stopped and the second + will start immediately. This allows for responsive manual control of the + stage, for example with a joystick. + + :param stop: if this is set to `True` the jog will be terminated. + :param \**kwargs: Keyword arguments should be axis names. + """ + if stop: + self._send_jog_command(JogStopCommand(), wait=True, timeout=1) + else: + self._hardware_jog(**self._apply_axis_direction(kwargs)) + + def _hardware_jog(self, **kwargs: int) -> None: + r"""Make a relative move that may be interrupted by a future ``jog``\ . + + This function uses hardware coordinates, ``jog`` is the public wrapper that + applies any neecessary transform and then calls this function. See the + docs for that function for more explanation. + + See `_jog_loop` for an explanation of the mechanism. + + :param \**kwargs: Keyword arguments should be axis names. + """ + move = [kwargs.get(axis, 0) for axis in self.axis_names] + self._send_jog_command( + JogMoveCommand(move), + timeout=self._move_duration(move) + 1, + ) + + def _send_jog_command( + self, command: JogCommand, timeout: Optional[float] = None + ) -> None: + """Send a jog command to the background jog thread. + + This function will start the background thread if it is not running. + This function acquires `_jog_lock` and uses the `_jog_send` event to signal + the thread to read the next command. As commands interrupt each other, this + function should never block for a long time. + + :param command: the jog command to send. + :param timeout: how long to wait for the command to be completed, or `None` + to skip waiting. + """ + with self._jog_lock: + # Make sure the queue exists. + # Check the background thread is running, and restart it if not. + if self._jog_thread is None or not self._jog_thread.is_alive(): + self._jog_queue = Queue[JogCommand](maxsize=1) + self._jog_thread = threading.Thread(target=self._jog_loop) + self._jog_thread.start() + self._jog_queue.put(command) + command.received.wait(1) + if timeout is not None: + command.finished.wait(timeout) + + def _jog_loop(self) -> None: + r"""Execute jog commands in a background thread. + + This function is intended to be run in a background thread. It will look at + `self._jog_command` when the `self._jog_send` event is set, and signal that + the command is being processed with `self._jog_received`\ . + + If no new command is received before the current command finishes, the thread + will terminate, and `self._jog_received` will be set again. This means that + it should be safe to + """ + duration = 1 # How long to wait initially. This shouldn't ever need to be long. + # On subsequent loop iterations, we'll wait long enough that we expect the last + # move to have finished. + previous_command: Optional[JogCommand] = None + with ( + self.sangaboard() as _sb + ): # prevent others using the sangaboard while jogging. + while True: + try: + command = self._jog_queue.get(timeout=duration) + except TimeoutError: + if self._poll_moving(): + # If there's no new command, keep checking for new commands until + # we stop. + duration = 0.1 + continue + # Once the stage is no longer moving, stop listening for commands. + break + command.received.set() # Acknowledge the command + if previous_command: + previous_command.finished.set() # Notify the last command it's superseded + if isinstance(command, JogMoveCommand): + self._hardware_start_move_relative(command.displacement) + duration = self._move_duration(command.displacement) + elif isinstance(command, JogStopCommand): + self._hardware_stop() + self._poll_until_stopped() + duration = 0.1 # Next iteration, we will probably time out. + else: + raise RuntimeError(f"Unknown jog command: {command}") + if previous_command: + # Notify the last command that it finished, because we stopped moving. + previous_command.finished.set()