firmware: check and send position updates every 5ms, not 10ms

firmware/octopi_firmware_v2/main_controller_teensy41/main_controller_teensy41.ino

@@ -303,10 +303,10 @@ static const int TIMER_PERIOD = 500; // in us
 volatile int counter_send_pos_update = 0;
 volatile bool flag_send_pos_update = false;
 
-static const int interval_send_pos_update = 10000; // in us
+static const int interval_send_pos_update = 5000; // in us
 elapsedMicros us_since_last_pos_update;
 
-static const int interval_check_position = 10000; // in us
+static const int interval_check_position = 5000; // in us
 elapsedMicros us_since_last_check_position;
 
 static const int interval_send_joystick_update = 30000; // in us

software/control/core/core.py

@@ -856,14 +856,6 @@ def move_to_slide_scanning_position(self):
 
         # restore z
         if self.slidePositionController.objective_retracted:
-            # NOTE(imo): We want to move backlash compensation down to the firmware level.  Also, before the Stage
-            # migration, we only compensated for backlash in the case that we were using PID control.  Since that
-            # info isn't plumbed through yet (or ever from now on?), we just always compensate now.  It doesn't hurt
-            # in the case of not needing it, except that it's a little slower because we need 2 moves.
-            mm_to_clear_backlash = self.stage.get_config().Z_AXIS.convert_to_real_units(
-                max(160, 20 * self.stage.get_config().Z_AXIS.MICROSTEPS_PER_STEP)
-            )
-            self.stage.move_z_to(self.slidePositionController.z_pos - mm_to_clear_backlash)
             self.stage.move_z_to(self.slidePositionController.z_pos)
             self.slidePositionController.objective_retracted = False
             print("z position restored")
@@ -981,14 +973,7 @@ def run_autofocus(self):
 
         z_af_offset = self.deltaZ * round(self.N / 2)
 
-        # maneuver for achiving uniform step size and repeatability when using open-loop control
-        # can be moved to the firmware
-        mm_to_clear_backlash = self.stage.get_config().Z_AXIS.convert_to_real_units(
-            max(160, 20 * self.stage.get_config().Z_AXIS.MICROSTEPS_PER_STEP)
-        )
-
-        self.stage.move_z(-mm_to_clear_backlash - z_af_offset)
-        self.stage.move_z(mm_to_clear_backlash)
+        self.stage.move_z(-z_af_offset)
 
         steps_moved = 0
         for i in range(self.N):
@@ -1034,14 +1019,10 @@ def run_autofocus(self):
         QApplication.processEvents()
 
         # maneuver for achiving uniform step size and repeatability when using open-loop control
-        # TODO(imo): The backlash handling should be done at a lower level.  For now, do backlash compensation no matter if it makes sense to do or not (it is not harmful if it doesn't make sense)
-        mm_to_clear_backlash = self.stage.get_config().Z_AXIS.convert_to_real_units(
-            max(160, 20 * self.stage.get_config().Z_AXIS.MICROSTEPS_PER_STEP)
-        )
-        self.stage.move_z(-mm_to_clear_backlash - steps_moved * self.deltaZ)
+        self.stage.move_z(-steps_moved * self.deltaZ)
         # determine the in-focus position
         idx_in_focus = focus_measure_vs_z.index(max(focus_measure_vs_z))
-        self.stage.move_z(mm_to_clear_backlash + (idx_in_focus + 1) * self.deltaZ)
+        self.stage.move_z((idx_in_focus + 1) * self.deltaZ)
 
         QApplication.processEvents()
 
@@ -1544,17 +1525,6 @@ def move_to_coordinate(self, coordinate_mm):
     def move_to_z_level(self, z_mm):
         print("moving z")
         self.stage.move_z_to(z_mm)
-        # TODO(imo): If we are moving to a more +z position, we'll approach the position from the negative side.  But then our backlash elimination goes negative and positive.  This seems like the final move is in the same direction as the original full move?  Does that actually eliminate backlash?
-        if z_mm >= self.stage.get_pos().z_mm:
-            # Attempt to remove backlash.
-            # TODO(imo): We used to only do this if in PID control mode, but we don't expose the PID mode settings
-            # yet, so for now just do this for all.
-            # TODO(imo): Ideally this would be done at a lower level, and only if needed.  As is we only remove backlash in this specific case (and no other Z moves!)
-            distance_to_clear_backlash = self.stage.get_config().Z_AXIS.convert_to_real_units(
-                max(160, 20 * self.stage.get_config().Z_AXIS.MICROSTEPS_PER_STEP)
-            )
-            self.stage.move_z(-distance_to_clear_backlash)
-            self.stage.move_z(distance_to_clear_backlash)
         time.sleep(SCAN_STABILIZATION_TIME_MS_Z / 1000)
 
     def run_coordinate_acquisition(self, current_path):
@@ -1709,14 +1679,7 @@ def perform_autofocus(self, region_id, fov):
         else:
             self._log.info("laser reflection af")
             try:
-                if HAS_OBJECTIVE_PIEZO:  # when piezo is available, one move is sufficient as piezo is closed loop
-                    self.microscope.laserAutofocusController.move_to_target(0)
-                else:
-                    # TODO(imo): We used to have a case here to try to fix backlash by double commanding a position.  Now, just double command it whether or not we are using PID since we don't expose that now.  But in the future, backlash handing shouldb e done at a lower level (and we can remove the double here)
-                    self.microscope.laserAutofocusController.move_to_target(0)
-                    self.microscope.laserAutofocusController.move_to_target(
-                        0
-                    )  # for stepper in open loop mode, repeat the operation to counter backlash.  It's harmless if any other case.
+                self.microscope.laserAutofocusController.move_to_target(0)
             except Exception as e:
                 file_ID = f"{region_id}_focus_camera.bmp"
                 saving_path = os.path.join(self.base_path, self.experiment_ID, str(self.time_point), file_ID)
@@ -1733,13 +1696,6 @@ def prepare_z_stack(self):
         if self.z_stacking_config == "FROM CENTER":
             self.stage.move_z(-self.deltaZ * round((self.NZ - 1) / 2.0))
             time.sleep(SCAN_STABILIZATION_TIME_MS_Z / 1000)
-        # TODO(imo): This is some sort of backlash compensation.  We should move this down to the low level, and remove it from here.
-        # maneuver for achiving uniform step size and repeatability when using open-loop control
-        distance_to_clear_backlash = self.stage.get_config().Z_AXIS.convert_to_real_units(
-            max(160, 20 * self.stage.get_config().Z_AXIS.MICROSTEPS_PER_STEP)
-        )
-        self.stage.move_z(-distance_to_clear_backlash)
-        self.stage.move_z(distance_to_clear_backlash)
         time.sleep(SCAN_STABILIZATION_TIME_MS_Z / 1000)
 
     def handle_z_offset(self, config, not_offset):
@@ -2014,17 +1970,12 @@ def move_z_back_after_stack(self):
             if MULTIPOINT_PIEZO_UPDATE_DISPLAY:
                 self.signal_z_piezo_um.emit(self.z_piezo_um)
         else:
-            distance_to_clear_backlash = self.stage.get_config().Z_AXIS.convert_to_real_units(
-                max(160, 20 * self.stage.get_config().Z_AXIS.MICROSTEPS_PER_STEP)
-            )
             if self.z_stacking_config == "FROM CENTER":
                 rel_z_to_start = -self.deltaZ * (self.NZ - 1) + self.deltaZ * round((self.NZ - 1) / 2)
             else:
                 rel_z_to_start = -self.deltaZ * (self.NZ - 1)
 
-            # TODO(imo): backlash should be handled at a lower level.  For now, we do it here no matter what control scheme is being used below.
-            self.stage.move_z(rel_z_to_start - distance_to_clear_backlash)
-            self.stage.move_z(distance_to_clear_backlash)
+            self.stage.move_z(rel_z_to_start)
 
 
 class MultiPointController(QObject):
@@ -4814,13 +4765,9 @@ def _calibrate_pixel_to_um(self) -> bool:
             return False
 
         # Move to first position and measure
+        self._move_z(-self.laser_af_properties.pixel_to_um_calibration_distance / 2)
         if self.piezo is not None:
-            self._move_z(-self.laser_af_properties.pixel_to_um_calibration_distance / 2)
             time.sleep(MULTIPOINT_PIEZO_DELAY_MS / 1000)
-        else:
-            # TODO: change to _move_z after backlash correction is absorbed into firmware
-            self.stage.move_z(-1.5 * self.laser_af_properties.pixel_to_um_calibration_distance / 1000)
-            self.stage.move_z(self.laser_af_properties.pixel_to_um_calibration_distance / 1000)
 
         result = self._get_laser_spot_centroid()
         if result is None:
@@ -4859,13 +4806,9 @@ def _calibrate_pixel_to_um(self) -> bool:
             )
 
         # move back to initial position
+        self._move_z(-self.laser_af_properties.pixel_to_um_calibration_distance / 2)
         if self.piezo is not None:
-            self._move_z(-self.laser_af_properties.pixel_to_um_calibration_distance / 2)
             time.sleep(MULTIPOINT_PIEZO_DELAY_MS / 1000)
-        else:
-            # TODO: change to _move_z after backlash correction is absorbed into firmware
-            self.stage.move_z(-1.5 * self.laser_af_properties.pixel_to_um_calibration_distance / 1000)
-            self.stage.move_z(self.laser_af_properties.pixel_to_um_calibration_distance / 1000)
 
         # Calculate conversion factor
         if x1 - x0 == 0:

software/control/microscope.py

@@ -385,24 +385,6 @@ def get_z(self):
         return self.stage.get_pos().z_mm
 
     def move_z_to(self, z_mm, blocking=True):
-        # From Hongquan, we want the z axis to rest on the "up" (wrt gravity) direction of gravity. So if we
-        # are moving in the negative (down) z direction, we need to move past our mark a bit then
-        # back up.  If we are already moving in the "up" position, we can move straight there.
-        need_clear_backlash = z_mm < self.stage.get_pos().z_mm
-
-        # NOTE(imo): It seems really tricky to only clear backlash if via the blocking call?
-        if blocking and need_clear_backlash:
-            backlash_offset = -abs(
-                self.stage.get_config().Z_AXIS.convert_to_real_units(
-                    max(160, 20 * self.stage.get_config().Z_AXIS.MICROSTEPS_PER_STEP)
-                )
-            )
-            # Move past our final position, so we can move up to the final position and
-            # rest on the downside of the drive mechanism.  But make sure we don't drive past the min position
-            # to do this.
-            clamped_z_backlash_pos = max(z_mm + backlash_offset, self.stage.get_config().Z_AXIS.MIN_POSITION)
-            self.stage.move_z_to(clamped_z_backlash_pos, blocking=blocking)
-
         self.stage.move_z_to(z_mm)
 
     def start_live(self):

software/squid/stage/cephla.py

@@ -8,6 +8,8 @@
 
 
 class CephlaStage(AbstractStage):
+    _BACKLASH_COMPENSATION_DISTANCE_MM = 0.005
+
     @staticmethod
     def _calc_move_timeout(distance, max_speed):
         # We arbitrarily guess that if a move takes 3x the naive "infinite acceleration" time, then it
@@ -62,10 +64,33 @@ def move_y(self, rel_mm: float, blocking: bool = True):
             )
 
     def move_z(self, rel_mm: float, blocking: bool = True):
-        self._microcontroller.move_z_usteps(self._config.Z_AXIS.convert_real_units_to_ustep(rel_mm))
+        # From Hongquan, we want the z axis to rest on the "up" (wrt gravity) direction of gravity. So if we
+        # are moving in the negative (down) z direction, we need to move past our mark a bit then
+        # back up.  If we are already moving in the "up" position, we can move straight there.
+        need_clear_backlash = rel_mm < 0
+
+        # NOTE(imo): It seems really tricky to only clear backlash if via the blocking call?
+        final_rel_move_mm = rel_mm
+        if blocking and need_clear_backlash:
+            backlash_offset = -CephlaStage._BACKLASH_COMPENSATION_DISTANCE_MM
+            final_rel_move_mm = -backlash_offset
+            # Move past our final position, so we can move up to the final position and
+            # rest on the downside of the drive mechanism.  But make sure we don't drive past the min position
+            # to do this.
+            rel_move_with_backlash_offset_mm = rel_mm + backlash_offset
+            rel_move_with_backlash_offset_usteps = self._config.Z_AXIS.convert_real_units_to_ustep(
+                rel_move_with_backlash_offset_mm
+            )
+            self._microcontroller.move_z_usteps(rel_move_with_backlash_offset_usteps)
+            if blocking:
+                self._microcontroller.wait_till_operation_is_completed(
+                    self._calc_move_timeout(rel_move_with_backlash_offset_mm, self.get_config().Z_AXIS.MAX_SPEED)
+                )
+
+        self._microcontroller.move_z_usteps(self._config.Z_AXIS.convert_real_units_to_ustep(final_rel_move_mm))
         if blocking:
             self._microcontroller.wait_till_operation_is_completed(
-                self._calc_move_timeout(rel_mm, self.get_config().Z_AXIS.MAX_SPEED)
+                self._calc_move_timeout(final_rel_move_mm, self.get_config().Z_AXIS.MAX_SPEED)
             )
 
     def move_x_to(self, abs_mm: float, blocking: bool = True):
@@ -83,6 +108,27 @@ def move_y_to(self, abs_mm: float, blocking: bool = True):
             )
 
     def move_z_to(self, abs_mm: float, blocking: bool = True):
+        # From Hongquan, we want the z axis to rest on the "up" (wrt gravity) direction of gravity. So if we
+        # are moving in the negative (down) z direction, we need to move past our mark a bit then
+        # back up.  If we are already moving in the "up" position, we can move straight there.
+        need_clear_backlash = abs_mm < self.get_pos().z_mm
+
+        # NOTE(imo): It seems really tricky to only clear backlash if via the blocking call?
+        if blocking and need_clear_backlash:
+            backlash_offset = -CephlaStage._BACKLASH_COMPENSATION_DISTANCE_MM
+            # Move past our final position, so we can move up to the final position and
+            # rest on the downside of the drive mechanism.  But make sure we don't drive past the min position
+            # to do this.
+            clamped_z_backlash_pos = max(abs_mm + backlash_offset, self.get_config().Z_AXIS.MIN_POSITION)
+            clamped_z_backlash_pos_usteps = self._config.Z_AXIS.convert_real_units_to_ustep(clamped_z_backlash_pos)
+            self._microcontroller.move_z_to_usteps(clamped_z_backlash_pos_usteps)
+            if blocking:
+                self._microcontroller.wait_till_operation_is_completed(
+                    self._calc_move_timeout(
+                        clamped_z_backlash_pos - self.get_pos().z_mm, self.get_config().Z_AXIS.MAX_SPEED
+                    )
+                )
+
         self._microcontroller.move_z_to_usteps(self._config.Z_AXIS.convert_real_units_to_ustep(abs_mm))
         if blocking:
             self._microcontroller.wait_till_operation_is_completed(

software/tools/stage_timing.py

@@ -1,20 +1,21 @@
 import logging
 
 from control.microscope import Microscope
+import squid.abc
 import squid.logging
 import time
 
 log = squid.logging.get_logger("stage timing")
 
 
-def get_move_fn(scope: Microscope, axis: str):
+def get_move_fn(scope: Microscope, stage: squid.abc.AbstractStage, axis: str, relative):
     match axis.lower():
         case "z":
-            return scope.move_z_to
+            return stage.move_z if relative else scope.move_z_to
         case "y":
-            return scope.move_y_to
+            return stage.move_y if relative else scope.move_y_to
         case "x":
-            return scope.move_x_to
+            return stage.move_x if relative else scope.move_x_to
         case _:
             raise ValueError(f"Unknown axis {axis}")
 
@@ -35,7 +36,7 @@ def main(args):
     if args.home:
         home(scope)
 
-    axis_move_fn = get_move_fn(scope, args.axis)
+    axis_move_fn = get_move_fn(scope, scope.stage, args.axis, args.relative)
     axis_move_fn(args.start)
 
     t0 = time.time()
@@ -52,7 +53,8 @@ def report(moves_so_far):
     start_pos = args.start
     for i in range(total_moves):
         this_move_num = i + 1
-        axis_move_fn(start_pos + step * this_move_num)
+        move_pos = step if args.relative else start_pos + step * this_move_num
+        axis_move_fn(move_pos)
         if this_move_num % args.report_interval == 0:
             report(this_move_num)
     report(total_moves)
@@ -73,5 +75,6 @@ def report(moves_so_far):
         "--step", type=float, default=0.001, help="The step size to use in mm.  This should be small!  EG 0.001"
     )
     ap.add_argument("--no_home", dest="home", action="store_false", help="Do not home zxy before running.")
+    ap.add_argument("--relative", action="store_true", help="Use relative moves instead of absolute.")
 
     sys.exit(main(ap.parse_args()))