diff options
Diffstat (limited to 'klippy/kinematics')
| -rw-r--r-- | klippy/kinematics/__init__.py | 5 | ||||
| -rw-r--r-- | klippy/kinematics/cartesian.py | 173 | ||||
| -rw-r--r-- | klippy/kinematics/corexy.py | 146 | ||||
| -rw-r--r-- | klippy/kinematics/delta.py | 203 | ||||
| -rw-r--r-- | klippy/kinematics/extruder.py | 253 |
5 files changed, 780 insertions, 0 deletions
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 <kevin@koconnor.net> +# +# 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 <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) 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 <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) 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 <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) 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 <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 |