toolhead: Move kinematic modules to new kinematics/ directory

Move extruder.py, cartesian.py, corexy.py, and delta.py to a new
kinematics/ sub-directory.  This is intended to make adding new
kinematics a little easier.

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

klippy/kinematics/__init__.py

@@ -0,0 +1,5 @@
+# Package definition for the kinematics directory
+#
+# Copyright (C) 2018  Kevin O'Connor <kevin@koconnor.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.

klippy/kinematics/cartesian.py

@@ -0,0 +1,173 @@
+# Code for handling the kinematics of cartesian robots
+#
+# Copyright (C) 2016-2018  Kevin O'Connor <kevin@koconnor.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+import logging
+import stepper, homing, chelper
+
+class CartKinematics:
+    def __init__(self, toolhead, config):
+        self.printer = config.get_printer()
+        self.rails = [stepper.LookupMultiRail(config.getsection('stepper_' + n))
+                      for n in ['x', 'y', 'z']]
+        max_velocity, max_accel = toolhead.get_max_velocity()
+        self.max_z_velocity = config.getfloat(
+            'max_z_velocity', max_velocity, above=0., maxval=max_velocity)
+        self.max_z_accel = config.getfloat(
+            'max_z_accel', max_accel, above=0., maxval=max_accel)
+        self.need_motor_enable = True
+        self.limits = [(1.0, -1.0)] * 3
+        # Setup iterative solver
+        ffi_main, ffi_lib = chelper.get_ffi()
+        self.cmove = ffi_main.gc(ffi_lib.move_alloc(), ffi_lib.free)
+        self.move_fill = ffi_lib.move_fill
+        for axis, rail in zip('xyz', self.rails):
+            rail.setup_cartesian_itersolve(axis)
+        # Setup stepper max halt velocity
+        max_halt_velocity = toolhead.get_max_axis_halt()
+        self.rails[0].set_max_jerk(max_halt_velocity, max_accel)
+        self.rails[1].set_max_jerk(max_halt_velocity, max_accel)
+        self.rails[2].set_max_jerk(
+            min(max_halt_velocity, self.max_z_velocity), max_accel)
+        # Check for dual carriage support
+        self.dual_carriage_axis = None
+        self.dual_carriage_rails = []
+        if config.has_section('dual_carriage'):
+            dc_config = config.getsection('dual_carriage')
+            dc_axis = dc_config.getchoice('axis', {'x': 'x', 'y': 'y'})
+            self.dual_carriage_axis = {'x': 0, 'y': 1}[dc_axis]
+            dc_rail = stepper.LookupMultiRail(dc_config)
+            dc_rail.setup_cartesian_itersolve(dc_axis)
+            dc_rail.set_max_jerk(max_halt_velocity, max_accel)
+            self.dual_carriage_rails = [
+                self.rails[self.dual_carriage_axis], dc_rail]
+            self.printer.lookup_object('gcode').register_command(
+                'SET_DUAL_CARRIAGE', self.cmd_SET_DUAL_CARRIAGE,
+                desc=self.cmd_SET_DUAL_CARRIAGE_help)
+    def get_rails(self, flags=""):
+        if flags == "Z":
+            return [self.rails[2]]
+        return list(self.rails)
+    def calc_position(self):
+        return [rail.get_commanded_position() for rail in self.rails]
+    def set_position(self, newpos, homing_axes):
+        for i, rail in enumerate(self.rails):
+            rail.set_position(newpos)
+            if i in homing_axes:
+                self.limits[i] = rail.get_range()
+    def _home_axis(self, homing_state, axis, rail):
+        # Determine moves
+        position_min, position_max = rail.get_range()
+        hi = rail.get_homing_info()
+        if hi.positive_dir:
+            pos = hi.position_endstop - 1.5*(hi.position_endstop - position_min)
+            rpos = hi.position_endstop - hi.retract_dist
+            r2pos = rpos - hi.retract_dist
+        else:
+            pos = hi.position_endstop + 1.5*(position_max - hi.position_endstop)
+            rpos = hi.position_endstop + hi.retract_dist
+            r2pos = rpos + hi.retract_dist
+        # Initial homing
+        homing_speed = hi.speed
+        if axis == 2:
+            homing_speed = min(homing_speed, self.max_z_velocity)
+        homepos = [None, None, None, None]
+        homepos[axis] = hi.position_endstop
+        coord = [None, None, None, None]
+        coord[axis] = pos
+        homing_state.home(coord, homepos, rail.get_endstops(), homing_speed)
+        # Retract
+        coord[axis] = rpos
+        homing_state.retract(coord, homing_speed)
+        # Home again
+        coord[axis] = r2pos
+        homing_state.home(coord, homepos, rail.get_endstops(),
+                          homing_speed/2.0, second_home=True)
+        # Set final homed position
+        coord[axis] = hi.position_endstop + rail.get_homed_offset()
+        homing_state.set_homed_position(coord)
+    def home(self, homing_state):
+        # Each axis is homed independently and in order
+        for axis in homing_state.get_axes():
+            if axis == self.dual_carriage_axis:
+                dc1, dc2 = self.dual_carriage_rails
+                altc = self.rails[axis] == dc2
+                self._activate_carriage(0)
+                self._home_axis(homing_state, axis, dc1)
+                self._activate_carriage(1)
+                self._home_axis(homing_state, axis, dc2)
+                self._activate_carriage(altc)
+            else:
+                self._home_axis(homing_state, axis, self.rails[axis])
+    def motor_off(self, print_time):
+        self.limits = [(1.0, -1.0)] * 3
+        for rail in self.rails:
+            rail.motor_enable(print_time, 0)
+        for rail in self.dual_carriage_rails:
+            rail.motor_enable(print_time, 0)
+        self.need_motor_enable = True
+    def _check_motor_enable(self, print_time, move):
+        need_motor_enable = False
+        for i, rail in enumerate(self.rails):
+            if move.axes_d[i]:
+                rail.motor_enable(print_time, 1)
+            need_motor_enable |= not rail.is_motor_enabled()
+        self.need_motor_enable = need_motor_enable
+    def _check_endstops(self, move):
+        end_pos = move.end_pos
+        for i in (0, 1, 2):
+            if (move.axes_d[i]
+                and (end_pos[i] < self.limits[i][0]
+                     or end_pos[i] > self.limits[i][1])):
+                if self.limits[i][0] > self.limits[i][1]:
+                    raise homing.EndstopMoveError(
+                        end_pos, "Must home axis first")
+                raise homing.EndstopMoveError(end_pos)
+    def check_move(self, move):
+        limits = self.limits
+        xpos, ypos = move.end_pos[:2]
+        if (xpos < limits[0][0] or xpos > limits[0][1]
+            or ypos < limits[1][0] or ypos > limits[1][1]):
+            self._check_endstops(move)
+        if not move.axes_d[2]:
+            # Normal XY move - use defaults
+            return
+        # Move with Z - update velocity and accel for slower Z axis
+        self._check_endstops(move)
+        z_ratio = move.move_d / abs(move.axes_d[2])
+        move.limit_speed(
+            self.max_z_velocity * z_ratio, self.max_z_accel * z_ratio)
+    def move(self, print_time, move):
+        if self.need_motor_enable:
+            self._check_motor_enable(print_time, move)
+        self.move_fill(
+            self.cmove, print_time,
+            move.accel_t, move.cruise_t, move.decel_t,
+            move.start_pos[0], move.start_pos[1], move.start_pos[2],
+            move.axes_d[0], move.axes_d[1], move.axes_d[2],
+            move.start_v, move.cruise_v, move.accel)
+        for i, rail in enumerate(self.rails):
+            if move.axes_d[i]:
+                rail.step_itersolve(self.cmove)
+    # Dual carriage support
+    def _activate_carriage(self, carriage):
+        toolhead = self.printer.lookup_object('toolhead')
+        toolhead.get_last_move_time()
+        dc_rail = self.dual_carriage_rails[carriage]
+        dc_axis = self.dual_carriage_axis
+        self.rails[dc_axis] = dc_rail
+        extruder_pos = toolhead.get_position()[3]
+        toolhead.set_position(self.calc_position() + [extruder_pos])
+        if self.limits[dc_axis][0] <= self.limits[dc_axis][1]:
+            self.limits[dc_axis] = dc_rail.get_range()
+        self.need_motor_enable = True
+    cmd_SET_DUAL_CARRIAGE_help = "Set which carriage is active"
+    def cmd_SET_DUAL_CARRIAGE(self, params):
+        gcode = self.printer.lookup_object('gcode')
+        carriage = gcode.get_int('CARRIAGE', params, minval=0, maxval=1)
+        self._activate_carriage(carriage)
+        gcode.reset_last_position()
+
+def load_kinematics(toolhead, config):
+    return CartKinematics(toolhead, config)

klippy/kinematics/corexy.py

@@ -0,0 +1,146 @@
+# Code for handling the kinematics of corexy robots
+#
+# Copyright (C) 2017-2018  Kevin O'Connor <kevin@koconnor.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+import logging, math
+import stepper, homing, chelper
+
+class CoreXYKinematics:
+    def __init__(self, toolhead, config):
+        self.rails = [ stepper.PrinterRail(config.getsection('stepper_x')),
+                       stepper.PrinterRail(config.getsection('stepper_y')),
+                       stepper.LookupMultiRail(config.getsection('stepper_z')) ]
+        self.rails[0].add_to_endstop(self.rails[1].get_endstops()[0][0])
+        self.rails[1].add_to_endstop(self.rails[0].get_endstops()[0][0])
+        max_velocity, max_accel = toolhead.get_max_velocity()
+        self.max_z_velocity = config.getfloat(
+            'max_z_velocity', max_velocity, above=0., maxval=max_velocity)
+        self.max_z_accel = config.getfloat(
+            'max_z_accel', max_accel, above=0., maxval=max_accel)
+        self.need_motor_enable = True
+        self.limits = [(1.0, -1.0)] * 3
+        # Setup iterative solver
+        ffi_main, ffi_lib = chelper.get_ffi()
+        self.cmove = ffi_main.gc(ffi_lib.move_alloc(), ffi_lib.free)
+        self.move_fill = ffi_lib.move_fill
+        self.rails[0].setup_itersolve(ffi_main.gc(
+            ffi_lib.corexy_stepper_alloc('+'), ffi_lib.free))
+        self.rails[1].setup_itersolve(ffi_main.gc(
+            ffi_lib.corexy_stepper_alloc('-'), ffi_lib.free))
+        self.rails[2].setup_cartesian_itersolve('z')
+        # Setup stepper max halt velocity
+        max_halt_velocity = toolhead.get_max_axis_halt()
+        max_xy_halt_velocity = max_halt_velocity * math.sqrt(2.)
+        self.rails[0].set_max_jerk(max_xy_halt_velocity, max_accel)
+        self.rails[1].set_max_jerk(max_xy_halt_velocity, max_accel)
+        self.rails[2].set_max_jerk(
+            min(max_halt_velocity, self.max_z_velocity), self.max_z_accel)
+    def get_rails(self, flags=""):
+        if flags == "Z":
+            return [self.rails[2]]
+        return list(self.rails)
+    def calc_position(self):
+        pos = [rail.get_commanded_position() for rail in self.rails]
+        return [0.5 * (pos[0] + pos[1]), 0.5 * (pos[0] - pos[1]), pos[2]]
+    def set_position(self, newpos, homing_axes):
+        for i, rail in enumerate(self.rails):
+            rail.set_position(newpos)
+            if i in homing_axes:
+                self.limits[i] = rail.get_range()
+    def home(self, homing_state):
+        # Each axis is homed independently and in order
+        for axis in homing_state.get_axes():
+            rail = self.rails[axis]
+            # Determine moves
+            position_min, position_max = rail.get_range()
+            hi = rail.get_homing_info()
+            if hi.positive_dir:
+                pos = hi.position_endstop - 1.5*(
+                    hi.position_endstop - position_min)
+                rpos = hi.position_endstop - hi.retract_dist
+                r2pos = rpos - hi.retract_dist
+            else:
+                pos = hi.position_endstop + 1.5*(
+                    position_max - hi.position_endstop)
+                rpos = hi.position_endstop + hi.retract_dist
+                r2pos = rpos + hi.retract_dist
+            # Initial homing
+            homing_speed = hi.speed
+            if axis == 2:
+                homing_speed = min(homing_speed, self.max_z_velocity)
+            homepos = [None, None, None, None]
+            homepos[axis] = hi.position_endstop
+            coord = [None, None, None, None]
+            coord[axis] = pos
+            homing_state.home(coord, homepos, rail.get_endstops(), homing_speed)
+            # Retract
+            coord[axis] = rpos
+            homing_state.retract(coord, homing_speed)
+            # Home again
+            coord[axis] = r2pos
+            homing_state.home(coord, homepos, rail.get_endstops(),
+                              homing_speed/2.0, second_home=True)
+            if axis == 2:
+                # Support endstop phase detection on Z axis
+                coord[axis] = hi.position_endstop + rail.get_homed_offset()
+                homing_state.set_homed_position(coord)
+    def motor_off(self, print_time):
+        self.limits = [(1.0, -1.0)] * 3
+        for rail in self.rails:
+            rail.motor_enable(print_time, 0)
+        self.need_motor_enable = True
+    def _check_motor_enable(self, print_time, move):
+        if move.axes_d[0] or move.axes_d[1]:
+            self.rails[0].motor_enable(print_time, 1)
+            self.rails[1].motor_enable(print_time, 1)
+        if move.axes_d[2]:
+            self.rails[2].motor_enable(print_time, 1)
+        need_motor_enable = False
+        for rail in self.rails:
+            need_motor_enable |= not rail.is_motor_enabled()
+        self.need_motor_enable = need_motor_enable
+    def _check_endstops(self, move):
+        end_pos = move.end_pos
+        for i in (0, 1, 2):
+            if (move.axes_d[i]
+                and (end_pos[i] < self.limits[i][0]
+                     or end_pos[i] > self.limits[i][1])):
+                if self.limits[i][0] > self.limits[i][1]:
+                    raise homing.EndstopMoveError(
+                        end_pos, "Must home axis first")
+                raise homing.EndstopMoveError(end_pos)
+    def check_move(self, move):
+        limits = self.limits
+        xpos, ypos = move.end_pos[:2]
+        if (xpos < limits[0][0] or xpos > limits[0][1]
+            or ypos < limits[1][0] or ypos > limits[1][1]):
+            self._check_endstops(move)
+        if not move.axes_d[2]:
+            # Normal XY move - use defaults
+            return
+        # Move with Z - update velocity and accel for slower Z axis
+        self._check_endstops(move)
+        z_ratio = move.move_d / abs(move.axes_d[2])
+        move.limit_speed(
+            self.max_z_velocity * z_ratio, self.max_z_accel * z_ratio)
+    def move(self, print_time, move):
+        if self.need_motor_enable:
+            self._check_motor_enable(print_time, move)
+        axes_d = move.axes_d
+        cmove = self.cmove
+        self.move_fill(
+            cmove, print_time,
+            move.accel_t, move.cruise_t, move.decel_t,
+            move.start_pos[0], move.start_pos[1], move.start_pos[2],
+            axes_d[0], axes_d[1], axes_d[2],
+            move.start_v, move.cruise_v, move.accel)
+        rail_x, rail_y, rail_z = self.rails
+        if axes_d[0] or axes_d[1]:
+            rail_x.step_itersolve(cmove)
+            rail_y.step_itersolve(cmove)
+        if axes_d[2]:
+            rail_z.step_itersolve(cmove)
+
+def load_kinematics(toolhead, config):
+    return CoreXYKinematics(toolhead, config)

klippy/kinematics/delta.py

@@ -0,0 +1,203 @@
+# Code for handling the kinematics of linear delta robots
+#
+# Copyright (C) 2016-2018  Kevin O'Connor <kevin@koconnor.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+import math, logging
+import stepper, homing, chelper, mathutil
+
+# Slow moves once the ratio of tower to XY movement exceeds SLOW_RATIO
+SLOW_RATIO = 3.
+
+class DeltaKinematics:
+    def __init__(self, toolhead, config):
+        # Setup tower rails
+        stepper_configs = [config.getsection('stepper_' + n)
+                           for n in ['a', 'b', 'c']]
+        rail_a = stepper.PrinterRail(
+            stepper_configs[0], need_position_minmax = False)
+        a_endstop = rail_a.get_homing_info().position_endstop
+        rail_b = stepper.PrinterRail(
+            stepper_configs[1], need_position_minmax = False,
+            default_position_endstop=a_endstop)
+        rail_c = stepper.PrinterRail(
+            stepper_configs[2], need_position_minmax = False,
+            default_position_endstop=a_endstop)
+        self.rails = [rail_a, rail_b, rail_c]
+        # Read radius and arm lengths
+        self.radius = radius = config.getfloat('delta_radius', above=0.)
+        arm_length_a = stepper_configs[0].getfloat('arm_length', above=radius)
+        self.arm_lengths = arm_lengths = [
+            sconfig.getfloat('arm_length', arm_length_a, above=radius)
+            for sconfig in stepper_configs]
+        self.arm2 = [arm**2 for arm in arm_lengths]
+        self.endstops = [(rail.get_homing_info().position_endstop
+                          + math.sqrt(arm2 - radius**2))
+                         for rail, arm2 in zip(self.rails, self.arm2)]
+        # Setup boundary checks
+        self.need_motor_enable = self.need_home = True
+        self.limit_xy2 = -1.
+        self.max_z = min([rail.get_homing_info().position_endstop
+                          for rail in self.rails])
+        self.min_z = config.getfloat('minimum_z_position', 0, maxval=self.max_z)
+        self.limit_z = min([ep - arm
+                            for ep, arm in zip(self.endstops, arm_lengths)])
+        logging.info(
+            "Delta max build height %.2fmm (radius tapered above %.2fmm)" % (
+                self.max_z, self.limit_z))
+        # Setup stepper max halt velocity
+        self.max_velocity, self.max_accel = toolhead.get_max_velocity()
+        self.max_z_velocity = config.getfloat(
+            'max_z_velocity', self.max_velocity,
+            above=0., maxval=self.max_velocity)
+        max_halt_velocity = toolhead.get_max_axis_halt()
+        for rail in self.rails:
+            rail.set_max_jerk(max_halt_velocity, self.max_accel)
+        # Determine tower locations in cartesian space
+        self.angles = [sconfig.getfloat('angle', angle)
+                       for sconfig, angle in zip(stepper_configs,
+                                                 [210., 330., 90.])]
+        self.towers = [(math.cos(math.radians(angle)) * radius,
+                        math.sin(math.radians(angle)) * radius)
+                       for angle in self.angles]
+        # Setup iterative solver
+        ffi_main, ffi_lib = chelper.get_ffi()
+        self.cmove = ffi_main.gc(ffi_lib.move_alloc(), ffi_lib.free)
+        self.move_fill = ffi_lib.move_fill
+        for r, a, t in zip(self.rails, self.arm2, self.towers):
+            sk = ffi_main.gc(ffi_lib.delta_stepper_alloc(a, t[0], t[1]),
+                             ffi_lib.free)
+            r.setup_itersolve(sk)
+        # Find the point where an XY move could result in excessive
+        # tower movement
+        half_min_step_dist = min([r.get_steppers()[0].get_step_dist()
+                                  for r in self.rails]) * .5
+        min_arm_length = min(arm_lengths)
+        def ratio_to_dist(ratio):
+            return (ratio * math.sqrt(min_arm_length**2 / (ratio**2 + 1.)
+                                      - half_min_step_dist**2)
+                    + half_min_step_dist)
+        self.slow_xy2 = (ratio_to_dist(SLOW_RATIO) - radius)**2
+        self.very_slow_xy2 = (ratio_to_dist(2. * SLOW_RATIO) - radius)**2
+        self.max_xy2 = min(radius, min_arm_length - radius,
+                           ratio_to_dist(4. * SLOW_RATIO) - radius)**2
+        logging.info(
+            "Delta max build radius %.2fmm (moves slowed past %.2fmm and %.2fmm)"
+            % (math.sqrt(self.max_xy2), math.sqrt(self.slow_xy2),
+               math.sqrt(self.very_slow_xy2)))
+        self.set_position([0., 0., 0.], ())
+    def get_rails(self, flags=""):
+        return list(self.rails)
+    def _actuator_to_cartesian(self, spos):
+        sphere_coords = [(t[0], t[1], sp) for t, sp in zip(self.towers, spos)]
+        return mathutil.trilateration(sphere_coords, self.arm2)
+    def calc_position(self):
+        spos = [rail.get_commanded_position() for rail in self.rails]
+        return self._actuator_to_cartesian(spos)
+    def set_position(self, newpos, homing_axes):
+        for rail in self.rails:
+            rail.set_position(newpos)
+        self.limit_xy2 = -1.
+        if tuple(homing_axes) == (0, 1, 2):
+            self.need_home = False
+    def home(self, homing_state):
+        # All axes are homed simultaneously
+        homing_state.set_axes([0, 1, 2])
+        endstops = [es for rail in self.rails for es in rail.get_endstops()]
+        # Initial homing - assume homing speed same for all steppers
+        hi = self.rails[0].get_homing_info()
+        homing_speed = min(hi.speed, self.max_z_velocity)
+        homepos = [0., 0., self.max_z, None]
+        coord = list(homepos)
+        coord[2] = -1.5 * math.sqrt(max(self.arm2)-self.max_xy2)
+        homing_state.home(coord, homepos, endstops, homing_speed)
+        # Retract
+        coord[2] = homepos[2] - hi.retract_dist
+        homing_state.retract(coord, homing_speed)
+        # Home again
+        coord[2] -= hi.retract_dist
+        homing_state.home(coord, homepos, endstops,
+                          homing_speed/2.0, second_home=True)
+        # Set final homed position
+        spos = [ep + rail.get_homed_offset()
+                for ep, rail in zip(self.endstops, self.rails)]
+        homing_state.set_homed_position(self._actuator_to_cartesian(spos))
+    def motor_off(self, print_time):
+        self.limit_xy2 = -1.
+        for rail in self.rails:
+            rail.motor_enable(print_time, 0)
+        self.need_motor_enable = self.need_home = True
+    def _check_motor_enable(self, print_time):
+        for rail in self.rails:
+            rail.motor_enable(print_time, 1)
+        self.need_motor_enable = False
+    def check_move(self, move):
+        end_pos = move.end_pos
+        xy2 = end_pos[0]**2 + end_pos[1]**2
+        if xy2 <= self.limit_xy2 and not move.axes_d[2]:
+            # Normal XY move
+            return
+        if self.need_home:
+            raise homing.EndstopMoveError(end_pos, "Must home first")
+        limit_xy2 = self.max_xy2
+        if end_pos[2] > self.limit_z:
+            limit_xy2 = min(limit_xy2, (self.max_z - end_pos[2])**2)
+        if xy2 > limit_xy2 or end_pos[2] < self.min_z or end_pos[2] > self.max_z:
+            raise homing.EndstopMoveError(end_pos)
+        if move.axes_d[2]:
+            move.limit_speed(self.max_z_velocity, move.accel)
+            limit_xy2 = -1.
+        # Limit the speed/accel of this move if is is at the extreme
+        # end of the build envelope
+        extreme_xy2 = max(xy2, move.start_pos[0]**2 + move.start_pos[1]**2)
+        if extreme_xy2 > self.slow_xy2:
+            r = 0.5
+            if extreme_xy2 > self.very_slow_xy2:
+                r = 0.25
+            max_velocity = self.max_velocity
+            if move.axes_d[2]:
+                max_velocity = self.max_z_velocity
+            move.limit_speed(max_velocity * r, self.max_accel * r)
+            limit_xy2 = -1.
+        self.limit_xy2 = min(limit_xy2, self.slow_xy2)
+    def move(self, print_time, move):
+        if self.need_motor_enable:
+            self._check_motor_enable(print_time)
+        self.move_fill(
+            self.cmove, print_time,
+            move.accel_t, move.cruise_t, move.decel_t,
+            move.start_pos[0], move.start_pos[1], move.start_pos[2],
+            move.axes_d[0], move.axes_d[1], move.axes_d[2],
+            move.start_v, move.cruise_v, move.accel)
+        for rail in self.rails:
+            rail.step_itersolve(self.cmove)
+    # Helper functions for DELTA_CALIBRATE script
+    def get_stable_position(self):
+        steppers = [rail.get_steppers()[0] for rail in self.rails]
+        return [int((ep - s.get_commanded_position()) / s.get_step_dist() + .5)
+                * s.get_step_dist()
+                for ep, s in zip(self.endstops, steppers)]
+    def get_calibrate_params(self):
+        return {
+            'endstop_a': self.rails[0].get_homing_info().position_endstop,
+            'endstop_b': self.rails[1].get_homing_info().position_endstop,
+            'endstop_c': self.rails[2].get_homing_info().position_endstop,
+            'angle_a': self.angles[0], 'angle_b': self.angles[1],
+            'angle_c': self.angles[2], 'radius': self.radius,
+            'arm_a': self.arm_lengths[0], 'arm_b': self.arm_lengths[1],
+            'arm_c': self.arm_lengths[2] }
+
+def get_position_from_stable(spos, params):
+    angles = [params['angle_a'], params['angle_b'], params['angle_c']]
+    radius = params['radius']
+    radius2 = radius**2
+    towers = [(math.cos(angle) * radius, math.sin(angle) * radius)
+              for angle in map(math.radians, angles)]
+    arm2 = [a**2 for a in [params['arm_a'], params['arm_b'], params['arm_c']]]
+    endstops = [params['endstop_a'], params['endstop_b'], params['endstop_c']]
+    sphere_coords = [(t[0], t[1], es + math.sqrt(a2 - radius2) - p)
+                     for t, es, a2, p in zip(towers, endstops, arm2, spos)]
+    return mathutil.trilateration(sphere_coords, arm2)
+
+def load_kinematics(toolhead, config):
+    return DeltaKinematics(toolhead, config)

klippy/kinematics/extruder.py

@@ -0,0 +1,253 @@
+# Code for handling printer nozzle extruders
+#
+# Copyright (C) 2016-2018  Kevin O'Connor <kevin@koconnor.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+import math, logging
+import stepper, homing, chelper
+
+EXTRUDE_DIFF_IGNORE = 1.02
+
+class PrinterExtruder:
+    def __init__(self, config):
+        self.printer = config.get_printer()
+        self.name = config.get_name()
+        shared_heater = config.get('shared_heater', None)
+        pheater = self.printer.lookup_object('heater')
+        if shared_heater is None:
+            self.heater = pheater.setup_heater(config)
+        else:
+            self.heater = pheater.lookup_heater(shared_heater)
+        self.stepper = stepper.PrinterStepper(config)
+        self.nozzle_diameter = config.getfloat('nozzle_diameter', above=0.)
+        filament_diameter = config.getfloat(
+            'filament_diameter', minval=self.nozzle_diameter)
+        self.filament_area = math.pi * (filament_diameter * .5)**2
+        max_cross_section = config.getfloat(
+            'max_extrude_cross_section', 4. * self.nozzle_diameter**2
+            , above=0.)
+        self.max_extrude_ratio = max_cross_section / self.filament_area
+        logging.info("Extruder max_extrude_ratio=%.6f", self.max_extrude_ratio)
+        toolhead = self.printer.lookup_object('toolhead')
+        max_velocity, max_accel = toolhead.get_max_velocity()
+        self.max_e_velocity = config.getfloat(
+            'max_extrude_only_velocity', max_velocity * self.max_extrude_ratio
+            , above=0.)
+        self.max_e_accel = config.getfloat(
+            'max_extrude_only_accel', max_accel * self.max_extrude_ratio
+            , above=0.)
+        self.stepper.set_max_jerk(9999999.9, 9999999.9)
+        self.max_e_dist = config.getfloat(
+            'max_extrude_only_distance', 50., minval=0.)
+        self.activate_gcode = config.get('activate_gcode', '')
+        self.deactivate_gcode = config.get('deactivate_gcode', '')
+        self.pressure_advance = config.getfloat(
+            'pressure_advance', 0., minval=0.)
+        self.pressure_advance_lookahead_time = config.getfloat(
+            'pressure_advance_lookahead_time', 0.010, minval=0.)
+        self.need_motor_enable = True
+        self.extrude_pos = 0.
+        # Setup iterative solver
+        ffi_main, ffi_lib = chelper.get_ffi()
+        self.cmove = ffi_main.gc(ffi_lib.move_alloc(), ffi_lib.free)
+        self.extruder_move_fill = ffi_lib.extruder_move_fill
+        sk = ffi_main.gc(ffi_lib.extruder_stepper_alloc(), ffi_lib.free)
+        self.stepper.setup_itersolve(sk)
+        # Setup SET_PRESSURE_ADVANCE command
+        gcode = self.printer.lookup_object('gcode')
+        if self.name in ('extruder', 'extruder0'):
+            gcode.register_mux_command("SET_PRESSURE_ADVANCE", "EXTRUDER", None,
+                                       self.cmd_default_SET_PRESSURE_ADVANCE,
+                                       desc=self.cmd_SET_PRESSURE_ADVANCE_help)
+        gcode.register_mux_command("SET_PRESSURE_ADVANCE", "EXTRUDER", self.name,
+                                   self.cmd_SET_PRESSURE_ADVANCE,
+                                   desc=self.cmd_SET_PRESSURE_ADVANCE_help)
+    def get_heater(self):
+        return self.heater
+    def set_active(self, print_time, is_active):
+        return self.extrude_pos
+    def get_activate_gcode(self, is_active):
+        if is_active:
+            return self.activate_gcode
+        return self.deactivate_gcode
+    def stats(self, eventtime):
+        return self.heater.stats(eventtime)
+    def motor_off(self, print_time):
+        self.stepper.motor_enable(print_time, 0)
+        self.need_motor_enable = True
+    def check_move(self, move):
+        move.extrude_r = move.axes_d[3] / move.move_d
+        move.extrude_max_corner_v = 0.
+        if not self.heater.can_extrude:
+            raise homing.EndstopError(
+                "Extrude below minimum temp\n"
+                "See the 'min_extrude_temp' config option for details")
+        if not move.is_kinematic_move or move.extrude_r < 0.:
+            # Extrude only move (or retraction move) - limit accel and velocity
+            if abs(move.axes_d[3]) > self.max_e_dist:
+                raise homing.EndstopError(
+                    "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))
+            inv_extrude_r = 1. / abs(move.extrude_r)
+            move.limit_speed(self.max_e_velocity * inv_extrude_r
+                             , self.max_e_accel * inv_extrude_r)
+        elif move.extrude_r > self.max_extrude_ratio:
+            if move.axes_d[3] <= self.nozzle_diameter * self.max_extrude_ratio:
+                # Permit extrusion if amount extruded is tiny
+                move.extrude_r = self.max_extrude_ratio
+                return
+            area = move.axes_d[3] * self.filament_area / move.move_d
+            logging.debug("Overextrude: %s vs %s (area=%.3f dist=%.3f)",
+                          move.extrude_r, self.max_extrude_ratio,
+                          area, move.move_d)
+            raise homing.EndstopError(
+                "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):
+        extrude = move.axes_d[3]
+        prev_extrude = prev_move.axes_d[3]
+        if extrude or prev_extrude:
+            if not extrude or not prev_extrude:
+                # Extrude move to non-extrude move - disable lookahead
+                return 0.
+            if ((move.extrude_r > prev_move.extrude_r * EXTRUDE_DIFF_IGNORE
+                 or prev_move.extrude_r > move.extrude_r * EXTRUDE_DIFF_IGNORE)
+                and abs(move.move_d * prev_move.extrude_r - extrude) >= .001):
+                # Extrude ratio between moves is too different
+                return 0.
+            move.extrude_r = prev_move.extrude_r
+        return move.max_cruise_v2
+    def lookahead(self, moves, flush_count, lazy):
+        lookahead_t = self.pressure_advance_lookahead_time
+        if not self.pressure_advance or not lookahead_t:
+            return flush_count
+        # Calculate max_corner_v - the speed the head will accelerate
+        # to after cornering.
+        for i in range(flush_count):
+            move = moves[i]
+            if not move.decel_t:
+                continue
+            cruise_v = move.cruise_v
+            max_corner_v = 0.
+            sum_t = lookahead_t
+            for j in range(i+1, flush_count):
+                fmove = moves[j]
+                if not fmove.max_start_v2:
+                    break
+                if fmove.cruise_v > max_corner_v:
+                    if (not max_corner_v
+                        and not fmove.accel_t and not fmove.cruise_t):
+                        # Start timing after any full decel moves
+                        continue
+                    if sum_t >= fmove.accel_t:
+                        max_corner_v = fmove.cruise_v
+                    else:
+                        max_corner_v = max(
+                            max_corner_v, fmove.start_v + fmove.accel * sum_t)
+                    if max_corner_v >= cruise_v:
+                        break
+                sum_t -= fmove.accel_t + fmove.cruise_t + fmove.decel_t
+                if sum_t <= 0.:
+                    break
+            else:
+                if lazy:
+                    return i
+            move.extrude_max_corner_v = max_corner_v
+        return flush_count
+    def move(self, print_time, move):
+        if self.need_motor_enable:
+            self.stepper.motor_enable(print_time, 1)
+            self.need_motor_enable = False
+        axis_d = move.axes_d[3]
+        axis_r = axis_d / move.move_d
+        accel = move.accel * axis_r
+        start_v = move.start_v * axis_r
+        cruise_v = move.cruise_v * axis_r
+        accel_t, cruise_t, decel_t = move.accel_t, move.cruise_t, move.decel_t
+
+        # Update for pressure advance
+        extra_accel_v = extra_decel_v = 0.
+        start_pos = self.extrude_pos
+        if (axis_d >= 0. and (move.axes_d[0] or move.axes_d[1])
+            and self.pressure_advance):
+            # Calculate extra_accel_v
+            pressure_advance = self.pressure_advance * move.extrude_r
+            prev_pressure_d = start_pos - move.start_pos[3]
+            if accel_t:
+                npd = move.cruise_v * pressure_advance
+                extra_accel_d = npd - prev_pressure_d
+                if extra_accel_d > 0.:
+                    extra_accel_v = extra_accel_d / accel_t
+                    axis_d += extra_accel_d
+                    prev_pressure_d += extra_accel_d
+            # Calculate extra_decel_v
+            emcv = move.extrude_max_corner_v
+            if decel_t and emcv < move.cruise_v:
+                npd = max(emcv, move.end_v) * pressure_advance
+                extra_decel_d = npd - prev_pressure_d
+                if extra_decel_d < 0.:
+                    axis_d += extra_decel_d
+                    extra_decel_v = extra_decel_d / decel_t
+
+        # Generate steps
+        self.extruder_move_fill(
+            self.cmove, print_time, accel_t, cruise_t, decel_t, start_pos,
+            start_v, cruise_v, accel, extra_accel_v, extra_decel_v)
+        self.stepper.step_itersolve(self.cmove)
+        self.extrude_pos = start_pos + axis_d
+    cmd_SET_PRESSURE_ADVANCE_help = "Set pressure advance parameters"
+    def cmd_default_SET_PRESSURE_ADVANCE(self, params):
+        extruder = self.printer.lookup_object('toolhead').get_extruder()
+        extruder.cmd_SET_PRESSURE_ADVANCE(params)
+    def cmd_SET_PRESSURE_ADVANCE(self, params):
+        self.printer.lookup_object('toolhead').get_last_move_time()
+        gcode = self.printer.lookup_object('gcode')
+        pressure_advance = gcode.get_float(
+            'ADVANCE', params, self.pressure_advance, minval=0.)
+        pressure_advance_lookahead_time = gcode.get_float(
+            'ADVANCE_LOOKAHEAD_TIME', params,
+            self.pressure_advance_lookahead_time, minval=0.)
+        self.pressure_advance = pressure_advance
+        self.pressure_advance_lookahead_time = pressure_advance_lookahead_time
+        msg = ("pressure_advance: %.6f\n"
+               "pressure_advance_lookahead_time: %.6f" % (
+                   pressure_advance, pressure_advance_lookahead_time))
+        self.printer.set_rollover_info(self.name, "%s: %s" % (self.name, msg))
+        gcode.respond_info(msg)
+
+# Dummy extruder class used when a printer has no extruder at all
+class DummyExtruder:
+    def set_active(self, print_time, is_active):
+        return 0.
+    def motor_off(self, move_time):
+        pass
+    def check_move(self, move):
+        raise homing.EndstopMoveError(
+            move.end_pos, "Extrude when no extruder present")
+    def calc_junction(self, prev_move, move):
+        return move.max_cruise_v2
+    def lookahead(self, moves, flush_count, lazy):
+        return flush_count
+
+def add_printer_objects(config):
+    printer = config.get_printer()
+    for i in range(99):
+        section = 'extruder%d' % (i,)
+        if not config.has_section(section):
+            if not i and config.has_section('extruder'):
+                pe = PrinterExtruder(config.getsection('extruder'))
+                printer.add_object('extruder0', pe)
+                continue
+            break
+        printer.add_object(section, PrinterExtruder(config.getsection(section)))
+
+def get_printer_extruders(printer):
+    out = []
+    for i in range(99):
+        extruder = printer.lookup_object('extruder%d' % (i,), None)
+        if extruder is None:
+            break
+        out.append(extruder)
+    return out