From 975b0dbd842e3ffb29913912c88c5f462fcfd805 Mon Sep 17 00:00:00 2001 From: Voron <44981431+VoronDesign@users.noreply.github.com> Date: Fri, 8 Mar 2019 08:01:49 -0800 Subject: [PATCH] quad_gantry_level: Fixing logic to use probed Z values correctly (#1328) Signed-off-by: Maks Zolin --- klippy/extras/quad_gantry_level.py | 49 ++++++++++++++++++------------ 1 file changed, 30 insertions(+), 19 deletions(-) diff --git a/klippy/extras/quad_gantry_level.py b/klippy/extras/quad_gantry_level.py index ef8aae66..b3ca0252 100644 --- a/klippy/extras/quad_gantry_level.py +++ b/klippy/extras/quad_gantry_level.py @@ -1,6 +1,6 @@ # Mechanicaly conforms a moving gantry to the bed with 4 Z steppers # -# Copyright (C) 2018 Maks Zolin +# Copyright (C) 2018 Maks Zolin # # This file may be distributed under the terms of the GNU GPLv3 license. import logging @@ -9,6 +9,7 @@ import probe class QuadGantryLevel: def __init__(self, config): self.printer = config.get_printer() + self.horizontal_move_z = config.getfloat("horizontal_move_z", 5.0) self.printer.register_event_handler("klippy:connect", self.handle_connect) self.probe_helper = probe.ProbePointsHelper(config, self.probe_finalize) @@ -42,12 +43,17 @@ class QuadGantryLevel: def cmd_QUAD_GANTRY_LEVEL(self, params): self.probe_helper.start_probe(params) def probe_finalize(self, offsets, 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]] + # Mirror our perspective so the adjustments make sense + # from the perspective of the gantry + z_positions = [self.horizontal_move_z - p[2] for p in positions] + points_message = "Gantry-relative probe points:\n%s\n" % ( + "\n".join(["z%s = %.6f" % (z_id, z_positions[z_id]) + for z_id in range(len(z_positions))])) + self.gcode.respond_info(points_message) + p1 = [positions[0][0] + offsets[0],z_positions[0]] + p2 = [positions[1][0] + offsets[0],z_positions[1]] + p3 = [positions[2][0] + offsets[0],z_positions[2]] + p4 = [positions[3][0] + offsets[0],z_positions[3]] f1 = self.linefit(p1,p4) f2 = self.linefit(p2,p3) logging.info("quad_gantry_level f1: %s, f2: %s" % (f1,f2)) @@ -59,19 +65,22 @@ class QuadGantryLevel: 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] + z_height = [0,0,0,0] + z_height[0] = self.plot(af,self.gantry_corners[0][1]) + z_height[1] = self.plot(af,self.gantry_corners[1][1]) + z_height[2] = self.plot(bf,self.gantry_corners[1][1]) + z_height[3] = self.plot(bf,self.gantry_corners[0][1]) + self.gcode.respond_info("Actuator Positions:\n z1: %0.6f\n z2: %0.6f\ + \n z3: %0.6f\n z4: %0.6f\n" % ( + z_height[0],z_height[1],z_height[2],z_height[3])) + z_ave = sum(z_height) / len(z_height) + self.gcode.respond_info("Average: %0.6f" % z_ave) + z_adjust = [] + for z in z_height: + z_adjust.append(z_ave - z) try: self.adjust_steppers(z_adjust) except: @@ -96,16 +105,18 @@ class QuadGantryLevel: self.gcode.respond_info(msg) toolhead = self.printer.lookup_object('toolhead') cur_pos = toolhead.get_position() - speed = self.probe_helper.get_lift_speed() / 2 + speed = self.probe_helper.get_lift_speed() + # Disable moves on all Z steppers 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] + 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) + # Re-enable moves on all Z steppers for s in self.z_steppers: s.set_ignore_move(False) self.gcode.reset_last_position()