homing: Implement second home from homing.py

Move the logic for performing the second home from the kinematics
classes to the generic homing code.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>

1 files changed, 42 insertions, 29 deletions

diff --git a/klippy/homing.py b/klippy/homing.py
index b65c3124..c53a65d4 100644
--- a/klippy/homing.py
+++ b/klippy/homing.py
@@ -27,21 +27,23 @@ class Homing:
             if coord[i] is not None:
                 thcoord[i] = coord[i]
         return thcoord
-    def retract(self, newpos, speed):
-        self.toolhead.move(self._fill_coord(newpos), speed)
     def set_homed_position(self, pos):
         self.toolhead.set_position(self._fill_coord(pos))
     def _get_homing_speed(self, speed, endstops):
         # Round the requested homing speed so that it is an even
         # number of ticks per step.
-        speed = min(speed, self.toolhead.get_max_velocity()[0])
         mcu_stepper = endstops[0][0].get_steppers()[0]
         adjusted_freq = mcu_stepper.get_mcu().get_adjusted_freq()
         dist_ticks = adjusted_freq * mcu_stepper.get_step_dist()
         ticks_per_step = math.ceil(dist_ticks / speed)
         return dist_ticks / ticks_per_step
-    def _homing_move(self, movepos, endstops, speed,
-                     probe_pos=False, verify_movement=False):
+    def homing_move(self, movepos, endstops, speed, dwell_t=0.,
+                    probe_pos=False, verify_movement=False):
+        # Notify endstops of upcoming home
+        for mcu_endstop, name in endstops:
+            mcu_endstop.home_prepare()
+        if dwell_t:
+            self.toolhead.dwell(dwell_t, check_stall=False)
         # Start endstop checking
         print_time = self.toolhead.get_last_move_time()
         start_mcu_pos = [(s, name, s.get_mcu_position())
@@ -53,7 +55,6 @@ class Homing:
                 print_time, ENDSTOP_SAMPLE_TIME, ENDSTOP_SAMPLE_COUNT,
                 min_step_dist / speed)
         # Issue move
-        movepos = self._fill_coord(movepos)
         error = None
         try:
             self.toolhead.move(movepos, speed)
@@ -85,31 +86,43 @@ class Homing:
                         raise EndstopError("Probe triggered prior to movement")
                     raise EndstopError(
                         "Endstop %s still triggered after retract" % (name,))
-    def probing_move(self, movepos, mcu_probe, speed):
-        mcu_probe.home_prepare()
-        self._homing_move(movepos, [(mcu_probe, "probe")], speed,
-                          probe_pos=True, verify_movement=True)
-    def home(self, forcepos, movepos, endstops, speed, second_home=False):
-        if second_home and forcepos == movepos:
-            return
+    def home_rails(self, rails, forcepos, movepos, limit_speed=None):
         # Alter kinematics class to think printer is at forcepos
         homing_axes = [axis for axis in range(3) if forcepos[axis] is not None]
-        self.toolhead.set_position(
-            self._fill_coord(forcepos), homing_axes=homing_axes)
-        # Notify endstops of upcoming home
-        for mcu_endstop, name in endstops:
-            mcu_endstop.home_prepare()
-        # Add a CPU delay when homing a large axis
-        if not second_home:
-            est_move_d = sum([abs(forcepos[i]-movepos[i])
-                              for i in range(3) if movepos[i] is not None])
-            est_steps = sum([est_move_d / s.get_step_dist()
-                             for es, n in endstops for s in es.get_steppers()])
-            self.toolhead.dwell(est_steps * HOMING_STEP_DELAY, check_stall=False)
-            speed = self._get_homing_speed(speed, endstops)
-        # Issue homing move
-        self._homing_move(movepos, endstops, speed,
-                          verify_movement=second_home and self.verify_retract)
+        forcepos = self._fill_coord(forcepos)
+        movepos = self._fill_coord(movepos)
+        self.toolhead.set_position(forcepos, homing_axes=homing_axes)
+        # Determine homing speed
+        endstops = [es for rail in rails for es in rail.get_endstops()]
+        hi = rails[0].get_homing_info()
+        max_velocity = self.toolhead.get_max_velocity()[0]
+        if limit_speed is not None and limit_speed < max_velocity:
+            max_velocity = limit_speed
+        homing_speed = min(hi.speed, max_velocity)
+        homing_speed = self._get_homing_speed(homing_speed, endstops)
+        second_homing_speed = min(hi.second_homing_speed, max_velocity)
+        # Calculate a CPU delay when homing a large axis
+        axes_d = [mp - fp for mp, fp in zip(movepos, forcepos)]
+        est_move_d = abs(axes_d[0]) + abs(axes_d[1]) + abs(axes_d[2])
+        est_steps = sum([est_move_d / s.get_step_dist()
+                         for es, n in endstops for s in es.get_steppers()])
+        dwell_t = est_steps * HOMING_STEP_DELAY
+        # Perform first home
+        self.homing_move(movepos, endstops, homing_speed, dwell_t=dwell_t)
+        # Perform second home
+        if hi.retract_dist:
+            # Retract
+            move_d = math.sqrt(sum([d*d for d in axes_d[:3]]))
+            retract_r = min(1., hi.retract_dist / move_d)
+            retractpos = [mp - ad * retract_r
+                          for mp, ad in zip(movepos, axes_d)]
+            self.toolhead.move(retractpos, homing_speed)
+            # Home again
+            forcepos = [rp - ad * retract_r
+                        for rp, ad in zip(retractpos, axes_d)]
+            self.toolhead.set_position(forcepos)
+            self.homing_move(movepos, endstops, second_homing_speed,
+                             verify_movement=self.verify_retract)
     def home_axes(self, axes):
         self.changed_axes = axes
         try: