bed_tilt: Add support for automatic bed tilt move transformation

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

3 files changed, 144 insertions, 3 deletions

diff --git a/config/example-extras.cfg b/config/example-extras.cfg
index 70dc1c1f..23806689 100644
--- a/config/example-extras.cfg
+++ b/config/example-extras.cfg
@@ -17,6 +17,41 @@
 #   command. The default is 0.
 
 
+# Bed tilt compensation. One may define a [bed_tilt] config section to
+# enable move transformations that account for a tilted bed.
+#[bed_tilt]
+#x_adjust: 0
+#   The amount to add to each move's Z height for each mm on the X
+#   axis. The default is 0.
+#y_adjust: 0
+#   The amount to add to each move's Z height for each mm on the Y
+#   axis. The default is 0.
+# The remaining parameters control a BED_TILT_CALIBRATE extended
+# g-code command that may be used to calibrate appropriate x and y
+# adjustment parameters.
+#points:
+#    A newline separated list of X,Y points that should be probed
+#    during a BED_TILT_CALIBRATE command. The default is to not enable
+#    the command.
+#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.
+#probe_z_offset: 0
+#   The Z height (in mm) of the head when the probe triggers. The
+#   default is 0.
+#manual_probe:
+#   If true, then BED_TILT_CALIBRATE will perform manual probing. If
+#   false, then a PROBE command will be run at each probe
+#   point. Manual probing is accomplished by manually jogging the Z
+#   position of the print head at each probe point and then issuing a
+#   NEXT extended g-code command to record the position at that
+#   point. The default is false if a [probe] config section is present
+#   and true otherwise.
+
+
 # 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/klippy/extras/bed_tilt.py b/klippy/extras/bed_tilt.py
new file mode 100644
index 00000000..4307e8c7
--- /dev/null
+++ b/klippy/extras/bed_tilt.py
@@ -0,0 +1,95 @@
+# Bed tilt compensation
+#
+# 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
+
+class BedTilt:
+    def __init__(self, config):
+        self.printer = config.get_printer()
+        self.x_adjust = config.getfloat('x_adjust', 0.)
+        self.y_adjust = config.getfloat('y_adjust', 0.)
+        if config.get('points', None) is not None:
+            BedTiltCalibrate(config, self)
+        self.toolhead = None
+        gcode = self.printer.lookup_object('gcode')
+        gcode.set_move_transform(self)
+    def printer_state(self, state):
+        if state == 'connect':
+            self.toolhead = self.printer.lookup_object('toolhead')
+    def get_position(self):
+        x, y, z, e = self.toolhead.get_position()
+        return [x, y, z - x*self.x_adjust - y*self.y_adjust, e]
+    def move(self, newpos, speed):
+        x, y, z, e = newpos
+        self.toolhead.move([x, y, z + x*self.x_adjust + y*self.y_adjust, e],
+                           speed)
+
+# Helper script to calibrate the bed tilt
+class BedTiltCalibrate:
+    def __init__(self, config, bedtilt):
+        self.bedtilt = bedtilt
+        self.printer = config.get_printer()
+        points = config.get('points').split('\n')
+        try:
+            points = [line.split(',', 1) for line in points if line.strip()]
+            self.points = [(float(p[0].strip()), float(p[1].strip()))
+                           for p in points]
+        except:
+            raise config.error("Unable to parse bed tilt points")
+        self.speed = config.getfloat('speed', 50., above=0.)
+        self.horizontal_move_z = config.getfloat('horizontal_move_z', 5.)
+        self.probe_z_offset = config.getfloat('probe_z_offset', 0.)
+        self.manual_probe = config.getboolean('manual_probe', None)
+        if self.manual_probe is None:
+            self.manual_probe = not config.has_section('probe')
+        self.gcode = self.printer.lookup_object('gcode')
+        self.gcode.register_command(
+            'BED_TILT_CALIBRATE', self.cmd_BED_TILT_CALIBRATE,
+            desc=self.cmd_BED_TILT_CALIBRATE_help)
+    cmd_BED_TILT_CALIBRATE_help = "Bed tilt calibration script"
+    def cmd_BED_TILT_CALIBRATE(self, params):
+        self.gcode.run_script("G28")
+        probe.ProbePointsHelper(
+            self.printer, self.points, self.horizontal_move_z,
+            self.speed, self.manual_probe, self)
+    def get_position(self):
+        kin = self.printer.lookup_object('toolhead').get_kinematics()
+        return kin.get_position()
+    def finalize(self, positions):
+        logging.debug("Got: %s", positions)
+        params = { 'x_adjust': self.bedtilt.x_adjust,
+                   'y_adjust': self.bedtilt.y_adjust,
+                   'z_adjust': self.probe_z_offset }
+        logging.debug("Params: %s", params)
+        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 = probe.coordinate_descent(params.keys(), params, errorfunc)
+        logging.debug("Got2: %s", new_params)
+        for pos in positions:
+            logging.debug("orig: %s new: %s",
+                          adjusted_height(pos, params),
+                          adjusted_height(pos, new_params))
+        z_warn = ""
+        z_diff = new_params['z_adjust'] - self.probe_z_offset
+        if abs(z_diff) > .010:
+            z_warn = "Note: Z offset was %.6f\n" % (z_diff,)
+        self.gcode.respond_info(
+            "%sx_adjust: %.6f y_adjust: %.6f\n"
+            "To use these parameters, update the printer config file with\n"
+            "the above and then issue a RESTART command" % (
+                z_warn, new_params['x_adjust'], new_params['y_adjust']))
+
+def load_config(config):
+    if config.get_name() != 'bed_tilt':
+        raise config.error("Invalid bed_tilt config name")
+    return BedTilt(config)
diff --git a/klippy/gcode.py b/klippy/gcode.py
index c6d5e23d..2c6a983b 100644
--- a/klippy/gcode.py
+++ b/klippy/gcode.py
@@ -45,6 +45,8 @@ class GCodeParser:
         self.homing_add = [0.0, 0.0, 0.0, 0.0]
         self.speed_factor = 1. / 60.
         self.extrude_factor = 1.
+        self.move_transform = self.move_with_transform = None
+        self.position_with_transform = (lambda: [0., 0., 0., 0.])
         # G-Code state
         self.need_ack = False
         self.toolhead = self.fan = self.extruder = None
@@ -66,6 +68,13 @@ class GCodeParser:
             self.base_gcode_handlers[cmd] = func
         if desc is not None:
             self.gcode_help[cmd] = desc
+    def set_move_transform(self, transform):
+        if self.move_transform is not None:
+            raise self.printer.config_error(
+                "G-Code move transform already specified")
+        self.move_transform = transform
+        self.move_with_transform = transform.move
+        self.position_with_transform = transform.get_position
     def stats(self, eventtime):
         return "gcodein=%d" % (self.bytes_read,)
     def printer_state(self, state):
@@ -84,6 +93,9 @@ class GCodeParser:
         self.gcode_handlers = self.ready_gcode_handlers
         # Lookup printer components
         self.toolhead = self.printer.lookup_object('toolhead')
+        if self.move_transform is None:
+            self.move_with_transform = self.toolhead.move
+            self.position_with_transform = self.toolhead.get_position
         extruders = extruder.get_printer_extruders(self.printer)
         if extruders:
             self.extruder = extruders[0]
@@ -94,8 +106,7 @@ class GCodeParser:
         if self.is_fileinput and self.fd_handle is None:
             self.fd_handle = self.reactor.register_fd(self.fd, self.process_data)
     def reset_last_position(self):
-        if self.toolhead is not None:
-            self.last_position = self.toolhead.get_position()
+        self.last_position = self.position_with_transform()
     def motor_heater_off(self):
         if self.toolhead is None:
             return
@@ -361,7 +372,7 @@ class GCodeParser:
         except ValueError as e:
             raise error("Unable to parse move '%s'" % (params['#original'],))
         try:
-            self.toolhead.move(self.last_position, self.speed)
+            self.move_with_transform(self.last_position, self.speed)
         except homing.EndstopError as e:
             raise error(str(e))
     def cmd_G4(self, params):