From 359951c231c92eeb4b8cd3d219cd1dfa7ffe796b Mon Sep 17 00:00:00 2001 From: Ian OHara Date: Fri, 27 Dec 2024 10:49:07 -0800 Subject: [PATCH] rebase to stage: add test, fix names, use NotImplementedError --- software/squid/abc.py | 5 ++-- software/squid/config.py | 38 ++++++++++++++++-------- software/squid/stage/cephla.py | 23 ++++++++++++--- software/squid/stage/prior.py | 2 +- software/squid/stage/utils.py | 46 +++++++++++++++++++++++++++++ software/tests/squid/test_config.py | 35 ++++++++++++++++++++++ software/tests/squid/test_stage.py | 34 +++++++++++++++++++++ 7 files changed, 164 insertions(+), 19 deletions(-) create mode 100644 software/squid/stage/utils.py create mode 100644 software/tests/squid/test_config.py create mode 100644 software/tests/squid/test_stage.py diff --git a/software/squid/abc.py b/software/squid/abc.py index cdc8c181..17494cdd 100644 --- a/software/squid/abc.py +++ b/software/squid/abc.py @@ -13,7 +13,8 @@ class Pos(pydantic.BaseModel): x_mm: float y_mm: float z_mm: float - theta_rad: float + # NOTE/TODO(imo): If essentially none of our stages have a theta, this is probably fine. But If it's a mix we probably want a better way of handling the "maybe has theta" case. + theta_rad: Optional[float] class StageStage(pydantic.BaseModel): busy: bool @@ -81,7 +82,7 @@ def set_limits(self, pass def get_config(self) -> StageConfig: - pass + return self._config def wait_for_idle(self, timeout_s): start_time = time.time() diff --git a/software/squid/config.py b/software/squid/config.py index 6d4e0340..95e1473a 100644 --- a/software/squid/config.py +++ b/software/squid/config.py @@ -1,5 +1,6 @@ import enum import math +from typing import Optional import pydantic @@ -9,6 +10,12 @@ class DirectionSign(enum.IntEnum): DIRECTION_SIGN_POSITIVE = 1 DIRECTION_SIGN_NEGATIVE = -1 +class PIDConfig(pydantic.BaseModel): + ENABLED: bool + P: float + I: float + D: float + class AxisConfig(pydantic.BaseModel): MOVEMENT_SIGN: DirectionSign USE_ENCODER: bool @@ -25,7 +32,7 @@ class AxisConfig(pydantic.BaseModel): # The number of microsteps per full step the axis uses (or should use if we can set it). # If MICROSTEPS_PER_STEP == 8, and SCREW_PITCH=2, then in 8 commanded steps the motor will do 1 full # step and so will travel a distance of 2. - MICROSTEPS_PER_STEP: float + MICROSTEPS_PER_STEP: int # The Max speed the axis is allowed to travel in denoted in its native units. This means mm/s for # linear axes, and radians/s for rotary axes. @@ -37,15 +44,18 @@ class AxisConfig(pydantic.BaseModel): MIN_POSITION: float MAX_POSITION: float + # Some axes have a PID controller. This says whether or not to use the PID control loop, and if so what + # gains to use. + PID: Optional[PIDConfig] + def convert_to_real_units(self, usteps: float): if self.USE_ENCODER: - # TODO(imo): Do we need ENCODER_SIGN here too? - return usteps * self.MOVEMENT_SIGN.value * self.ENCODER_STEP_SIZE + return usteps * self.MOVEMENT_SIGN.value * self.ENCODER_STEP_SIZE * self.ENCODER_SIGN.value else: - return usteps * self.MOVEMENT_SIGN.value * self.SCREW_PITCH / (self.MICROSTEPS_PER_STEP * self.FULLSTEPS_PER_REV) + return usteps * self.MOVEMENT_SIGN.value * self.SCREW_PITCH / (self.MICROSTEPS_PER_STEP * self.FULL_STEPS_PER_REV) def convert_real_units_to_ustep(self, real_unit: float): - return real_unit / (self.MOVEMENT_SIGN.value * self.SCREW_PITCH / (self.MICROSTEPS_PER_STEP * self.FULLSTEPS_PER_REV)) + return round(real_unit / (self.MOVEMENT_SIGN.value * self.SCREW_PITCH / (self.MICROSTEPS_PER_STEP * self.FULL_STEPS_PER_REV))) class StageConfig(pydantic.BaseModel): X_AXIS: AxisConfig @@ -66,8 +76,9 @@ class StageConfig(pydantic.BaseModel): MICROSTEPS_PER_STEP=_def.MICROSTEPPING_DEFAULT_X, MAX_SPEED=_def.MAX_VELOCITY_X_mm, MAX_ACCELERATION=_def.MAX_ACCELERATION_X_mm, - MIN_POSITION=0, # NOTE(imo): Min and Max need adjusting. They are arbitrary right now! - MAX_POSITION=10 + MIN_POSITION=_def.SOFTWARE_POS_LIMIT.X_NEGATIVE, + MAX_POSITION=_def.SOFTWARE_POS_LIMIT.X_POSITIVE, + PID=None ), Y_AXIS=AxisConfig( MOVEMENT_SIGN=_def.STAGE_MOVEMENT_SIGN_Y, @@ -79,8 +90,9 @@ class StageConfig(pydantic.BaseModel): MICROSTEPS_PER_STEP=_def.MICROSTEPPING_DEFAULT_Y, MAX_SPEED=_def.MAX_VELOCITY_Y_mm, MAX_ACCELERATION=_def.MAX_ACCELERATION_Y_mm, - MIN_POSITION=0, # NOTE(imo): Min and Max need adjusting. They are arbitrary right now! - MAX_POSITION=10 + MIN_POSITION=_def.SOFTWARE_POS_LIMIT.Y_NEGATIVE, + MAX_POSITION=_def.SOFTWARE_POS_LIMIT.Y_POSITIVE, + PID=None ), Z_AXIS=AxisConfig( MOVEMENT_SIGN=_def.STAGE_MOVEMENT_SIGN_Z, @@ -92,8 +104,9 @@ class StageConfig(pydantic.BaseModel): MICROSTEPS_PER_STEP=_def.MICROSTEPPING_DEFAULT_Z, MAX_SPEED=_def.MAX_VELOCITY_Z_mm, MAX_ACCELERATION=_def.MAX_ACCELERATION_Z_mm, - MIN_POSITION=0, # NOTE(imo): Min and Max need adjusting. They are arbitrary right now! - MAX_POSITION=1 + MIN_POSITION=_def.SOFTWARE_POS_LIMIT.Z_NEGATIVE, + MAX_POSITION=_def.SOFTWARE_POS_LIMIT.Z_POSITIVE, + PID=None ), THETA_AXIS=AxisConfig( MOVEMENT_SIGN=_def.STAGE_MOVEMENT_SIGN_THETA, @@ -106,7 +119,8 @@ class StageConfig(pydantic.BaseModel): MAX_SPEED=2.0 * math.pi / 4, # NOTE(imo): I arbitrarily guessed this at 4 sec / rev, so it probably needs adjustment. MAX_ACCELERATION=_def.MAX_ACCELERATION_X_mm, MIN_POSITION=0, # NOTE(imo): Min and Max need adjusting. They are arbitrary right now! - MAX_POSITION=2.0 * math.pi / 4 + MAX_POSITION=2.0 * math.pi / 4, + PID=None ) ) diff --git a/software/squid/stage/cephla.py b/software/squid/stage/cephla.py index 3501107a..3ba18a76 100644 --- a/software/squid/stage/cephla.py +++ b/software/squid/stage/cephla.py @@ -4,7 +4,7 @@ import control.microcontroller import control._def as _def from squid.abc import AbstractStage, Pos, StageStage -from squid.config import StageConfig +from squid.config import StageConfig, AxisConfig class CephlaStage(AbstractStage): @@ -20,6 +20,21 @@ def __init__(self, microcontroller: control.microcontroller.Microcontroller, sta super().__init__(stage_config) self._microcontroller = microcontroller + # TODO(imo): configure theta here? Do we ever have theta? + self._configure_axis(_def.AXIS.X, stage_config.X_AXIS) + self._configure_axis(_def.AXIS.Y, stage_config.Y_AXIS) + self._configure_axis(_def.AXIS.Z, stage_config.Z_AXIS) + + def _configure_axis(self, microcontroller_axis_number: int, axis_config: AxisConfig): + if axis_config.USE_ENCODER: + # TODO(imo): The original navigationController had a "flip_direction" on configure_encoder, but it was unused in the implementation? + self._microcontroller.configure_stage_pid( + axis=microcontroller_axis_number, + transitions_per_revolution = axis_config.SCREW_PITCH / axis_config.ENCODER_STEP_SIZE) + if axis_config.PID and axis_config.PID.ENABLED: + self._microcontroller.set_pid_arguments(microcontroller_axis_number, axis_config.PID.P, axis_config.PID.I, axis_config.PID.D) + self._microcontroller.turn_on_stage_pid(microcontroller_axis_number) + def move_x(self, rel_mm: float, blocking: bool = True): self._microcontroller.move_x_usteps(self._config.X_AXIS.convert_real_units_to_ustep(rel_mm)) if blocking: @@ -42,18 +57,18 @@ def move_x_to(self, abs_mm: float, blocking: bool = True): self._microcontroller.move_x_to_usteps(self._config.X_AXIS.convert_real_units_to_ustep(abs_mm)) if blocking: self._microcontroller.wait_till_operation_is_completed( - self._calc_move_timeout(abs_mm - self.get_pos().x, self.get_config().X_AXIS.MAX_SPEED)) + self._calc_move_timeout(abs_mm - self.get_pos().x_mm, self.get_config().X_AXIS.MAX_SPEED)) def move_y_to(self, abs_mm: float, blocking: bool = True): self._microcontroller.move_y_to_usteps(self._config.Y_AXIS.convert_real_units_to_ustep(abs_mm)) if blocking: self._microcontroller.wait_till_operation_is_completed( - self._calc_move_timeout(abs_mm - self.get_pos().y, self.get_config().Y_AXIS.MAX_SPEED)) + self._calc_move_timeout(abs_mm - self.get_pos().y_mm, self.get_config().Y_AXIS.MAX_SPEED)) def move_z_to(self, abs_mm: float, blocking: bool = True): if blocking: self._microcontroller.wait_till_operation_is_completed( - self._calc_move_timeout(abs_mm - self.get_pos().z, self.get_config().Z_AXIS.MAX_SPEED)) + self._calc_move_timeout(abs_mm - self.get_pos().z_mm, self.get_config().Z_AXIS.MAX_SPEED)) def get_pos(self) -> Pos: pos_usteps = self._microcontroller.get_pos() diff --git a/software/squid/stage/prior.py b/software/squid/stage/prior.py index 7556b940..5fa62e48 100644 --- a/software/squid/stage/prior.py +++ b/software/squid/stage/prior.py @@ -12,7 +12,7 @@ def __init__(self, stage_config: StageConfig): self._not_impl() def _not_impl(self): - raise NotImplemented("The Prior Stage is not yet implemented!") + raise NotImplementedError("The Prior Stage is not yet implemented!") def move_x(self, rel_mm: float, blocking: bool = True): self._not_impl() diff --git a/software/squid/stage/utils.py b/software/squid/stage/utils.py new file mode 100644 index 00000000..f6c72fa2 --- /dev/null +++ b/software/squid/stage/utils.py @@ -0,0 +1,46 @@ +from typing import Optional +import os + +import squid.logging +from squid.abc import Pos +from squid.config import StageConfig + +_log = squid.logging.get_logger(__package__) +_DEFAULT_CACHE_PATH = "cache/last_coords.txt" +""" +Attempts to load a cached stage position and return it. +""" +def get_cached_position(cache_path=_DEFAULT_CACHE_PATH) -> Optional[Pos]: + if not os.path.isfile(cache_path): + _log.debug(f"Cache file '{cache_path}' not found, no cached pos found.") + return None + with open(cache_path, "r") as f: + for line in f: + try: + x, y, z = line.strip("\n").strip().split(",") + x = float(x) + y = float(y) + z = float(z) + return Pos(x_mm=x, y_mm=y, z_mm=z, theta_rad=None) + except RuntimeError as e: + raise e + pass + return None + +""" +Write out the current x, y, z position, in mm, so we can use it later as a cached position. +""" +def cache_position(pos: Pos, stage_config: StageConfig, cache_path=_DEFAULT_CACHE_PATH): + x_min = stage_config.X_AXIS.MIN_POSITION + x_max = stage_config.X_AXIS.MAX_POSITION + y_min = stage_config.Y_AXIS.MIN_POSITION + y_max = stage_config.Y_AXIS.MAX_POSITION + z_min = stage_config.Z_AXIS.MIN_POSITION + z_max = stage_config.Z_AXIS.MAX_POSITION + if not (x_min <= pos.x_mm <= x_max and + y_min <= pos.y_mm <= y_max and + z_min <= pos.z_mm <= z_max): + raise ValueError(f"Position {pos} is not cacheable because it is outside of the min/max of at least one axis. x_range=({x_min}, {x_max}), y_range=({y_min}, {y_max}), z_range=({z_min}, {z_max})") + with open(cache_path, "w") as f: + _log.debug(f"Writing position={pos} to cache path='{cache_path}'") + f.write(",".join([str(pos.x_mm), str(pos.y_mm), str(pos.z_mm)])) diff --git a/software/tests/squid/test_config.py b/software/tests/squid/test_config.py new file mode 100644 index 00000000..1eb4052d --- /dev/null +++ b/software/tests/squid/test_config.py @@ -0,0 +1,35 @@ +import pytest + +import squid.config +from squid.config import AxisConfig + + +def test_axis_config(): + stage_config = squid.config.get_stage_config() + # micro step conversion round tripping + trials = (1.0, 0.001, 2.2, 3.123456) + + # Test with easy to reason about axis config, then with real ones + easy_config = stage_config.X_AXIS + easy_config.ENCODER_SIGN = 1 + easy_config.USE_ENCODER = False + # 400 steps -> 1 mm (2*200 = 1 rev, 1 rev == 1 mm) + easy_config.SCREW_PITCH = 1.0 + easy_config.MICROSTEPS_PER_STEP = 2 + easy_config.FULL_STEPS_PER_REV = 200 + + def round_trip_mm(config: AxisConfig, mm): + # Round tripping should match within 1 ustep + usteps = config.convert_real_units_to_ustep(mm) + mm_round_tripped = config.convert_to_real_units(usteps) + eps = abs(config.convert_to_real_units(1)) + assert mm_round_tripped == pytest.approx(mm, abs=eps) + + for trial in trials: + round_trip_mm(easy_config, trial) + + for trial in trials: + round_trip_mm(stage_config.X_AXIS, trial) + round_trip_mm(stage_config.Y_AXIS, trial) + round_trip_mm(stage_config.Z_AXIS, trial) + round_trip_mm(stage_config.THETA_AXIS, trial) diff --git a/software/tests/squid/test_stage.py b/software/tests/squid/test_stage.py new file mode 100644 index 00000000..86177537 --- /dev/null +++ b/software/tests/squid/test_stage.py @@ -0,0 +1,34 @@ +import pytest +import tempfile + +import squid.stage.cephla +import squid.stage.prior +import squid.stage.utils +import squid.config +from control.microcontroller import Microcontroller, SimSerial +import squid.abc + +def test_create_simulated_stages(): + microcontroller = Microcontroller(existing_serial=SimSerial()) + cephla_stage = squid.stage.cephla.CephlaStage(microcontroller, squid.config.get_stage_config()) + + with pytest.raises(NotImplementedError): + prior_stage = squid.stage.prior.PriorStage(squid.config.get_stage_config()) + +def test_simulated_cephla_stage_ops(): + microcontroller = Microcontroller(existing_serial=SimSerial()) + stage: squid.stage.cephla.CephlaStage = squid.stage.cephla.CephlaStage(microcontroller, squid.config.get_stage_config()) + + assert stage.get_pos() == squid.abc.Pos(x_mm=0.0, y_mm=0.0, z_mm=0.0, theta_rad=0.0) + + +def test_position_caching(): + (unused_temp_fd, temp_cache_path) = tempfile.mkstemp(".cache", "squid_testing_") + + # Use 6 figures after the decimal so we test that we can capture nanometers + p = squid.abc.Pos(x_mm=11.111111, y_mm=22.222222, z_mm=1.333333, theta_rad=None) + squid.stage.utils.cache_position(pos=p, stage_config=squid.config.get_stage_config(), cache_path=temp_cache_path) + + p_read = squid.stage.utils.get_cached_position(cache_path=temp_cache_path) + + assert p_read == p