toolhead: Initial support for adding extra axes to toolhead moves

Add a new add_extra_axes() to support adding additional axes.  Once
called, toolhead.get_position() will return a list object with more
than 4 items, and toolhead.move() requires the same size list.

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

2 files changed, 61 insertions, 32 deletions

diff --git a/klippy/kinematics/extruder.py b/klippy/kinematics/extruder.py
index 8de8ded2..2b07089b 100644
--- a/klippy/kinematics/extruder.py
+++ b/klippy/kinematics/extruder.py
@@ -199,24 +199,25 @@ class PrinterExtruder:
         return self.trapq
     def stats(self, eventtime):
         return self.heater.stats(eventtime)
-    def check_move(self, move):
-        axis_r = move.axes_r[3]
+    def check_move(self, move, ea_index):
         if not self.heater.can_extrude:
             raise self.printer.command_error(
                 "Extrude below minimum temp\n"
                 "See the 'min_extrude_temp' config option for details")
+        axis_r = move.axes_r[ea_index]
+        axis_d = move.axes_d[ea_index]
         if (not move.axes_d[0] and not move.axes_d[1]) or axis_r < 0.:
             # Extrude only move (or retraction move) - limit accel and velocity
-            if abs(move.axes_d[3]) > self.max_e_dist:
+            if abs(axis_d) > self.max_e_dist:
                 raise self.printer.command_error(
                     "Extrude only move too long (%.3fmm vs %.3fmm)\n"
                     "See the 'max_extrude_only_distance' config"
-                    " option for details" % (move.axes_d[3], self.max_e_dist))
+                    " option for details" % (axis_d, self.max_e_dist))
             inv_extrude_r = 1. / abs(axis_r)
             move.limit_speed(self.max_e_velocity * inv_extrude_r,
                              self.max_e_accel * inv_extrude_r)
         elif axis_r > self.max_extrude_ratio:
-            if move.axes_d[3] <= self.nozzle_diameter * self.max_extrude_ratio:
+            if axis_d <= self.nozzle_diameter * self.max_extrude_ratio:
                 # Permit extrusion if amount extruded is tiny
                 return
             area = axis_r * self.filament_area
@@ -226,13 +227,13 @@ class PrinterExtruder:
                 "Move exceeds maximum extrusion (%.3fmm^2 vs %.3fmm^2)\n"
                 "See the 'max_extrude_cross_section' config option for details"
                 % (area, self.max_extrude_ratio * self.filament_area))
-    def calc_junction(self, prev_move, move):
-        diff_r = move.axes_r[3] - prev_move.axes_r[3]
+    def calc_junction(self, prev_move, move, ea_index):
+        diff_r = move.axes_r[ea_index] - prev_move.axes_r[ea_index]
         if diff_r:
             return (self.instant_corner_v / abs(diff_r))**2
         return move.max_cruise_v2
-    def process_move(self, print_time, move):
-        axis_r = move.axes_r[3]
+    def process_move(self, print_time, move, ea_index):
+        axis_r = move.axes_r[ea_index]
         accel = move.accel * axis_r
         start_v = move.start_v * axis_r
         cruise_v = move.cruise_v * axis_r
@@ -242,10 +243,10 @@ class PrinterExtruder:
         # Queue movement (x is extruder movement, y is pressure advance flag)
         self.trapq_append(self.trapq, print_time,
                           move.accel_t, move.cruise_t, move.decel_t,
-                          move.start_pos[3], 0., 0.,
+                          move.start_pos[ea_index], 0., 0.,
                           1., can_pressure_advance, 0.,
                           start_v, cruise_v, accel)
-        self.last_position = move.end_pos[3]
+        self.last_position = move.end_pos[ea_index]
     def find_past_position(self, print_time):
         if self.extruder_stepper is None:
             return 0.
@@ -285,11 +286,11 @@ class PrinterExtruder:
 class DummyExtruder:
     def __init__(self, printer):
         self.printer = printer
-    def check_move(self, move):
+    def check_move(self, move, ea_index):
         raise move.move_error("Extrude when no extruder present")
     def find_past_position(self, print_time):
         return 0.
-    def calc_junction(self, prev_move, move):
+    def calc_junction(self, prev_move, move, ea_index):
         return move.max_cruise_v2
     def get_name(self):
         return ""
diff --git a/klippy/toolhead.py b/klippy/toolhead.py
index 93c87331..f835977c 100644
--- a/klippy/toolhead.py
+++ b/klippy/toolhead.py
@@ -21,14 +21,14 @@ class Move:
         self.timing_callbacks = []
         velocity = min(speed, toolhead.max_velocity)
         self.is_kinematic_move = True
-        self.axes_d = axes_d = [end_pos[i] - start_pos[i] for i in (0, 1, 2, 3)]
+        self.axes_d = axes_d = [ep - sp for sp, ep in zip(start_pos, end_pos)]
         self.move_d = move_d = math.sqrt(sum([d*d for d in axes_d[:3]]))
         if move_d < .000000001:
             # Extrude only move
-            self.end_pos = (start_pos[0], start_pos[1], start_pos[2],
-                            end_pos[3])
+            self.end_pos = ((start_pos[0], start_pos[1], start_pos[2])
+                            + self.end_pos[3:])
             axes_d[0] = axes_d[1] = axes_d[2] = 0.
-            self.move_d = move_d = abs(axes_d[3])
+            self.move_d = move_d = max([abs(ad) for ad in axes_d[3:]])
             inv_move_d = 0.
             if move_d:
                 inv_move_d = 1. / move_d
@@ -65,11 +65,13 @@ class Move:
     def calc_junction(self, prev_move):
         if not self.is_kinematic_move or not prev_move.is_kinematic_move:
             return
-        # Allow extruder to calculate its maximum junction
-        extruder_v2 = self.toolhead.extruder.calc_junction(prev_move, self)
-        max_start_v2 = min(extruder_v2, self.max_cruise_v2,
-                           prev_move.max_cruise_v2, prev_move.next_junction_v2,
-                           prev_move.max_start_v2 + prev_move.delta_v2)
+        # Allow extra axes to calculate maximum junction
+        ea_v2 = [ea.calc_junction(prev_move, self, e_index+3)
+                 for e_index, ea in enumerate(self.toolhead.extra_axes)]
+        max_start_v2 = min([self.max_cruise_v2,
+                            prev_move.max_cruise_v2, prev_move.next_junction_v2,
+                            prev_move.max_start_v2 + prev_move.delta_v2]
+                           + ea_v2)
         # Find max velocity using "approximated centripetal velocity"
         axes_r = self.axes_r
         prev_axes_r = prev_move.axes_r
@@ -263,7 +265,8 @@ class ToolHead:
         # Create kinematics class
         gcode = self.printer.lookup_object('gcode')
         self.Coord = gcode.Coord
-        self.extruder = kinematics.extruder.DummyExtruder(self.printer)
+        extruder = kinematics.extruder.DummyExtruder(self.printer)
+        self.extra_axes = [extruder]
         kin_name = config.get('kinematics')
         try:
             mod = importlib.import_module('kinematics.' + kin_name)
@@ -351,8 +354,9 @@ class ToolHead:
                     move.start_pos[0], move.start_pos[1], move.start_pos[2],
                     move.axes_r[0], move.axes_r[1], move.axes_r[2],
                     move.start_v, move.cruise_v, move.accel)
-            if move.axes_d[3]:
-                self.extruder.process_move(next_move_time, move)
+            for e_index, ea in enumerate(self.extra_axes):
+                if move.axes_d[e_index + 3]:
+                    ea.process_move(next_move_time, move, e_index + 3)
             next_move_time = (next_move_time + move.accel_t
                               + move.cruise_t + move.decel_t)
             for cb in move.timing_callbacks:
@@ -472,8 +476,9 @@ class ToolHead:
             return
         if move.is_kinematic_move:
             self.kin.check_move(move)
-        if move.axes_d[3]:
-            self.extruder.check_move(move)
+        for e_index, ea in enumerate(self.extra_axes):
+            if move.axes_d[e_index + 3]:
+                ea.check_move(move, e_index + 3)
         self.commanded_pos[:] = move.end_pos
         want_flush = self.lookahead.add_move(move)
         if want_flush:
@@ -500,16 +505,38 @@ class ToolHead:
                 break
             eventtime = self.reactor.pause(eventtime + 0.100)
     def set_extruder(self, extruder, extrude_pos):
-        prev_ea_trapq = self.extruder.get_trapq()
+        # XXX - should use add_extra_axis
+        prev_ea_trapq = self.extra_axes[0].get_trapq()
         if prev_ea_trapq in self.flush_trapqs:
             self.flush_trapqs.remove(prev_ea_trapq)
-        self.extruder = extruder
+        self.extra_axes[0] = extruder
         self.commanded_pos[3] = extrude_pos
         ea_trapq = extruder.get_trapq()
         if ea_trapq is not None:
             self.flush_trapqs.append(ea_trapq)
     def get_extruder(self):
-        return self.extruder
+        return self.extra_axes[0]
+    def add_extra_axis(self, ea, axis_pos):
+        self._flush_lookahead()
+        self.extra_axes.append(ea)
+        self.commanded_pos.append(axis_pos)
+        ea_trapq = ea.get_trapq()
+        if ea_trapq is not None:
+            self.flush_trapqs.append(ea_trapq)
+        self.printer.send_event("toolhead:update_extra_axes")
+    def remove_extra_axis(self, ea):
+        self._flush_lookahead()
+        if ea not in self.extra_axes:
+            return
+        ea_index = self.extra_axes.index(ea) + 3
+        ea_trapq = ea.get_trapq()
+        if ea_trapq in self.flush_trapqs:
+            self.flush_trapqs.remove(ea_trapq)
+        self.commanded_pos.pop(ea_index)
+        self.extra_axes.pop(ea_index - 3)
+        self.printer.send_event("toolhead:update_extra_axes")
+    def get_extra_axes(self):
+        return [None, None, None] + self.extra_axes
     # Homing "drip move" handling
     def drip_update_time(self, next_print_time, drip_completion, addstepper=()):
         # Transition from "NeedPrime"/"Priming"/main state to "Drip" state
@@ -593,12 +620,13 @@ class ToolHead:
     def get_status(self, eventtime):
         print_time = self.print_time
         estimated_print_time = self.mcu.estimated_print_time(eventtime)
+        extruder = self.extra_axes[0]
         res = dict(self.kin.get_status(eventtime))
         res.update({ 'print_time': print_time,
                      'stalls': self.print_stall,
                      'estimated_print_time': estimated_print_time,
-                     'extruder': self.extruder.get_name(),
-                     'position': self.Coord(*self.commanded_pos),
+                     'extruder': extruder.get_name(),
+                     'position': self.Coord(*self.commanded_pos[:4]),
                      'max_velocity': self.max_velocity,
                      'max_accel': self.max_accel,
                      'minimum_cruise_ratio': self.min_cruise_ratio,