Add stage backlash correction

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

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

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.
+            # The state delta is should be 1 for a move equal to backlash steps.
-            # moving as this will move the state from -1 (fully one direction) to
+            # moving as this will move the state from 0 (fully disengaged) to
             # 1, fully the other.
-            delta = (
+            delta = (self.position[axis] - pos_before[axis]) / self.backlash_steps[axis]
-                2 * (self.position[axis] - pos_before[axis]) / self.backlash_steps[axis]
+            # apply value and clamp to within range from 0 to 1
-            )
-            # apply value and clamp to within range from -1 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(
+        if backlash_compensation is not None:
-            block_cancellation=block_cancellation,
+            self._move_with_backlash_correction(
-            **self._apply_axis_direction(kwargs),
+                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(
+        if backlash_compensation is not None:
-            block_cancellation=block_cancellation,
+            self._move_with_backlash_correction(
-            **self._apply_axis_direction(kwargs),
+                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(

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(

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):

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