From 8faab46ed2fc05495e63bbca8fe3dfa6828f7db3 Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Thu, 12 Jul 2018 22:15:45 -0400 Subject: 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 --- klippy/cartesian.py | 170 -------------------------- klippy/corexy.py | 143 ---------------------- klippy/delta.py | 200 ------------------------------- klippy/extras/delta_calibrate.py | 9 +- klippy/extruder.py | 253 --------------------------------------- klippy/gcode.py | 6 +- klippy/kinematics/__init__.py | 5 + klippy/kinematics/cartesian.py | 173 ++++++++++++++++++++++++++ klippy/kinematics/corexy.py | 146 ++++++++++++++++++++++ klippy/kinematics/delta.py | 203 +++++++++++++++++++++++++++++++ klippy/kinematics/extruder.py | 253 +++++++++++++++++++++++++++++++++++++++ klippy/klippy.py | 4 +- klippy/toolhead.py | 21 ++-- 13 files changed, 803 insertions(+), 783 deletions(-) delete mode 100644 klippy/cartesian.py delete mode 100644 klippy/corexy.py delete mode 100644 klippy/delta.py delete mode 100644 klippy/extruder.py create mode 100644 klippy/kinematics/__init__.py create mode 100644 klippy/kinematics/cartesian.py create mode 100644 klippy/kinematics/corexy.py create mode 100644 klippy/kinematics/delta.py create mode 100644 klippy/kinematics/extruder.py diff --git a/klippy/cartesian.py b/klippy/cartesian.py deleted file mode 100644 index 362ba555..00000000 --- a/klippy/cartesian.py +++ /dev/null @@ -1,170 +0,0 @@ -# Code for handling the kinematics of cartesian robots -# -# Copyright (C) 2016-2018 Kevin O'Connor -# -# 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() diff --git a/klippy/corexy.py b/klippy/corexy.py deleted file mode 100644 index 07330641..00000000 --- a/klippy/corexy.py +++ /dev/null @@ -1,143 +0,0 @@ -# Code for handling the kinematics of corexy robots -# -# Copyright (C) 2017-2018 Kevin O'Connor -# -# 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) diff --git a/klippy/delta.py b/klippy/delta.py deleted file mode 100644 index 328d01f5..00000000 --- a/klippy/delta.py +++ /dev/null @@ -1,200 +0,0 @@ -# Code for handling the kinematics of linear delta robots -# -# Copyright (C) 2016-2018 Kevin O'Connor -# -# 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) diff --git a/klippy/extras/delta_calibrate.py b/klippy/extras/delta_calibrate.py index 15b9001c..42104d29 100644 --- a/klippy/extras/delta_calibrate.py +++ b/klippy/extras/delta_calibrate.py @@ -4,7 +4,7 @@ # # This file may be distributed under the terms of the GNU GPLv3 license. import math, logging -import probe, delta, mathutil +import probe, mathutil, kinematics.delta class DeltaCalibrate: def __init__(self, config): @@ -40,10 +40,11 @@ class DeltaCalibrate: logging.info("Initial delta_calibrate parameters: %s", params) adj_params = ('endstop_a', 'endstop_b', 'endstop_c', 'radius', 'angle_a', 'angle_b') + get_position_from_stable = kinematics.delta.get_position_from_stable def delta_errorfunc(params): total_error = 0. for spos in positions: - x, y, z = delta.get_position_from_stable(spos, params) + x, y, z = get_position_from_stable(spos, params) total_error += (z - z_offset)**2 return total_error new_params = mathutil.coordinate_descent( @@ -51,8 +52,8 @@ class DeltaCalibrate: logging.info("Calculated delta_calibrate parameters: %s", new_params) for spos in positions: logging.info("orig: %s new: %s", - delta.get_position_from_stable(spos, params), - delta.get_position_from_stable(spos, new_params)) + get_position_from_stable(spos, params), + get_position_from_stable(spos, new_params)) self.gcode.respond_info( "stepper_a: position_endstop: %.6f angle: %.6f\n" "stepper_b: position_endstop: %.6f angle: %.6f\n" diff --git a/klippy/extruder.py b/klippy/extruder.py deleted file mode 100644 index 8e564d62..00000000 --- a/klippy/extruder.py +++ /dev/null @@ -1,253 +0,0 @@ -# Code for handling printer nozzle extruders -# -# Copyright (C) 2016-2018 Kevin O'Connor -# -# 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 diff --git a/klippy/gcode.py b/klippy/gcode.py index 68e367c0..e9c20055 100644 --- a/klippy/gcode.py +++ b/klippy/gcode.py @@ -4,7 +4,7 @@ # # This file may be distributed under the terms of the GNU GPLv3 license. import os, re, logging, collections -import homing, extruder +import homing, kinematics.extruder class error(Exception): pass @@ -116,7 +116,7 @@ class GCodeParser: if self.move_transform is None: self.move_with_transform = self.toolhead.move self.position_with_transform = self.toolhead.get_position - extruders = extruder.get_printer_extruders(self.printer) + extruders = kinematics.extruder.get_printer_extruders(self.printer) if extruders: self.extruder = extruders[0] self.toolhead.set_extruder(self.extruder) @@ -396,7 +396,7 @@ class GCodeParser: self.respond_info('Unknown command:"%s"' % (cmd,)) def cmd_Tn(self, params): # Select Tool - extruders = extruder.get_printer_extruders(self.printer) + extruders = kinematics.extruder.get_printer_extruders(self.printer) index = self.get_int('T', params, minval=0, maxval=len(extruders)-1) e = extruders[index] if self.extruder is e: diff --git a/klippy/kinematics/__init__.py b/klippy/kinematics/__init__.py new file mode 100644 index 00000000..09f968af --- /dev/null +++ b/klippy/kinematics/__init__.py @@ -0,0 +1,5 @@ +# Package definition for the kinematics directory +# +# Copyright (C) 2018 Kevin O'Connor +# +# This file may be distributed under the terms of the GNU GPLv3 license. diff --git a/klippy/kinematics/cartesian.py b/klippy/kinematics/cartesian.py new file mode 100644 index 00000000..22ee5b98 --- /dev/null +++ b/klippy/kinematics/cartesian.py @@ -0,0 +1,173 @@ +# Code for handling the kinematics of cartesian robots +# +# Copyright (C) 2016-2018 Kevin O'Connor +# +# 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) diff --git a/klippy/kinematics/corexy.py b/klippy/kinematics/corexy.py new file mode 100644 index 00000000..931793e7 --- /dev/null +++ b/klippy/kinematics/corexy.py @@ -0,0 +1,146 @@ +# Code for handling the kinematics of corexy robots +# +# Copyright (C) 2017-2018 Kevin O'Connor +# +# 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) diff --git a/klippy/kinematics/delta.py b/klippy/kinematics/delta.py new file mode 100644 index 00000000..ac023242 --- /dev/null +++ b/klippy/kinematics/delta.py @@ -0,0 +1,203 @@ +# Code for handling the kinematics of linear delta robots +# +# Copyright (C) 2016-2018 Kevin O'Connor +# +# 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) diff --git a/klippy/kinematics/extruder.py b/klippy/kinematics/extruder.py new file mode 100644 index 00000000..8e564d62 --- /dev/null +++ b/klippy/kinematics/extruder.py @@ -0,0 +1,253 @@ +# Code for handling printer nozzle extruders +# +# Copyright (C) 2016-2018 Kevin O'Connor +# +# 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 diff --git a/klippy/klippy.py b/klippy/klippy.py index 03d71eff..ba6b4579 100644 --- a/klippy/klippy.py +++ b/klippy/klippy.py @@ -7,7 +7,7 @@ import sys, os, optparse, logging, time, threading import collections, ConfigParser, importlib import util, reactor, queuelogger, msgproto -import gcode, pins, heater, mcu, toolhead, extruder +import gcode, pins, heater, mcu, toolhead message_ready = "Printer is ready" @@ -216,7 +216,7 @@ class Printer: m.add_printer_objects(config) for section in fileconfig.sections(): self.try_load_module(config, section) - for m in [toolhead, extruder]: + for m in [toolhead]: m.add_printer_objects(config) # Validate that there are no undefined parameters in the config file valid_sections = { s: 1 for s, o in self.all_config_options } diff --git a/klippy/toolhead.py b/klippy/toolhead.py index 6b0058d3..d9b26ef8 100644 --- a/klippy/toolhead.py +++ b/klippy/toolhead.py @@ -3,8 +3,8 @@ # Copyright (C) 2016-2018 Kevin O'Connor # # This file may be distributed under the terms of the GNU GPLv3 license. -import math, logging -import mcu, homing, cartesian, corexy, delta, extruder +import math, logging, importlib +import mcu, homing, kinematics.extruder # Common suffixes: _d is distance (in mm), _v is velocity (in # mm/second), _v2 is velocity squared (mm^2/s^2), _t is time (in @@ -227,12 +227,16 @@ class ToolHead: self.motor_off_timer = self.reactor.register_timer( self._motor_off_handler, self.reactor.NOW) # Create kinematics class - self.extruder = extruder.DummyExtruder() + self.extruder = kinematics.extruder.DummyExtruder() self.move_queue.set_extruder(self.extruder) - kintypes = {'cartesian': cartesian.CartKinematics, - 'corexy': corexy.CoreXYKinematics, - 'delta': delta.DeltaKinematics} - self.kin = config.getchoice('kinematics', kintypes)(self, config) + kin_name = config.get('kinematics') + try: + mod = importlib.import_module('kinematics.' + kin_name) + self.kin = mod.load_kinematics(self, config) + except: + msg = "Error loading kinematics '%s'" % (kin_name,) + logging.exception(msg) + raise config.error(msg) # SET_VELOCITY_LIMIT command gcode = self.printer.lookup_object('gcode') gcode.register_command('SET_VELOCITY_LIMIT', self.cmd_SET_VELOCITY_LIMIT, @@ -353,7 +357,7 @@ class ToolHead: self.dwell(STALL_TIME) last_move_time = self.get_last_move_time() self.kin.motor_off(last_move_time) - for ext in extruder.get_printer_extruders(self.printer): + for ext in kinematics.extruder.get_printer_extruders(self.printer): ext.motor_off(last_move_time) self.dwell(STALL_TIME) self.need_motor_off = False @@ -444,3 +448,4 @@ class ToolHead: def add_printer_objects(config): config.get_printer().add_object('toolhead', ToolHead(config)) + kinematics.extruder.add_printer_objects(config) -- cgit v1.2.3-70-g09d2