klipper update

klippy/extras/z_tilt.py

@@ -0,0 +1,179 @@
+# Mechanical bed tilt calibration with multiple Z steppers
+#
+# Copyright (C) 2018-2019  Kevin O'Connor <kevin@koconnor.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+import logging
+import mathutil
+from . import probe
+
+class ZAdjustHelper:
+    def __init__(self, config, z_count):
+        self.printer = config.get_printer()
+        self.name = config.get_name()
+        self.z_count = z_count
+        self.z_steppers = []
+        self.printer.register_event_handler("klippy:connect",
+                                            self.handle_connect)
+    def handle_connect(self):
+        kin = self.printer.lookup_object('toolhead').get_kinematics()
+        z_steppers = [s for s in kin.get_steppers() if s.is_active_axis('z')]
+        if len(z_steppers) != self.z_count:
+            raise self.printer.config_error(
+                "%s z_positions needs exactly %d items" % (
+                    self.name, len(z_steppers)))
+        if len(z_steppers) < 2:
+            raise self.printer.config_error(
+                "%s requires multiple z steppers" % (self.name,))
+        self.z_steppers = z_steppers
+    def adjust_steppers(self, adjustments, speed):
+        toolhead = self.printer.lookup_object('toolhead')
+        gcode = self.printer.lookup_object('gcode')
+        curpos = toolhead.get_position()
+        # Report on movements
+        stepstrs = ["%s = %.6f" % (s.get_name(), a)
+                    for s, a in zip(self.z_steppers, adjustments)]
+        msg = "Making the following Z adjustments:\n%s" % ("\n".join(stepstrs),)
+        gcode.respond_info(msg)
+        # Disable Z stepper movements
+        toolhead.flush_step_generation()
+        for s in self.z_steppers:
+            s.set_trapq(None)
+        # Move each z stepper (sorted from lowest to highest) until they match
+        positions = [(-a, s) for a, s in zip(adjustments, self.z_steppers)]
+        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]
+            toolhead.flush_step_generation()
+            stepper.set_trapq(toolhead.get_trapq())
+            curpos[2] = z_low + next_stepper_offset
+            try:
+                toolhead.move(curpos, speed)
+                toolhead.set_position(curpos)
+            except:
+                logging.exception("ZAdjustHelper adjust_steppers")
+                toolhead.flush_step_generation()
+                for s in self.z_steppers:
+                    s.set_trapq(toolhead.get_trapq())
+                raise
+        # Z should now be level - do final cleanup
+        last_stepper_offset, last_stepper = positions[-1]
+        toolhead.flush_step_generation()
+        last_stepper.set_trapq(toolhead.get_trapq())
+        curpos[2] += first_stepper_offset
+        toolhead.set_position(curpos)
+
+class ZAdjustStatus:
+    def __init__(self, printer):
+        self.applied = False
+        printer.register_event_handler("stepper_enable:motor_off",
+                                        self._motor_off)
+    def check_retry_result(self, retry_result):
+        if retry_result == "done":
+            self.applied = True
+        return retry_result
+    def reset(self):
+        self.applied = False
+    def get_status(self, eventtime):
+        return {'applied': self.applied}
+    def _motor_off(self, print_time):
+        self.reset()
+
+class RetryHelper:
+    def __init__(self, config, error_msg_extra = ""):
+        self.gcode = config.get_printer().lookup_object('gcode')
+        self.default_max_retries = config.getint("retries", 0, minval=0)
+        self.default_retry_tolerance = \
+            config.getfloat("retry_tolerance", 0., above=0.)
+        self.value_label = "Probed points range"
+        self.error_msg_extra = error_msg_extra
+    def start(self, gcmd):
+        self.max_retries = gcmd.get_int('RETRIES', self.default_max_retries,
+                                        minval=0, maxval=30)
+        self.retry_tolerance = gcmd.get_float('RETRY_TOLERANCE',
+                                              self.default_retry_tolerance,
+                                              minval=0.0, maxval=1.0)
+        self.current_retry = 0
+        self.previous = None
+        self.increasing = 0
+    def check_increase(self, error):
+        if self.previous and error > self.previous + 0.0000001:
+            self.increasing += 1
+        elif self.increasing > 0:
+            self.increasing -= 1
+        self.previous = error
+        return self.increasing > 1
+    def check_retry(self, z_positions):
+        if self.max_retries == 0:
+            return
+        error = round(max(z_positions) - min(z_positions),6)
+        self.gcode.respond_info(
+            "Retries: %d/%d %s: %0.6f tolerance: %0.6f" % (
+                self.current_retry, self.max_retries, self.value_label,
+                error, self.retry_tolerance))
+        if self.check_increase(error):
+            raise self.gcode.error("Retries aborting: %s is increasing. %s"
+                                   % (self.value_label, self.error_msg_extra))
+        if error <= self.retry_tolerance:
+            return "done"
+        self.current_retry += 1
+        if self.current_retry > self.max_retries:
+            raise self.gcode.error("Too many retries")
+        return "retry"
+
+class ZTilt:
+    def __init__(self, config):
+        self.printer = config.get_printer()
+        self.z_positions = config.getlists('z_positions', seps=(',', '\n'),
+                                           parser=float, count=2)
+        self.retry_helper = RetryHelper(config)
+        self.probe_helper = probe.ProbePointsHelper(config, self.probe_finalize)
+        self.probe_helper.minimum_points(2)
+        self.z_status = ZAdjustStatus(self.printer)
+        self.z_helper = ZAdjustHelper(config, len(self.z_positions))
+        # Register Z_TILT_ADJUST command
+        gcode = self.printer.lookup_object('gcode')
+        gcode.register_command('Z_TILT_ADJUST', self.cmd_Z_TILT_ADJUST,
+                               desc=self.cmd_Z_TILT_ADJUST_help)
+    cmd_Z_TILT_ADJUST_help = "Adjust the Z tilt"
+    def cmd_Z_TILT_ADJUST(self, gcmd):
+        self.z_status.reset()
+        self.retry_helper.start(gcmd)
+        self.probe_helper.start_probe(gcmd)
+    def probe_finalize(self, offsets, positions):
+        # Setup for coordinate descent analysis
+        z_offset = offsets[2]
+        logging.info("Calculating bed tilt with: %s", positions)
+        params = { 'x_adjust': 0., 'y_adjust': 0., 'z_adjust': z_offset }
+        # Perform coordinate descent
+        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)
+        # Apply results
+        speed = self.probe_helper.get_lift_speed()
+        logging.info("Calculated bed tilt parameters: %s", new_params)
+        x_adjust = new_params['x_adjust']
+        y_adjust = new_params['y_adjust']
+        z_adjust = (new_params['z_adjust'] - z_offset
+                    - x_adjust * offsets[0] - y_adjust * offsets[1])
+        adjustments = [x*x_adjust + y*y_adjust + z_adjust
+                       for x, y in self.z_positions]
+        self.z_helper.adjust_steppers(adjustments, speed)
+        return self.z_status.check_retry_result(
+            self.retry_helper.check_retry([p[2] for p in positions]))
+    def get_status(self, eventtime):
+            return self.z_status.get_status(eventtime)
+
+def load_config(config):
+    return ZTilt(config)