Split the logic for deciding how to move in recentre from the moves and the main loop
This commit is contained in:
Julian Stirling
parent
7f0e7c52f9
commit
dcf4b0b383
2 changed files with 109 additions and 17 deletions
|
|
@ -331,7 +331,11 @@ class RangeofMotionThing(lt.Thing):
|
|||
rom_deps.stage.move_absolute(**starting_position, block_cancellation=True)
|
||||
|
||||
def _recentre_axis(self, axis: Literal["x", "y"], rom_deps: RomDeps) -> None:
|
||||
"""Recentre a single axis."""
|
||||
"""Recentre a single axis.
|
||||
|
||||
:param axis: The axis to recentre
|
||||
:param rom_deps: All dependencies that were passed to the calling Action.
|
||||
"""
|
||||
rom_deps.logger.info(f"Finding centre in {axis}.")
|
||||
# A new tracker for this axis.
|
||||
self._rom_data = RomDataTracker()
|
||||
|
|
@ -356,22 +360,9 @@ class RangeofMotionThing(lt.Thing):
|
|||
# Try to estimate position/direction the first time, skip to making a big
|
||||
# move in the same direction
|
||||
if i > 1:
|
||||
estimate = self._rom_data.find_turning_point(axis=axis)
|
||||
img_perc = self._distance_in_img_percentage(estimate, axis, rom_deps)
|
||||
|
||||
# if the distance is less than 1 big step away then move to it and exit
|
||||
if abs(img_perc) < BIG_STEP:
|
||||
rom_deps.logger.info(
|
||||
f"Estimated centre of {axis}-axis is {estimate[axis]}"
|
||||
)
|
||||
rom_deps.stage.move_absolute(**estimate)
|
||||
centred, direction = self._recentre_decision(axis, rom_deps)
|
||||
if centred:
|
||||
break
|
||||
rom_deps.logger.info(
|
||||
f"Estimated centre {abs(img_perc):.0f}% of a field of view away, "
|
||||
"that is too far to move in one move."
|
||||
)
|
||||
# Else calculate the desired direction in image coordinates.
|
||||
direction = self._img_dir_from_stage_coords(estimate, axis, rom_deps)
|
||||
|
||||
# If still going at iteration 9 exit
|
||||
if i > 9:
|
||||
|
|
@ -380,6 +371,41 @@ class RangeofMotionThing(lt.Thing):
|
|||
# Make a big z-corrected move towards estimate of centre.
|
||||
self._big_z_corrected_movement(axis, direction, rom_deps)
|
||||
|
||||
def _recentre_decision(
|
||||
self, axis: Literal["x", "y"], rom_deps: RomDeps
|
||||
) -> tuple[bool, Literal[1, -1]]:
|
||||
"""Decide what to do next during recentreing.
|
||||
|
||||
The algorithm here:
|
||||
|
||||
* Estimate turning point location
|
||||
* Check if distance to point is further than a "BIG_STEP"
|
||||
* If smaller, move to estimated centre, and return that it is centred
|
||||
* If larger, return that it is not centred, and the direction to move in.
|
||||
|
||||
:param axis: The axis to recentre
|
||||
:param rom_deps: All dependencies that were passed to the calling Action.
|
||||
:return: A tuple. The first value is True for "the stage is now centred" and
|
||||
False for "the stage is not centred". The second value is the direction to
|
||||
move in, this only has meaning if the first value is False.
|
||||
"""
|
||||
estimate = self._rom_data.find_turning_point(axis=axis)
|
||||
img_perc = self._distance_in_img_percentage(estimate, axis, rom_deps)
|
||||
|
||||
# if the distance is less than 1 big step away then move to it and exit
|
||||
if abs(img_perc) < BIG_STEP:
|
||||
rom_deps.logger.info(f"Estimated centre of {axis}-axis is {estimate[axis]}")
|
||||
rom_deps.stage.move_absolute(**estimate)
|
||||
# Note the second return, the direction, is meaningless here.
|
||||
return True, 1
|
||||
rom_deps.logger.info(
|
||||
f"Estimated centre {abs(img_perc):.0f}% of a field of view away, "
|
||||
"that is too far to move in one move."
|
||||
)
|
||||
# Else calculate the desired direction in image coordinates.
|
||||
direction = self._img_dir_from_stage_coords(estimate, axis, rom_deps)
|
||||
return False, direction
|
||||
|
||||
def _img_percentage_to_img_coords(
|
||||
self, fov_perc: int, axis: Literal["x", "y"]
|
||||
) -> float:
|
||||
|
|
|
|||
|
|
@ -194,10 +194,11 @@ def mock_rom_deps(csm_matrix, mocker) -> stage_measure.RomDeps:
|
|||
@pytest.mark.parametrize(
|
||||
("pos", "target_pos", "axis", "expected_dir"),
|
||||
[
|
||||
# X is flipped due to CSM sign
|
||||
({"x": 5000, "y": 0, "z": 0}, {"x": 0, "y": 0, "z": 0}, "x", 1),
|
||||
({"x": -5000, "y": 0, "z": 0}, {"x": 0, "y": 0, "z": 0}, "x", -1),
|
||||
({"x": 0, "y": 0, "z": 0}, {"x": 5000, "y": 0, "z": 0}, "x", -1),
|
||||
# Y is flipped due to CSM sign
|
||||
# Y is not flipped due to CSM sign
|
||||
({"x": 0, "y": 5000, "z": 0}, {"x": 0, "y": 0, "z": 0}, "y", -1),
|
||||
],
|
||||
)
|
||||
|
|
@ -693,3 +694,68 @@ def test_perform_recenter(rom_thing, mock_rom_deps, mocker):
|
|||
|
||||
# check that the position set to zero once
|
||||
assert mock_deps_dict["stage"].set_zero_position.call_count == 1
|
||||
|
||||
|
||||
@pytest.mark.parametrize("true_on", [1, 4, 9, 10])
|
||||
def test_recentre_axis(true_on, rom_thing, mock_rom_deps, mocker):
|
||||
"""Test the high level algorithm of recentring an axis.
|
||||
|
||||
This doesn't include deciding if we are centred or choosing the direction to move.
|
||||
"""
|
||||
## Start such that movement starts negative
|
||||
mock_rom_deps.stage.position = {"x": -10000, "y": 10000, "z": 0}
|
||||
mock_moves = mocker.patch.object(rom_thing, "_moves_for_z_prediction")
|
||||
|
||||
# Mock _recentre_decision so it returns (False, 1) a number of times then finally
|
||||
# (True, 1).
|
||||
decisions = [(False, 1)] * (true_on - 1) + [(True, 1)]
|
||||
mock_recentre_decision = mocker.patch.object(
|
||||
rom_thing, "_recentre_decision", side_effect=decisions
|
||||
)
|
||||
|
||||
if true_on > 9:
|
||||
with pytest.raises(RuntimeError, match="Couldn't find centre"):
|
||||
rom_thing._recentre_axis("x", mock_rom_deps)
|
||||
else:
|
||||
rom_thing._recentre_axis("x", mock_rom_deps)
|
||||
|
||||
# _recentre_decision is called until True or exits after 9 goes
|
||||
assert mock_recentre_decision.call_count == min(true_on, 9)
|
||||
|
||||
# The _moves_for_z_prediction is called twice before any decision, and not called
|
||||
# After a decision of centred, but the max call count is 10
|
||||
assert mock_moves.call_count == min(true_on + 1, 10)
|
||||
for i, arg_list in enumerate(mock_moves.call_args_list):
|
||||
# First two directions are -ve due to starting pos, then it changes direction
|
||||
# as the mock always returns 1
|
||||
expected_dir = -1 if i <= 1 else 1
|
||||
assert arg_list.kwargs["direction"] == expected_dir
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
("dist", "direction", "expected_decision"),
|
||||
[
|
||||
(500, 1, (False, 1)), # Big step is 200% this is over, movement is positive.
|
||||
(-500, -1, (False, -1)),
|
||||
(200, 1, (False, 1)),
|
||||
(199, 1, (True, 1)), # Less than 200% FOV movement return centred=True
|
||||
(-199, -1, (True, 1)), # Always return 1 when c
|
||||
],
|
||||
)
|
||||
def test_recentre_decision(
|
||||
dist, direction, expected_decision, rom_thing, mock_rom_deps, mocker
|
||||
):
|
||||
"""What the algorithm decides to do in different situations."""
|
||||
# Mock find turning point just because there is no _rom_data
|
||||
mocker.patch.object(rom_thing._rom_data, "find_turning_point")
|
||||
|
||||
# As _distance_in_img_percentage and _img_dir_from_stage_coords are tested
|
||||
# this test mocks their values and checks the return is as expected
|
||||
mocker.patch.object(rom_thing, "_distance_in_img_percentage", return_value=dist)
|
||||
mocker.patch.object(rom_thing, "_img_dir_from_stage_coords", return_value=direction)
|
||||
|
||||
assert rom_thing._recentre_decision("x", mock_rom_deps) == expected_decision
|
||||
|
||||
# Check that we make an absolute move (to the centre) before returning centred.
|
||||
centred = expected_decision[0]
|
||||
mock_rom_deps.stage.move_absolute.call_count == 1 if centred else 0
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue