z_tilt: Add support for Z_TILT_ADJUST

Add new module to support independent adjustments to multiple Z
steppers to account for bed tilt.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>

5 files changed, 152 insertions, 2 deletions

diff --git a/config/example-extras.cfg b/config/example-extras.cfg
index 82fdfd9d..0cc95b0f 100644
--- a/config/example-extras.cfg
+++ b/config/example-extras.cfg
@@ -65,6 +65,28 @@
 #   and true otherwise.
 
 
+# Multiple Z stepper tilt adjustment. This feature enables independent
+# adjustment of multiple z steppers (see stepper_z1 section below) to
+# adjust for tilt. If this section is present then a Z_TILT_ADJUST
+# extended G-Code command becomes available.
+#[z_tilt]
+#z_positions:
+#    A newline separated list of X,Y coordinates describing the
+#    location of each Z stepper. The first entry corresponds to
+#    stepper_z, the second to stepper_z1, the third to stepper_z2,
+#    etc. This parameter must be provided.
+#points:
+#    A newline separated list of X,Y points that should be probed
+#    during a Z_TILT_ADJUST command. The default is to use the same
+#    positions described in z_positions.
+#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.
+
+
 # In a multi-extruder printer add an additional extruder section for
 # each additional extruder. The additional extruder sections should be
 # named "extruder1", "extruder2", "extruder3", and so on. See the
diff --git a/docs/G-Codes.md b/docs/G-Codes.md
index aa795205..009debc1 100644
--- a/docs/G-Codes.md
+++ b/docs/G-Codes.md
@@ -139,6 +139,14 @@ section is enabled:
     command to move to the next probing point during a
     BED_TILT_CALIBRATE operation.
 
+## Z Tilt
+
+The following commands are available when the "z_tilt" config section
+is enabled:
+- `Z_TILT_ADJUST`: This command will probe the points specified in the
+  config and then make independent adjustments to each Z stepper to
+  compensate for tilt.
+
 ## Dual Carriages
 
 The following command is available when the "dual_carriage" config
diff --git a/klippy/extras/probe.py b/klippy/extras/probe.py
index 7408c90d..ee7757fe 100644
--- a/klippy/extras/probe.py
+++ b/klippy/extras/probe.py
@@ -169,6 +169,8 @@ class ProbePointsHelper:
         self.results = []
         self.busy = False
         self.gcode = self.toolhead = None
+    def get_lift_speed(self):
+        return self.lift_speed
     def start_probe(self):
         # Begin probing
         self.toolhead = self.printer.lookup_object('toolhead')
diff --git a/klippy/extras/z_tilt.py b/klippy/extras/z_tilt.py
new file mode 100644
index 00000000..4966a2e3
--- /dev/null
+++ b/klippy/extras/z_tilt.py
@@ -0,0 +1,109 @@
+# Mechanical bed tilt calibration with multiple Z steppers
+#
+# Copyright (C) 2018  Kevin O'Connor <kevin@koconnor.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+import logging
+import probe, mathutil
+
+class ZTilt:
+    def __init__(self, config):
+        self.printer = config.get_printer()
+        z_positions = config.get('z_positions').split('\n')
+        try:
+            z_positions = [line.split(',', 1)
+                           for line in z_positions if line.strip()]
+            self.z_positions = [(float(zp[0].strip()), float(zp[1].strip()))
+                                for zp in z_positions]
+        except:
+            raise config.error("Unable to parse z_positions in %s" % (
+                config.get_name()))
+        self.probe_helper = probe.ProbePointsHelper(
+            config, self, default_points=self.z_positions)
+        self.z_steppers = []
+        # Register Z_TILT_ADJUST command
+        self.gcode = self.printer.lookup_object('gcode')
+        self.gcode.register_command(
+            'Z_TILT_ADJUST', self.cmd_Z_TILT_ADJUST,
+            desc=self.cmd_Z_TILT_ADJUST_help)
+    def printer_state(self, state):
+        if state == 'connect':
+            self.handle_connect()
+    def handle_connect(self):
+        kin = self.printer.lookup_object('toolhead').get_kinematics()
+        try:
+            z_stepper = kin.get_steppers('Z')[0]
+            z_steppers = [z_stepper] + z_stepper.extras
+        except:
+            logging.exception("z_tilt stepper lookup")
+            raise self.printer.config_error(
+                "z_tilt requires multiple Z steppers")
+        if len(z_steppers) != len(self.z_positions):
+            raise self.printer.config_error(
+                "z_tilt z_positions needs exactly %d items" % (len(z_steppers),))
+        self.z_steppers = z_steppers
+    cmd_Z_TILT_ADJUST_help = "Adjust the Z tilt"
+    def cmd_Z_TILT_ADJUST(self, params):
+        self.probe_helper.start_probe()
+    def get_position(self):
+        kin = self.printer.lookup_object('toolhead').get_kinematics()
+        return kin.get_position()
+    def finalize(self, z_offset, positions):
+        logging.info("Calculating bed tilt with: %s", positions)
+        params = { 'x_adjust': 0., 'y_adjust': 0., 'z_adjust': z_offset }
+        def adjusted_height(pos, params):
+            x, y, z = pos
+            return (z - x*params['x_adjust'] - y*params['y_adjust']
+                    - params['z_adjust'])
+        def errorfunc(params):
+            total_error = 0.
+            for pos in positions:
+                total_error += adjusted_height(pos, params)**2
+            return total_error
+        new_params = mathutil.coordinate_descent(
+            params.keys(), params, errorfunc)
+        logging.info("Calculated bed tilt parameters: %s", new_params)
+        try:
+            self.adjust_steppers(new_params['x_adjust'], new_params['y_adjust'],
+                                 new_params['z_adjust'], z_offset)
+        except:
+            logging.exception("z_tilt adjust_steppers")
+            for s in self.z_steppers:
+                z.mcu_stepper.set_ignore_move(False)
+            raise
+    def adjust_steppers(self, x_adjust, y_adjust, z_adjust, z_offset):
+        toolhead = self.printer.lookup_object('toolhead')
+        curpos = toolhead.get_position()
+        speed = self.probe_helper.get_lift_speed()
+        # Find each stepper adjustment and disable all stepper movements
+        positions = []
+        for s, (x, y) in zip(self.z_steppers, self.z_positions):
+            s.mcu_stepper.set_ignore_move(True)
+            stepper_offset = -(x*x_adjust + y*y_adjust)
+            positions.append((stepper_offset, s))
+        # Report on movements
+        msg = "Making the following Z tilt adjustments:\n%s\nz_offset = %.6f" % (
+            "\n".join(["%s = %.6f" % (s.name, so) for so, s in positions]),
+            z_adjust - z_offset)
+        logging.info(msg)
+        self.gcode.respond_info(msg)
+        # Move each z stepper (sorted from lowest to highest) until they match
+        positions.sort()
+        first_stepper_offset, first_stepper = positions[0]
+        z_low = curpos[2] - first_stepper_offset
+        for i in range(len(positions)-1):
+            stepper_offset, stepper = positions[i]
+            next_stepper_offset, next_stepper = positions[i+1]
+            stepper.mcu_stepper.set_ignore_move(False)
+            curpos[2] = z_low + next_stepper_offset
+            toolhead.move(curpos, speed)
+            toolhead.set_position(curpos)
+        # Z should now be level - do final cleanup
+        last_stepper_offset, last_stepper = positions[-1]
+        last_stepper.mcu_stepper.set_ignore_move(False)
+        curpos[2] -= z_adjust - z_offset
+        toolhead.set_position(curpos)
+        self.gcode.reset_last_position()
+
+def load_config(config):
+    return ZTilt(config)
diff --git a/klippy/mcu.py b/klippy/mcu.py
index c124bd6d..1bfb14c1 100644
--- a/klippy/mcu.py
+++ b/klippy/mcu.py
@@ -23,6 +23,7 @@ class MCU_stepper:
         self._min_stop_interval = 0.
         self._reset_cmd_id = self._get_position_cmd = None
         self._ffi_lib = self._stepqueue = None
+        self._stepcompress_push_const = self._stepcompress_push_delta = None
     def get_mcu(self):
         return self._mcu
     def setup_dir_pin(self, pin_params):
@@ -60,6 +61,7 @@ class MCU_stepper:
             self._invert_dir, self._oid),
                                       self._ffi_lib.stepcompress_free)
         self._mcu.register_stepqueue(self._stepqueue)
+        self.set_ignore_move(False)
     def get_oid(self):
         return self._oid
     def get_step_dist(self):
@@ -75,6 +77,13 @@ class MCU_stepper:
         if mcu_pos >= 0.:
             return int(mcu_pos + 0.5)
         return int(mcu_pos - 0.5)
+    def set_ignore_move(self, ignore_move):
+        if ignore_move:
+            self._stepcompress_push_const = (lambda *args: 0)
+            self._stepcompress_push_delta = (lambda *args: 0)
+        else:
+            self._stepcompress_push_const = self._ffi_lib.stepcompress_push_const
+            self._stepcompress_push_delta = self._ffi_lib.stepcompress_push_delta
     def note_homing_start(self, homing_clock):
         ret = self._ffi_lib.stepcompress_set_homing(
             self._stepqueue, homing_clock)
@@ -109,7 +118,7 @@ class MCU_stepper:
     def step_const(self, print_time, start_pos, dist, start_v, accel):
         inv_step_dist = self._inv_step_dist
         step_offset = self._commanded_pos - start_pos * inv_step_dist
-        count = self._ffi_lib.stepcompress_push_const(
+        count = self._stepcompress_push_const(
             self._stepqueue, print_time, step_offset, dist * inv_step_dist,
             start_v * inv_step_dist, accel * inv_step_dist)
         if count == STEPCOMPRESS_ERROR_RET:
@@ -119,7 +128,7 @@ class MCU_stepper:
                    , height_base, startxy_d, arm_d, movez_r):
         inv_step_dist = self._inv_step_dist
         height = self._commanded_pos - height_base * inv_step_dist
-        count = self._ffi_lib.stepcompress_push_delta(
+        count = self._stepcompress_push_delta(
             self._stepqueue, print_time, dist * inv_step_dist,
             start_v * inv_step_dist, accel * inv_step_dist,
             height, startxy_d * inv_step_dist, arm_d * inv_step_dist, movez_r)