rebase to stage: add test, fix names, use NotImplementedError

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

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

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

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

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

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)

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