(#499) Add helper functions for moving smargon during pin tip centring

src/artemis/experiment_plans/pin_tip_centring_plan.py

@@ -1,9 +1,14 @@
 from typing import Generator, Tuple
 
 import bluesky.plan_stubs as bps
+import bluesky.preprocessors as bpp
+import numpy as np
 from bluesky.utils import Msg
+from dodal.devices.oav.oav_calculations import camera_coordinates_to_xyz
 from dodal.devices.oav.oav_detector import OAV
+from dodal.devices.oav.oav_parameters import OAVParameters
 from dodal.devices.smargon import Smargon
+from ophyd.utils.errors import LimitError
 
 from artemis.exceptions import WarningException
 from artemis.log import LOGGER
@@ -54,3 +59,54 @@ def move_pin_into_view(
         )
     else:
         return (tip_x_px, tip_y_px)
+
+
+def move_smargon_warn_on_out_of_range(
+    smargon: Smargon, position: Tuple[float, float, float]
+):
+    def warn_if_limit_error(exception: Exception):
+        if isinstance(exception, LimitError):
+            raise WarningException(
+                "Pin tip centring failed - pin too long/short/bent and out of range"
+            )
+        yield bps.null()
+
+    yield from bpp.contingency_wrapper(
+        bps.mv(
+            smargon.x,
+            position[0],
+            smargon.y,
+            position[1],
+            smargon.z,
+            position[2],
+        ),
+        except_plan=warn_if_limit_error,
+    )
+
+
+def move_so_that_beam_is_at_pixel(
+    smargon: Smargon, pixel: Tuple[int, int], oav_params: OAVParameters
+):
+    """Move so that the given pixel is in the centre of the beam."""
+    beam_distance_px: Tuple[int, int] = oav_params.calculate_beam_distance(*pixel)
+
+    current_motor_xyz = np.array(
+        [
+            (yield from bps.rd(smargon.x)),
+            (yield from bps.rd(smargon.y)),
+            (yield from bps.rd(smargon.z)),
+        ],
+        dtype=np.float64,
+    )
+    current_angle = yield from bps.rd(smargon.omega)
+
+    position_mm = current_motor_xyz + camera_coordinates_to_xyz(
+        beam_distance_px[0],
+        beam_distance_px[1],
+        current_angle,
+        oav_params.micronsPerXPixel,
+        oav_params.micronsPerYPixel,
+    )
+
+    LOGGER.info(f"Tip centring moving to : {position_mm}")
+    yield from move_smargon_warn_on_out_of_range(smargon, position_mm)

src/artemis/experiment_plans/tests/test_pin_tip_centring.py

@@ -3,10 +3,16 @@
 import pytest
 from bluesky.run_engine import RunEngine
 from dodal.devices.oav.oav_detector import OAV
+from dodal.devices.oav.oav_parameters import OAVParameters
 from dodal.devices.smargon import Smargon
+from ophyd.sim import make_fake_device
 
 from artemis.exceptions import WarningException
-from artemis.experiment_plans.pin_tip_centring_plan import move_pin_into_view
+from artemis.experiment_plans.pin_tip_centring_plan import (
+    move_pin_into_view,
+    move_smargon_warn_on_out_of_range,
+    move_so_that_beam_is_at_pixel,
+)
 
 
 def test_given_the_pin_tip_is_already_in_view_when_get_tip_into_view_then_tip_returned_and_smargon_not_moved(
@@ -82,3 +88,53 @@ def test_given_no_tip_found_ever_when_get_tip_into_view_then_smargon_moved_posit
         RE(move_pin_into_view(oav, smargon))
 
     assert smargon.x.user_readback.get() == 1
+
+
+def test_given_moving_out_of_range_when_move_with_warn_called_then_warning_exception(
+    RE: RunEngine,
+):
+    fake_smargon = make_fake_device(Smargon)(name="")
+    fake_smargon.x.user_setpoint.sim_set_limits([0, 10])
+
+    with pytest.raises(WarningException):
+        RE(move_smargon_warn_on_out_of_range(fake_smargon, (100, 0, 0)))
+
+
+@pytest.mark.parametrize(
+    "px_per_um, beam_centre, angle, pixel_to_move_to, expected_xyz",
+    [
+        # Simple case of beam being in the top left and each pixel being 1 mm
+        ([1000, 1000], [0, 0], 0, [100, 190], [100, 190, 0]),
+        ([1000, 1000], [0, 0], -90, [50, 250], [50, 0, 250]),
+        ([1000, 1000], [0, 0], 90, [-60, 450], [-60, 0, -450]),
+        # Beam offset
+        ([1000, 1000], [100, 100], 0, [100, 100], [0, 0, 0]),
+        ([1000, 1000], [100, 100], -90, [50, 250], [-50, 0, 150]),
+        # Pixels_per_micron different
+        ([10, 50], [0, 0], 0, [100, 190], [1, 9.5, 0]),
+        ([60, 80], [0, 0], -90, [50, 250], [3, 0, 20]),
+    ],
+)
+def test_values_for_move_so_that_beam_is_at_pixel(
+    smargon: Smargon,
+    test_config_files,
+    RE,
+    px_per_um,
+    beam_centre,
+    angle,
+    pixel_to_move_to,
+    expected_xyz,
+):
+    params = OAVParameters(context="loopCentring", **test_config_files)
+    params.micronsPerXPixel = px_per_um[0]
+    params.micronsPerYPixel = px_per_um[1]
+    params.beam_centre_i = beam_centre[0]
+    params.beam_centre_j = beam_centre[1]
+
+    smargon.omega.user_readback.sim_put(angle)
+
+    RE(move_so_that_beam_is_at_pixel(smargon, pixel_to_move_to, params))
+
+    assert smargon.x.user_readback.get() == pytest.approx(expected_xyz[0])
+    assert smargon.y.user_readback.get() == pytest.approx(expected_xyz[1])
+    assert smargon.z.user_readback.get() == pytest.approx(expected_xyz[2])