quad_gantry_level: Fixing logic to use probed Z values correctly (#1328)

Signed-off-by: Maks Zolin <mzolin@vorondesign.com>

1 files 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 <mzolin@mzbot.us>
+# Copyright (C) 2018  Maks Zolin <mzolin@vorondesign.com>
 #
 # 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()