manual_probe: Add a helper script for performing manual Z probing

Add MANUAL_PROBE and Z_ENDSTOP_CALIBRATE commands.

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

3 files changed, 181 insertions, 0 deletions

diff --git a/docs/G-Codes.md b/docs/G-Codes.md
index 6848279f..60cb8038 100644
--- a/docs/G-Codes.md
+++ b/docs/G-Codes.md
@@ -118,6 +118,23 @@ The following standard commands are supported:
 - `STEPPER_BUZZ STEPPER=<config_name>`: Move the given stepper forward
   one mm and then backward one mm, repeated 10 times. This is a
   diagnostic tool to help verify stepper connectivity.
+- `MANUAL_PROBE [SPEED=<speed>]`: Run a helper script useful for
+  measuring the height of the nozzle at a given location. If SPEED is
+  specified, it sets the speed of TESTZ commands (the default is
+  5mm/s). During a manual probe, the following additional commands are
+  available:
+  - `ACCEPT`: This command accepts the current Z position and
+  concludes the manual probing tool.
+  - `ABORT`: This command terminates the manual probing tool.
+  - `TESTZ Z=<value>`: This command moves the nozzle up or down by the
+    amount specified in "value". For example, `TESTZ Z=-.1` would move
+    the nozzle down .1mm while `TESTZ Z=.1` would move the nozzle up
+    .1mm. The value may also be `+`, `-`, `++`, or `--` to move the
+    nozzle up or down an amount relative to previous attempts.
+- `Z_ENDSTOP_CALIBRATE [SPEED=<speed>]`: Run a helper script useful
+  for calibrating a Z position_endstop config setting. See the
+  MANUAL_PROBE command for details on the parameters and the
+  additional commands available while the tool is active.
 - `RESTART`: This will cause the host software to reload its config
   and perform an internal reset. This command will not clear error
   state from the micro-controller (see FIRMWARE_RESTART) nor will it
diff --git a/klippy/extras/manual_probe.py b/klippy/extras/manual_probe.py
new file mode 100644
index 00000000..a1fc63cf
--- /dev/null
+++ b/klippy/extras/manual_probe.py
@@ -0,0 +1,163 @@
+# Helper script for manual z height probing
+#
+# Copyright (C) 2019  Kevin O'Connor <kevin@koconnor.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+import logging, bisect
+import homing
+
+class ManualProbe:
+    def __init__(self, config):
+        self.printer = config.get_printer()
+        # Register commands
+        self.gcode = self.printer.lookup_object('gcode')
+        self.gcode.register_command('MANUAL_PROBE', self.cmd_MANUAL_PROBE,
+                                    desc=self.cmd_MANUAL_PROBE_help)
+        self.z_position_endstop = None
+        if config.has_section('stepper_z'):
+            zconfig = config.getsection('stepper_z')
+            if zconfig.get_prefix_options('position_endstop'):
+                self.z_position_endstop = zconfig.getfloat('position_endstop')
+                self.gcode.register_command(
+                    'Z_ENDSTOP_CALIBRATE', self.cmd_Z_ENDSTOP_CALIBRATE,
+                    desc=self.cmd_Z_ENDSTOP_CALIBRATE_help)
+    def manual_probe_finalize(self, kin_pos):
+        if kin_pos is not None:
+            self.gcode.respond_info("Z position is %.3f" % (kin_pos[2],))
+    cmd_MANUAL_PROBE_help = "Start manual probe helper script"
+    def cmd_MANUAL_PROBE(self, params):
+        ManualProbeHelper(self.printer, params, self.manual_probe_finalize)
+    def z_endstop_finalize(self, kin_pos):
+        if kin_pos is None:
+            return
+        z_pos = self.z_position_endstop - kin_pos[2]
+        self.gcode.respond_info(
+            "stepper_z: position_endstop: %.3f\n"
+            "The SAVE_CONFIG command will update the printer config file\n"
+            "with the above and restart the printer." % (z_pos,))
+        configfile = self.printer.lookup_object('configfile')
+        configfile.set('stepper_z', 'position_endstop', "%.3f" % (z_pos,))
+    cmd_Z_ENDSTOP_CALIBRATE_help = "Calibrate a Z endstop"
+    def cmd_Z_ENDSTOP_CALIBRATE(self, params):
+        ManualProbeHelper(self.printer, params, self.z_endstop_finalize)
+
+Z_BOB_MINIMUM = 0.500
+BISECT_MAX = 0.200
+
+# Helper script to determine a Z height
+class ManualProbeHelper:
+    def __init__(self, printer, params, finalize_callback):
+        self.printer = printer
+        self.finalize_callback = finalize_callback
+        self.gcode = self.printer.lookup_object('gcode')
+        self.toolhead = self.printer.lookup_object('toolhead')
+        self.speed = self.gcode.get_float("SPEED", params, 5.)
+        self.past_positions = []
+        self.last_toolhead_pos = self.last_kinematics_pos = None
+        # Register commands
+        try:
+            self.gcode.register_command('ACCEPT', self.cmd_ACCEPT,
+                                        desc=self.cmd_ACCEPT_help)
+        except self.gcode.error as e:
+            self.gcode.respond_error(
+                "Already in a manual Z probe. Use ABORT to abort it.")
+            self.finalize_callback(None)
+            return
+        self.gcode.register_command('NEXT', self.cmd_ACCEPT)
+        self.gcode.register_command('ABORT', self.cmd_ABORT,
+                                    desc=self.cmd_ABORT_help)
+        self.gcode.register_command('TESTZ', self.cmd_TESTZ,
+                                    desc=self.cmd_TESTZ_help)
+        self.gcode.respond_info(
+            "Starting manual Z probe. Use TESTZ to adjust position.\n"
+            "Finish with ACCEPT or ABORT command.")
+        self.report_z_status()
+    def get_kinematics_pos(self):
+        toolhead_pos = self.toolhead.get_position()
+        if toolhead_pos == self.last_toolhead_pos:
+            return self.last_kinematics_pos
+        self.toolhead.get_last_move_time()
+        kin_pos = self.toolhead.get_kinematics().calc_position()
+        self.last_toolhead_pos = toolhead_pos
+        self.last_kinematics_pos = kin_pos
+        return kin_pos
+    def move_z(self, z_pos):
+        curpos = self.toolhead.get_position()
+        try:
+            if curpos[2] - z_pos < Z_BOB_MINIMUM:
+                curpos[2] = z_pos + Z_BOB_MINIMUM
+                self.toolhead.move(curpos, self.speed)
+            curpos[2] = z_pos
+            self.toolhead.move(curpos, self.speed)
+        except homing.EndstopError as e:
+            self.finalize(False)
+            raise self.gcode.error(str(e))
+    def report_z_status(self, warn_no_change=False, prev_pos=None):
+        # Get position
+        kin_pos = self.get_kinematics_pos()
+        z_pos = kin_pos[2]
+        if warn_no_change and z_pos == prev_pos:
+            self.gcode.respond_info(
+                "WARNING: No change in position (reached stepper resolution)")
+        # Find recent positions that were tested
+        pp = self.past_positions
+        next_pos = bisect.bisect_left(pp, z_pos)
+        prev_pos = next_pos - 1
+        if next_pos < len(pp) and pp[next_pos] == z_pos:
+            next_pos += 1
+        prev_str = next_str = "??????"
+        if prev_pos >= 0:
+            prev_str = "%.3f" % (pp[prev_pos],)
+        if next_pos < len(pp):
+            next_str = "%.3f" % (pp[next_pos],)
+        # Find recent positions
+        self.gcode.respond_info("Z position: %s --> %.3f <-- %s" % (
+            prev_str, z_pos, next_str))
+    cmd_ACCEPT_help = "Accept the current Z position"
+    def cmd_ACCEPT(self, params):
+        self.finalize(True)
+    cmd_ABORT_help = "Abort manual Z probing tool"
+    def cmd_ABORT(self, params):
+        self.finalize(False)
+    cmd_TESTZ_help = "Move to new Z height"
+    def cmd_TESTZ(self, params):
+        # Store current position for later reference
+        kin_pos = self.get_kinematics_pos()
+        z_pos = kin_pos[2]
+        pp = self.past_positions
+        insert_pos = bisect.bisect_left(pp, z_pos)
+        if insert_pos >= len(pp) or pp[insert_pos] != z_pos:
+            pp.insert(insert_pos, z_pos)
+        # Determine next position to move to
+        req = self.gcode.get_str("Z", params)
+        if req in ('+', '++'):
+            check_z = 9999999999999.9
+            if insert_pos < len(self.past_positions) - 1:
+                check_z = self.past_positions[insert_pos + 1]
+            if req == '+':
+                check_z = (check_z + z_pos) / 2.
+            next_z_pos = min(check_z, z_pos + BISECT_MAX)
+        elif req in ('-', '--'):
+            check_z = -9999999999999.9
+            if insert_pos > 0:
+                check_z = self.past_positions[insert_pos - 1]
+            if req == '-':
+                check_z = (check_z + z_pos) / 2.
+            next_z_pos = max(check_z, z_pos - BISECT_MAX)
+        else:
+            next_z_pos = z_pos + self.gcode.get_float("Z", params)
+        # Move to given position and report it
+        self.move_z(next_z_pos)
+        self.report_z_status(next_z_pos != z_pos, z_pos)
+    def finalize(self, success):
+        self.gcode.register_command('ACCEPT', None)
+        self.gcode.register_command('NEXT', None)
+        self.gcode.register_command('ABORT', None)
+        self.gcode.register_command('TESTZ', None)
+        kin_pos = None
+        if success:
+            kin_pos = self.get_kinematics_pos()
+        self.finalize_callback(kin_pos)
+
+def load_config(config):
+    return ManualProbe(config)
diff --git a/klippy/toolhead.py b/klippy/toolhead.py
index cae280c7..9c7369e8 100644
--- a/klippy/toolhead.py
+++ b/klippy/toolhead.py
@@ -239,6 +239,7 @@ class ToolHead:
         self.move_queue.set_flush_time(self.buffer_time_high)
         self.printer.try_load_module(config, "idle_timeout")
         self.printer.try_load_module(config, "statistics")
+        self.printer.try_load_module(config, "manual_probe")
         # Setup iterative solver
         ffi_main, ffi_lib = chelper.get_ffi()
         self.cmove = ffi_main.gc(ffi_lib.move_alloc(), ffi_lib.free)