diff --git a/config/example-extras.cfg b/config/example-extras.cfg index 0c75e62d..4f8a61b2 100644 --- a/config/example-extras.cfg +++ b/config/example-extras.cfg @@ -933,3 +933,43 @@ # Replicape support - see the generic-replicape.cfg file for further # details. #[replicape] + +# Moving gantry leveling using 4 independently controlled Z motors. +# Corrects hyperbolic parabola effects (potato chip) on moving gantry +# which is more flexible. +# WARNING: Using this on a moving bed may lead to undesirable results. +# If this section is present then a QUAD_GANTRY_LEVEL extended G-Code +# command becomes available. This routine assumes the following Z motor +# configuration: +# ---------------- +# |Z1 Z2| +# | --------- | +# | | | | +# | | | | +# | x-------- | +# |Z Z3| +# ---------------- +# Where x is the (0,0) point on the bed +#[quad_gantry_level] +#gantry_corners: +# A newline separated list of X,Y coordinates describing the +# two opposing corners of the gantry. The first entry corresponds to +# Z, the second to Z2. +# This parameter must be provided. +#points: +# A newline separated list of four X,Y points that should be probed +# during a QUAD_GANTRY_LEVEL command. +# Order of the locations is important, and should correspond to Z,Z1 +# Z2, and Z3 location in order. +# This parameter must be provided. +# For maximum accuracy, ensure your probe offsets are configured. +#speed: 50 +# The speed (in mm/s) of non-probing moves during the calibration. +# The default is 50. +#horizontal_move_z: 5 +# The height (in mm) that the head should be commanded to move to +# just prior to starting a probe operation. The default is 5 +#samples: 1 +# Number of probe samples per point. The defaut is 1 +#sample_retract_dist: 2.0 +# Distance in mm to retract the probe between samples. Default is 2. diff --git a/klippy/extras/quad_gantry_level.py b/klippy/extras/quad_gantry_level.py new file mode 100644 index 00000000..4c167d15 --- /dev/null +++ b/klippy/extras/quad_gantry_level.py @@ -0,0 +1,124 @@ +# Mechanicaly conforms a moving gantry to the bed with 4 Z steppers +# +# Copyright (C) 2018 Maks Zolin +# +# This file may be distributed under the terms of the GNU GPLv3 license. +import logging +import probe + +class QuadGantryLevel: + def __init__(self, config): + self.printer = config.get_printer() + self.probe_helper = probe.ProbePointsHelper(config, self) + gantry_corners = config.get('gantry_corners').split('\n') + try: + gantry_corners = [line.split(',', 1) + for line in gantry_corners if line.strip()] + self.gantry_corners = [(float(zp[0].strip()), float(zp[1].strip())) + for zp in gantry_corners] + except: + raise config.error("Unable to parse gantry_corners in %s" % ( + config.get_name())) + if len(self.gantry_corners) < 2: + raise config.error("quad_gantry_level requires at least two gantry_corners") + self.z_steppers = [] + # Register QUAD_GANTRY_LEVEL command + self.gcode = self.printer.lookup_object('gcode') + self.gcode.register_command( + 'QUAD_GANTRY_LEVEL', self.cmd_QUAD_GANTRY_LEVEL, + desc=self.cmd_QUAD_GANTRY_LEVEL_help) + def printer_state(self, state): + if state == 'connect': + self.handle_connect() + def handle_connect(self): + kin = self.printer.lookup_object('toolhead').get_kinematics() + z_steppers = kin.get_steppers('Z') + if len(z_steppers) != 4: + raise self.printer.config_error("quad_gantry_level needs exactly 4 z steppers") + self.z_steppers = z_steppers + cmd_QUAD_GANTRY_LEVEL_help = "Conform a moving, twistable gantry to the shape of a stationary bed" + def cmd_QUAD_GANTRY_LEVEL(self, params): + self.probe_helper.start_probe() + def get_probed_position(self): + kin = self.printer.lookup_object('toolhead').get_kinematics() + return kin.calc_position() + def squash_positions(self,positions): + # Group multi-probe data and average out the Z readings + # Assumes samples come in sequentially + grouped_pos = [] + for position in positions: + if len(grouped_pos) > 0 and grouped_pos[-1][0] == position[0] and grouped_pos[-1][1] == position[1]: + grouped_pos[-1][2].append(position[2]) + else: + grouped_pos.append(position) + grouped_pos[-1][2] = [grouped_pos[-1][2]] + for id,pos in enumerate(grouped_pos): + grouped_pos[id][2] = sum(grouped_pos[id][2]) / len(grouped_pos[id][2]) + return grouped_pos + def finalize(self, offsets, positions): + if len(positions) > 4: + positions = self.squash_positions(positions) + logging.info("quad_gantry_level Calculating gantry geometry with: %s", positions) + p1 = [positions[0][0] + offsets[0],positions[0][2]] + p2 = [positions[1][0] + offsets[0],positions[1][2]] + p3 = [positions[2][0] + offsets[0],positions[2][2]] + p4 = [positions[3][0] + offsets[0],positions[3][2]] + f1 = self.linefit(p1,p4) + f2 = self.linefit(p2,p3) + logging.info("quad_gantry_level f1: %s, f2: %s" % (f1,f2)) + a1 = [positions[0][1] + offsets[1], self.plot(f1,self.gantry_corners[0][0])] + a2 = [positions[1][1] + offsets[1], self.plot(f2,self.gantry_corners[0][0])] + b1 = [positions[0][1] + offsets[1], self.plot(f1,self.gantry_corners[1][0])] + b2 = [positions[1][1] + offsets[1], self.plot(f2,self.gantry_corners[1][0])] + logging.info("quad_gantry_level a1: %s a2: %s b1: %s b2: %s\n" % (a1,a2,b1,b2)) + af = self.linefit(a1,a2) + bf = self.linefit(b1,b2) + logging.info("quad_gantry_level af: %s, bf: %s" % (af,bf)) + z_adjust = [0,0,0,0] + z_adjust[0] = self.plot(af,self.gantry_corners[0][1]) + z_adjust[1] = self.plot(af,self.gantry_corners[1][1]) + z_adjust[2] = self.plot(bf,self.gantry_corners[1][1]) + z_adjust[3] = self.plot(bf,self.gantry_corners[0][1]) + z_avg = sum(z_adjust)/len(z_adjust) + for z_id in range(4): + z_adjust[z_id] = z_avg - z_adjust[z_id] + try: + self.adjust_steppers(z_adjust) + except: + logging.exception("quad_gantry_level adjust_steppers") + for s in self.z_steppers: + z.set_ignore_move(False) + raise + def linefit(self,p1,p2): + if p1[1] == p2[1]: + # Straight line + return 0,p1[1] + m = (p2[1] - p1[1])/(p2[0] - p1[0]) + b = p1[1] - m * p1[0] + return m,b + def plot(self,f,x): + return f[0]*x + f[1] + def adjust_steppers(self, z_adjust): + msg = "Making the following gantry adjustments:\n%s\n" % ( + "\n".join(["%s = %.6f" % ( + self.z_steppers[z_id].get_name(), z_adjust[z_id] + ) for z_id in range(4)])) + self.gcode.respond_info(msg) + toolhead = self.printer.lookup_object('toolhead') + cur_pos = toolhead.get_position() + speed = self.probe_helper.get_lift_speed() / 2 + for s in self.z_steppers: + s.set_ignore_move(True) + for z_id in range(len(z_adjust)): + stepper = self.z_steppers[z_id] + stepper.set_ignore_move(False) + cur_pos[2] = cur_pos[2] - z_adjust[z_id] + toolhead.move(cur_pos, speed) + toolhead.set_position(cur_pos) + stepper.set_ignore_move(True) + for s in self.z_steppers: + s.set_ignore_move(False) + self.gcode.reset_last_position() + +def load_config(config): + return QuadGantryLevel(config)