homing: Implement homing via new toolhead "drip" movement

Rework the low-level implementation of homing movement.  The existing
mechanism buffers all homing movement into the micro-controller prior
to starting the home.  Replace with a system that buffers all movement
into the host look-ahead buffer and then "drip feed" those moves to
the micro-controllers.  Then clear the host look-ahead buffer when all
endstops trigger.

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

klippy/toolhead.py

@@ -194,6 +194,11 @@ class MoveQueue:
 
 STALL_TIME = 0.100
 
+DRIP_SEGMENT_TIME = 0.050
+DRIP_TIME = 0.150
+class DripModeEndSignal(Exception):
+    pass
+
 # Main code to track events (and their timing) on the printer toolhead
 class ToolHead:
     def __init__(self, config):
@@ -238,6 +243,7 @@ class ToolHead:
         self.last_print_start_time = 0.
         self.idle_flush_print_time = 0.
         self.print_stall = 0
+        self.drip_completion = None
         # Setup iterative solver
         ffi_main, ffi_lib = chelper.get_ffi()
         self.cmove = ffi_main.gc(ffi_lib.move_alloc(), ffi_lib.free)
@@ -282,12 +288,24 @@ class ToolHead:
             self.printer.send_event("toolhead:sync_print_time",
                                     curtime, est_print_time, self.print_time)
     def get_next_move_time(self):
-        if self.special_queuing_state:
-            # Transition from "Flushed"/"Priming" state to main state
-            self.special_queuing_state = ""
-            self.need_check_stall = -1.
-            self.reactor.update_timer(self.flush_timer, self.reactor.NOW)
-            self._calc_print_time()
+        if not self.special_queuing_state:
+            return self.print_time
+        if self.special_queuing_state == "Drip":
+            # In "Drip" state - wait until ready to send next move
+            while 1:
+                if self.drip_completion.test():
+                    raise DripModeEndSignal()
+                curtime = self.reactor.monotonic()
+                est_print_time = self.mcu.estimated_print_time(curtime)
+                wait_time = self.print_time - est_print_time - DRIP_TIME
+                if wait_time <= 0. or self.mcu.is_fileoutput():
+                    return self.print_time
+                self.drip_completion.wait(curtime + wait_time)
+        # Transition from "Flushed"/"Priming" state to main state
+        self.special_queuing_state = ""
+        self.need_check_stall = -1.
+        self.reactor.update_timer(self.flush_timer, self.reactor.NOW)
+        self._calc_print_time()
         return self.print_time
     def _full_flush(self):
         # Transition from "Flushed"/"Priming"/main state to "Flushed" state
@@ -405,6 +423,42 @@ class ToolHead:
         self.commanded_pos[3] = extrude_pos
     def get_extruder(self):
         return self.extruder
+    def drip_move(self, newpos, speed):
+        # Validate move
+        move = Move(self, self.commanded_pos, newpos, speed)
+        if move.axes_d[3]:
+            raise homing.CommandError("Invalid drip move")
+        if not move.move_d or not move.is_kinematic_move:
+            return
+        self.kin.check_move(move)
+        speed = math.sqrt(move.max_cruise_v2)
+        # Transition to "Flushed" state and then to "Drip" state
+        self._full_flush()
+        self.special_queuing_state = "Drip"
+        self.need_check_stall = self.reactor.NEVER
+        self.reactor.update_timer(self.flush_timer, self.reactor.NEVER)
+        self.move_queue.set_flush_time(self.reactor.NEVER)
+        self.drip_completion = self.reactor.completion()
+        # Split move into many tiny moves and queue them
+        num_moves = max(1, int(math.ceil(move.min_move_t / DRIP_SEGMENT_TIME)))
+        inv_num_moves = 1. / float(num_moves)
+        submove_d = [d * inv_num_moves for d in move.axes_d]
+        prev_pos = move.start_pos
+        for i in range(num_moves-1):
+            next_pos = [p + d for p, d in zip(prev_pos, submove_d)]
+            self.move_queue.add_move(Move(self, prev_pos, next_pos, speed))
+            prev_pos = next_pos
+        self.move_queue.add_move(Move(self, prev_pos, move.end_pos, speed))
+        # Transmit moves
+        self._calc_print_time()
+        try:
+            self.move_queue.flush()
+        except DripModeEndSignal as e:
+            self.move_queue.reset()
+        # Return to "Flushed" state
+        self._full_flush()
+    def signal_drip_mode_end(self):
+        self.drip_completion.complete(True)
     # Misc commands
     def stats(self, eventtime):
         for m in self.all_mcus: