microscope: fix backlash compensation to minimize moves

Author: ianohara

software/control/microscope.py

@@ -382,15 +382,23 @@ def get_z(self):
         return self.stage.get_pos().z_mm
 
     def move_z_to(self, z_mm, blocking=True):
-        self.stage.move_z_to(z_mm, blocking=blocking)
-        clear_backlash = z_mm >= self.stage.get_pos().z_mm
-        # clear backlash if moving backward in open loop mode
-        if blocking and clear_backlash:
+        # We want the z axis to rest on the "down" (wrt gravity) direction of gravity. So if we
+        # are moving in the positive (up) z direction, we need to move past our mark a bit then
+        # back.  If we are already moving in the "down" 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:
             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)
+
+            # Move past our final position, so we can move down to the final position and
+            # rest on the downside of the drive mechanism.
+            clamped_z_backlash_pos = min(z_mm + distance_to_clear_backlash, self.stage.get_config().Z_AXIS.MAX_POSITION)
+            self.stage.move_z_to(clamped_z_backlash_pos, blocking=blocking)
+
+        self.stage.move_z_to(z_mm)
 
     def start_live(self):
         self.camera.start_streaming()