From c2b784e606fe362a3b88f27a87add936eb2513e1 Mon Sep 17 00:00:00 2001 From: Julian Stirling Date: Tue, 24 Feb 2026 16:47:52 +0000 Subject: [PATCH] Add stage backlash correction --- .../things/camera_stage_mapping.py | 9 +- .../things/smart_scan.py | 4 +- .../things/stage/__init__.py | 148 +++++++++++++++--- .../things/stage_measure.py | 8 +- tests/unit_tests/test_stage.py | 104 ++++++++++-- tests/unit_tests/test_stage_measure.py | 4 +- 6 files changed, 244 insertions(+), 33 deletions(-) diff --git a/src/openflexure_microscope_server/things/camera_stage_mapping.py b/src/openflexure_microscope_server/things/camera_stage_mapping.py index 7860f938..813c762f 100644 --- a/src/openflexure_microscope_server/things/camera_stage_mapping.py +++ b/src/openflexure_microscope_server/things/camera_stage_mapping.py @@ -141,11 +141,15 @@ class CameraStageMapper(lt.Thing): except lt.exceptions.InvocationCancelledError as e: self.logger.info("User cancelled the camera stage mapping calibration") self.logger.info("Returning to starting position") - self._stage.move_absolute(**starting_position, block_cancellation=True) + self._stage.move_absolute( + **starting_position, block_cancellation=True, backlash_compensation=None + ) raise e except MappingError as e: self.logger.info("Returning to starting position due to failed calibration") - self._stage.move_absolute(**starting_position, block_cancellation=True) + self._stage.move_absolute( + **starting_position, block_cancellation=True, backlash_compensation=None + ) raise e result["move_history"] = recorded_move.history result["image_resolution"] = self._cam.capture_downsampled_array().shape[:2] @@ -264,6 +268,7 @@ class CameraStageMapper(lt.Thing): self._stage.move_relative( **self.convert_image_to_stage_coordinates(x=x, y=y), block_cancellation=False, + backlash_compensation=None, ) @lt.action diff --git a/src/openflexure_microscope_server/things/smart_scan.py b/src/openflexure_microscope_server/things/smart_scan.py index 84143e25..56cf7e0b 100644 --- a/src/openflexure_microscope_server/things/smart_scan.py +++ b/src/openflexure_microscope_server/things/smart_scan.py @@ -481,7 +481,9 @@ class SmartScanThing(lt.Thing): if self._scan_data is not None: self._stage.move_absolute( - **self.scan_data.starting_position, block_cancellation=True + **self.scan_data.starting_position, + block_cancellation=True, + backlash_compensation=None, ) @property diff --git a/src/openflexure_microscope_server/things/stage/__init__.py b/src/openflexure_microscope_server/things/stage/__init__.py index 4a41bd42..e949531d 100644 --- a/src/openflexure_microscope_server/things/stage/__init__.py +++ b/src/openflexure_microscope_server/things/stage/__init__.py @@ -11,6 +11,7 @@ As the object will be used as a context manager create the hardware connection i from __future__ import annotations +import enum import queue import threading from collections.abc import Mapping, Sequence @@ -32,6 +33,21 @@ class RedefinedBaseMovementError(RuntimeError): """ +class BacklashCompensation(enum.Enum): + """The axes to apply backlash compensation to. + + * MOVEMENT_AXES - Only the axes that are moving. + * ALL_AXES - All axes + * XY_ONLY - Only the x and y axes. + * Z_ONLY - The z-axis only. + """ + + MOVEMENT_AXES = enum.auto() + ALL_AXES = enum.auto() + XY_ONLY = enum.auto() + Z_ONLY = enum.auto() + + class JogCommand: """A base class for jog operations.""" @@ -102,7 +118,7 @@ class BaseStage(lt.Thing): self._jog_thread: Optional[threading.Thread] = None self._hardware_position: Mapping[str, int] = dict.fromkeys(self._axis_names, 0) # Backlash state is a dict as we mutate it during moves. - self._backlash_state: dict[str, float] = dict.fromkeys(self._axis_names, -1) + self._backlash_state: dict[str, float] = dict.fromkeys(self._axis_names, 0) # This must be the last thing the function does in case it is caught in a try. if ( @@ -149,15 +165,13 @@ class BaseStage(lt.Thing): pos_before = dict(self.position) self._hardware_update_position() for axis in self._axis_names: - # The state delta is should be 2 for a move equal to backlash steps. - # moving as this will move the state from -1 (fully one direction) to + # The state delta is should be 1 for a move equal to backlash steps. + # moving as this will move the state from 0 (fully disengaged) to # 1, fully the other. - delta = ( - 2 * (self.position[axis] - pos_before[axis]) / self.backlash_steps[axis] - ) - # apply value and clamp to within range from -1 to 1 + delta = (self.position[axis] - pos_before[axis]) / self.backlash_steps[axis] + # apply value and clamp to within range from 0 to 1 self._backlash_state[axis] = max( - -1, min(1, self._backlash_state[axis] + delta) + 0, min(1, self._backlash_state[axis] + delta) ) def _hardware_update_position(self) -> None: @@ -223,12 +237,25 @@ class BaseStage(lt.Thing): self.axis_inverted = direction @lt.action - def move_relative(self, block_cancellation: bool = False, **kwargs: int) -> None: + def move_relative( + self, + block_cancellation: bool = False, + backlash_compensation: Optional[BacklashCompensation] = None, + **kwargs: int, + ) -> None: """Make a relative move. Keyword arguments should be axis names.""" - self._hardware_move_relative( - block_cancellation=block_cancellation, - **self._apply_axis_direction(kwargs), - ) + if backlash_compensation is not None: + self._move_with_backlash_correction( + block_cancellation=block_cancellation, + relative=True, + backlash_compensation=backlash_compensation, + **kwargs, + ) + else: + self._hardware_move_relative( + block_cancellation=block_cancellation, + **self._apply_axis_direction(kwargs), + ) def _hardware_move_relative( self, block_cancellation: bool = False, **kwargs: int @@ -242,12 +269,25 @@ class BaseStage(lt.Thing): ) @lt.action - def move_absolute(self, block_cancellation: bool = False, **kwargs: int) -> None: + def move_absolute( + self, + block_cancellation: bool = False, + backlash_compensation: Optional[BacklashCompensation] = None, + **kwargs: int, + ) -> None: """Make an absolute move. Keyword arguments should be axis names.""" - self._hardware_move_absolute( - block_cancellation=block_cancellation, - **self._apply_axis_direction(kwargs), - ) + if backlash_compensation is not None: + self._move_with_backlash_correction( + block_cancellation=block_cancellation, + relative=False, + backlash_compensation=backlash_compensation, + **kwargs, + ) + else: + self._hardware_move_absolute( + block_cancellation=block_cancellation, + **self._apply_axis_direction(kwargs), + ) def _hardware_move_absolute( self, @@ -262,6 +302,78 @@ class BaseStage(lt.Thing): "StageThings must define their own _hardware_move_absolute method" ) + def _move_with_backlash_correction( + self, + block_cancellation: bool, + relative: bool, + backlash_compensation: BacklashCompensation, + **kwargs: int, + ) -> None: + """Make a movement with backlash correction. + + :param block_cancellation: True to prevent the move being cancelled. + :param relative: True if the kwargs are relative moves. False if they are + absolute. + :param backlash_compensation: A BacklashCompensation which sets which axes to + apply backalsh compensation to. + :param kwargs: A mapping of axis name to integer for the movement. + """ + # Calculate relative movement + if relative: + move = {ax: kwargs.get(ax, 0) for ax in self._axis_names} + else: + move = {ax: kwargs.get(ax, pos) - pos for ax, pos in self.position.items()} + + # Depending on the backlash method decide which axes to check + check_axes: tuple[str, ...] + match backlash_compensation: + case BacklashCompensation.MOVEMENT_AXES: + check_axes = tuple(ax for ax, move in move.items() if move != 0) + case BacklashCompensation.ALL_AXES: + check_axes = self._axis_names + case BacklashCompensation.XY_ONLY: + check_axes = ("x", "y") + case BacklashCompensation.Z_ONLY: + check_axes = ("z",) + case _: + raise ValueError( + f"Unknown backlash compensation method {self._axis_names}" + ) + + # Check the backlash state at the end of the move, no need to clip to range of + # 0-1 + final_state = { + ax: self._backlash_state[ax] + move[ax] / self.backlash_steps[ax] + for ax in check_axes + } + # If the state is 1 or greater the motors are engaged in the preferred + # direction, if not a correction move is needed. + correction = { + ax: self.backlash_steps[ax] + for ax, state in final_state.items() + if state < 1 + } + + if correction: + # If there is a correction to apply move in two goes + first_move = { + ax: move[ax] - correction.get(ax, 0) for ax in self._axis_names + } + self._hardware_move_relative( + block_cancellation=block_cancellation, + **self._apply_axis_direction(first_move), + ) + self._hardware_move_relative( + block_cancellation=block_cancellation, + **self._apply_axis_direction(correction), + ) + else: + # Else just complete the relative move + self._hardware_move_relative( + block_cancellation=block_cancellation, + **self._apply_axis_direction(move), + ) + def _hardware_start_move_relative(self, displacement: Sequence[int]) -> None: """Start a relative move.""" raise NotImplementedError( diff --git a/src/openflexure_microscope_server/things/stage_measure.py b/src/openflexure_microscope_server/things/stage_measure.py index 9162d6e9..411e57ed 100644 --- a/src/openflexure_microscope_server/things/stage_measure.py +++ b/src/openflexure_microscope_server/things/stage_measure.py @@ -267,7 +267,9 @@ class RangeofMotionThing(lt.Thing): self._rom_data.stage_coords[-1] = self._stage.position finally: - self._stage.move_absolute(**starting_position, block_cancellation=True) + self._stage.move_absolute( + **starting_position, block_cancellation=True, backlash_compensation=None + ) def _recentre_axis(self, axis: Literal["x", "y"]) -> None: """Recentre a single axis. @@ -327,7 +329,9 @@ class RangeofMotionThing(lt.Thing): # if the distance is less than 1 big step away then move to it and exit if abs(img_perc) < BIG_STEP: self.logger.info(f"Estimated centre of {axis}-axis is {estimate[axis]}") - self._stage.move_absolute(**estimate, block_cancellation=False) + self._stage.move_absolute( + **estimate, block_cancellation=False, backlash_compensation=None + ) # Note the second return, the direction, is meaningless here. return True, 1 self.logger.info( diff --git a/tests/unit_tests/test_stage.py b/tests/unit_tests/test_stage.py index e2d04448..b448ea84 100644 --- a/tests/unit_tests/test_stage.py +++ b/tests/unit_tests/test_stage.py @@ -4,6 +4,7 @@ import itertools import logging import threading import time +from dataclasses import dataclass import pytest from hypothesis import HealthCheck, given, settings @@ -14,6 +15,7 @@ from labthings_fastapi.testing import create_thing_without_server from openflexure_microscope_server.things.camera.simulation import SimulatedCamera from openflexure_microscope_server.things.stage import ( + BacklashCompensation, BaseStage, JogCommand, JogQueue, @@ -313,33 +315,117 @@ def test_backlash_state(dummy_stage): """Test that the backlash state updates as the stage moves.""" # Use with to start the movement thread. with dummy_stage: - assert dummy_stage._backlash_state == {"x": -1, "y": -1, "z": -1} + assert dummy_stage._backlash_state == {"x": 0, "y": 0, "z": 0} assert dummy_stage.backlash_steps == {"x": 200, "y": 200, "z": 200} # Move 100 steps to centre of x backlash range dummy_stage.move_relative(x=100) - assert dummy_stage._backlash_state == {"x": 0, "y": -1, "z": -1} + assert dummy_stage._backlash_state == {"x": 0.5, "y": 0, "z": 0} # Move 50 more steps to halfway between centre and "engaged" dummy_stage.move_relative(x=50) - assert dummy_stage._backlash_state == {"x": 0.5, "y": -1, "z": -1} + assert dummy_stage._backlash_state == {"x": 0.75, "y": 0, "z": 0} # Move back 50 steps to the centre dummy_stage.move_relative(x=-50) - assert dummy_stage._backlash_state == {"x": 0, "y": -1, "z": -1} + assert dummy_stage._backlash_state == {"x": 0.5, "y": 0, "z": 0} # Move 500 steps, now fully engaged dummy_stage.move_relative(x=500) - assert dummy_stage._backlash_state == {"x": 1, "y": -1, "z": -1} + assert dummy_stage._backlash_state == {"x": 1, "y": 0, "z": 0} # Move back 100 steps in the centre of backash range again. dummy_stage.move_relative(x=-100) - assert dummy_stage._backlash_state == {"x": 0, "y": -1, "z": -1} + assert dummy_stage._backlash_state == {"x": 0.5, "y": 0, "z": 0} # Move forward a load to re-engage dummy_stage.move_relative(x=500) - assert dummy_stage._backlash_state == {"x": 1, "y": -1, "z": -1} + assert dummy_stage._backlash_state == {"x": 1, "y": 0, "z": 0} # Move back 200 to be fully at the far side of the backlash range dummy_stage.move_relative(x=-200) - assert dummy_stage._backlash_state == {"x": -1, "y": -1, "z": -1} + assert dummy_stage._backlash_state == {"x": 0, "y": 0, "z": 0} # Further motion changes nothing in state dummy_stage.move_relative(x=-200) - assert dummy_stage._backlash_state == {"x": -1, "y": -1, "z": -1} + assert dummy_stage._backlash_state == {"x": 0, "y": 0, "z": 0} + + +@dataclass() +class BacklashMoveTestCase: + """Structured information for each test of backlash compensation.""" + + position: dict[str, float] + backlash_state: dict[str, float] + backlash_steps: dict[str, int] + movement: dict[str, int] + relative: bool + backlash_compensation: BacklashCompensation + result: list[dict[str, int]] + + @property + def rel_move(self): + """The relative movement.""" + if self.relative: + return self.movement + + return {ax: self.movement[ax] - self.position[ax] for ax in self.position} + + def validate(self): + """Check that the total movement in the result is the relative movement requested.""" + total = {ax: sum(move[ax] for move in self.result) for ax in self.position} + expected = self.rel_move + assert total == expected + + +BACKLASH_TEST_MOVEMENTS = [ + # Simple test case moving 1 step in x against backlash direction + BacklashMoveTestCase( + position={"x": 0, "y": 0, "z": 0}, + backlash_state={"x": 0, "y": 0, "z": 0}, + backlash_steps={"x": 200, "y": 200, "z": 200}, + movement={"x": -1, "y": 0, "z": 0}, + relative=True, + backlash_compensation=BacklashCompensation.MOVEMENT_AXES, + # There are 2 moves as it is against the direction + result=[{"x": -201, "y": 0, "z": 0}, {"x": 200, "y": 0, "z": 0}], + ), +] + + +@pytest.mark.parametrize("test_case", BACKLASH_TEST_MOVEMENTS) +def test_backlash_compensated_moves(test_case, dummy_stage, mocker): + """Test that backlash correction is applied correctly. + + See each test case for an explanation. + """ + test_case.validate() + mock_hw_move_rel = mocker.patch.object(dummy_stage, "_hardware_move_relative") + # Set position + dummy_stage._hardware_position = dummy_stage._apply_axis_direction( + test_case.position + ) + # double check position is set + assert dummy_stage.position == test_case.position + + dummy_stage._backlash_state = test_case.backlash_state + dummy_stage.backlash_steps = test_case.backlash_steps + + if test_case.relative: + dummy_stage.move_relative( + **test_case.movement, + block_cancellation=False, + backlash_compensation=test_case.backlash_compensation, + ) + else: + dummy_stage.move_absolute( + **test_case.movement, + block_cancellation=False, + backlash_compensation=test_case.backlash_compensation, + ) + assert mock_hw_move_rel.call_count == len(test_case.result) + for res, call in zip( + test_case.result, mock_hw_move_rel.call_args_list, strict=True + ): + applied = call.kwargs + # Remove the block_cancellation argument and add in any axes that are 0 + applied.pop("block_cancellation") + applied = {ax: applied.get(ax, 0) for ax in res} + # Check the expected result was applied for each move. + assert applied == dummy_stage._apply_axis_direction(res) def test_thing_description_equivalence(dummy_stage, mocker): diff --git a/tests/unit_tests/test_stage_measure.py b/tests/unit_tests/test_stage_measure.py index 5c9e0690..cbb91ed2 100644 --- a/tests/unit_tests/test_stage_measure.py +++ b/tests/unit_tests/test_stage_measure.py @@ -490,7 +490,9 @@ def test_move_until_edge_error(rom_thing, mocker): # However the "finally" should have executed returning to the starting position assert rom_thing._stage.move_absolute.call_count == 1 abs_move_kwargs = rom_thing._stage.move_absolute.call_args.kwargs - expected_abs_move_kwargs = dict(**mock_position_dict, block_cancellation=True) + expected_abs_move_kwargs = dict( + **mock_position_dict, block_cancellation=True, backlash_compensation=None + ) assert abs_move_kwargs == expected_abs_move_kwargs