screws_tilt_adjust: Add new screws_tilt_adjust tool (#1367)

Signed-off-by: Rui Caridade <rui.mcbc@gmail.com>

8 files changed, 336 insertions, 3 deletions

diff --git a/config/example-delta.cfg b/config/example-delta.cfg
index f54f0477..71ad750a 100644
--- a/config/example-delta.cfg
+++ b/config/example-delta.cfg
@@ -126,6 +126,9 @@ radius: 50
 #samples: 1
 #   The number of times to probe each point.  The probed z-values will
 #   be averaged. The default is to probe 1 time.
+#samples_result: average
+#   One can choose median or average between probes samples
+#   The default is average.
 #sample_retract_dist: 2.0
 #   The distance (in mm) to retract between each sample if sampling
 #   more than once. The default is 2mm.
diff --git a/config/example-extras.cfg b/config/example-extras.cfg
index a9a8591d..59a82c18 100644
--- a/config/example-extras.cfg
+++ b/config/example-extras.cfg
@@ -115,6 +115,9 @@
 #samples: 1
 #   The number of times to probe each point.  The probed z-values
 #   will be averaged.  The default is to probe 1 time.
+#samples_result: average
+#   One can choose median or average between probes samples
+#   The default is average.
 #sample_retract_dist: 2.0
 #   The distance (in mm) to retract between each sample if
 #   sampling more than once.  Default is 2mm.
@@ -157,6 +160,9 @@
 #samples: 1
 #   The number of times to probe each point.  The probed z-values
 #   will be averaged.  The default is to probe 1 time.
+#samples_result: average
+#   One can choose median or average between probes samples
+#   The default is average.
 #sample_retract_dist: 2.0
 #   The distance (in mm) to retract between each sample if
 #   sampling more than once.  Default is 2mm.
@@ -258,6 +264,47 @@
 #   The speed (in mm/s) when moving from a horizontal_move_z position
 #   to a probe_height position. The default is 5.
 
+# Tool to help adjust bed screws tilt using Z probe. One may define a
+# [screws_tilt_adjust] config section to enable a SCREWS_TILT_CALCULATE
+# g-code command.
+#[screws_tilt_adjust]
+#screw1: 100,100
+#   The X,Y coordinate of the first bed leveling screw. This is a
+#   position to command the nozzle to that is directly above the bed
+#   screw (or as close as possible while still being above the bed).
+#   This is the base screw used in calculations.
+#   This parameter must be provided.
+#screw1_name: front screw
+#   An arbitrary name for the given screw. This name is displayed when
+#   the helper script runs. The default is to use a name based upon
+#   the screw XY location.
+#screw2:
+#screw2_name:
+#...
+#   Additional bed leveling screws. At least two screws must be
+#   defined.
+#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.
+#samples: 1
+#   The number of times to probe each point.  The probed z-values
+#   will be averaged.  The default is to probe 1 time.
+#sample_retract_dist: 2.0
+#   The distance (in mm) to retract between each sample if
+#   sampling more than once.  Default is 2mm.
+#samples_result: median
+#   One can choose median or average between screw probes
+#   The default is average.
+#screw_thread: CW-M3
+#   The type of screw used for bed level, M3, M4 or M5 and the
+#   direction of the knob used to level the bed, clockwise decrease
+#   counter-clockwise decrease.
+#   Accepted values: CW-M3, CCW-M3, CW-M4, CCW-M4, CW-M5, CCW-M5.
+#   Default value is CW-M3, most printers use an M3 screw and
+#   turning the knob clockwise decrease distance.
 
 # Multiple Z stepper tilt adjustment. This feature enables independent
 # adjustment of multiple z steppers (see stepper_z1 section below) to
@@ -284,6 +331,9 @@
 #samples: 1
 #   The number of times to probe each point.  The probed z-values
 #   will be averaged.  The default is to probe 1 time.
+#samples_result: average
+#   One can choose median or average between probes samples
+#   The default is average.
 #sample_retract_dist: 2.0
 #   The distance (in mm) to retract between each sample if
 #   sampling more than once.  Default is 2mm.
@@ -326,6 +376,9 @@
 #   just prior to starting a probe operation. The default is 5
 #samples: 1
 #   Number of probe samples per point. The defaut is 1
+#samples_result: average
+#   One can choose median or average between probes samples
+#   The default is average.
 #sample_retract_dist: 2.0
 #   Distance in mm to retract the probe between samples. Default is 2.
 
diff --git a/docs/G-Codes.md b/docs/G-Codes.md
index a3d41cd3..993770d0 100644
--- a/docs/G-Codes.md
+++ b/docs/G-Codes.md
@@ -274,6 +274,16 @@ section is enabled:
   the bed screws so that the bed is a constant distance from the
   nozzle.
 
+## Bed Screws Tilt adjust Helper
+
+The following commands are available when the "screws_tilt_adjust"
+config section is enabled:
+- `SCREWS_TILT_CALCULATE`: This command will invoke the bed screws
+  adjustment tool. It will command the nozzle to different locations
+  (as defined in the config file) probing the z height and calculate
+  the number of knob turns to adjust the bed level.
+  IMPORTANT: You MUST always do a G28 before using this command.
+
 ## Z Tilt
 
 The following commands are available when the "z_tilt" config section
diff --git a/docs/Manual_Level.md b/docs/Manual_Level.md
index f9636932..687fd27c 100644
--- a/docs/Manual_Level.md
+++ b/docs/Manual_Level.md
@@ -133,3 +133,57 @@ prompt for coarse adjustments directly above each screw position, and
 once those are accepted, it will prompt for fine adjustments at the
 additional locations. Continue to use `ACCEPT` and `ADJUSTED` at each
 position.
+
+# Adjusting bed leveling screws using the bed probe
+
+This is another way to calibrate the bed level using the bed probe. To
+use it you must have a Z probe (BL Touch, Inductive sensor, etc).
+
+To enable this feature, one would determine the additional nozzle
+coordinates near the screws and add them to the config file. For example,
+it might look like:
+
+```
+[screws_tilt_adjust]
+screw1: -5,30
+screw1_name: front left screw
+screw2: 155,30
+screw2_name: front right screw
+screw3: 155,190
+screw3_name: rear right screw
+screw4: -5,190
+screw4_name: rear left screw
+horizontal_move_z: 10.
+speed: 50.
+samples: 3
+sample_retract_dist: 2.
+samples_result: median
+screw_thread: CW-M3
+```
+
+One can indicate the number of times to repeat probe on each screw and
+if the value is the median or the average read probe.
+
+The screw1 is always the reference point for the others, so the system
+assumes that screw1 is in the correct height.
+Then to use this feature you must preform every time `G28` before
+`SCREWS_TILT_CALCULATE` and after bed is probed you get an output like this:
+```
+Send: G28
+Recv: ok
+Send: SCREWS_TILT_CALCULATE
+Recv: // front left screw (Base): X -5.0, Y 30.0, Z 2.48750
+Recv: // front right screw : X 155.0, Y 30.0, Z 2.36000 : Adjust -> CW 01:15
+Recv: // rear right screw : X 155.0, Y 190.0, Z 2.71500 : Adjust -> CCW 00:50
+Recv: // read left screw : X -5.0, Y 190.0, Z 2.47250 : Adjust -> CW 00:02
+Recv: ok
+```
+This means that:
+
+    - front left screw is the reference point you must not change it.
+    - front right screw must be turned clockwise 1 full turn and a quarter turn
+    - rear right screw must be turned counter-clockwise 50 minutes
+    - read left screw must be turned clockwise 2 minutes (not need it's ok)
+
+Repeat the process several times until you get a good level bed, normally when
+all adjusts are below 6 minutes.
diff --git a/klippy/extras/probe.py b/klippy/extras/probe.py
index aa5f1d05..83871d15 100644
--- a/klippy/extras/probe.py
+++ b/klippy/extras/probe.py
@@ -240,6 +240,9 @@ class ProbePointsHelper:
         self.samples = config.getint('samples', 1, minval=1)
         self.sample_retract_dist = config.getfloat(
             'sample_retract_dist', 2., above=0.)
+        self.samples_result = config.getchoice('samples_result',
+                                               {'median': 0, 'average': 1},
+                                               default='average')
         # Internal probing state
         self.results = []
         self.busy = self.manual_probe = False
@@ -295,9 +298,26 @@ class ProbePointsHelper:
             if i < self.samples - 1:
                 # retract
                 self._lift_z(self.sample_retract_dist, add=True)
-        avg_pos = [sum([pos[i] for pos in positions]) / self.samples
-                   for i in range(3)]
-        self.results.append(avg_pos)
+        if self.samples_result == 1:
+            # Calculate Average
+            calculated_value = [sum([pos[i] for pos in positions]) /
+                                self.samples for i in range(3)]
+        else:
+            # Calculate Median
+            sorted_z_positions = sorted([position[2]
+                                         for position in positions])
+            middle = self.samples // 2
+            if (self.samples & 1) == 1:
+                # odd number of samples
+                median = sorted_z_positions[middle]
+            else:
+                # even number of samples
+                median = (sorted_z_positions[middle] +
+                          sorted_z_positions[middle - 1]) / 2
+            calculated_value = [positions[0][0],
+                                positions[0][1],
+                                median]
+        self.results.append(calculated_value)
     def start_probe(self, params):
         # Lookup objects
         self.toolhead = self.printer.lookup_object('toolhead')
diff --git a/klippy/extras/screws_tilt_adjust.py b/klippy/extras/screws_tilt_adjust.py
new file mode 100644
index 00000000..663127c2
--- /dev/null
+++ b/klippy/extras/screws_tilt_adjust.py
@@ -0,0 +1,97 @@
+# Helper script to adjust bed screws tilt using Z probe
+#
+# Copyright (C) 2019  Rui Caridade <rui.mcbc@gmail.com>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+import math
+import probe
+
+def parse_coord(config, param):
+    pair = config.get(param).strip().split(',', 1)
+    try:
+        return (float(pair[0]), float(pair[1]))
+    except:
+        raise config.error("%s:%s needs to be an x,y coordinate" % (
+            config.get_name(), param))
+
+class ScrewsTiltAdjust:
+    def __init__(self, config):
+        self.config = config
+        self.printer = config.get_printer()
+        self.screws = []
+        # Verify that a probe exists
+        try:
+            self.printer.lookup_object("probe")
+        except:
+            raise self.gcode.error("Error: you must have a probe on "
+                                   "your config file.")
+        # Read config
+        for i in range(99):
+            prefix = "screw%d" % (i + 1,)
+            if config.get(prefix, None) is None:
+                break
+            screw_coord = parse_coord(config, prefix)
+            screw_name = "screw at %.3f,%.3f" % screw_coord
+            screw_name = config.get(prefix + "_name", screw_name)
+            self.screws.append((screw_coord, screw_name))
+        if len(self.screws) < 3:
+            raise config.error("screws_tilt_adjust: Must have "
+                               "at least three screws")
+        self.threads = {'CW-M3': 0, 'CCW-M3': 1, 'CW-M4': 2, 'CCW-M4': 3,
+                        'CW-M5': 4, 'CCW-M5': 5}
+        self.thread = config.getchoice('screw_thread', self.threads,
+                                       default='CW-M3')
+        # Initialize ProbePointsHelper
+        points = [coord for coord, name in self.screws]
+        self.probe_helper = probe.ProbePointsHelper(self.config,
+                                                    self.probe_finalize,
+                                                    default_points=points)
+        # Register command
+        self.gcode = self.printer.lookup_object('gcode')
+        self.gcode.register_command("SCREWS_TILT_CALCULATE",
+                                    self.cmd_SCREWS_TILT_CALCULATE,
+                                    desc=self.cmd_SCREWS_TILT_CALCULATE_help)
+    cmd_SCREWS_TILT_CALCULATE_help = "Tool to help adjust bed leveling " \
+                                     "screws by calculating the number " \
+                                     "of turns to level it."
+
+    def cmd_SCREWS_TILT_CALCULATE(self, params):
+        self.probe_helper.start_probe(params)
+
+    def probe_finalize(self, offsets, positions):
+        # Factors used for CW-M3, CCW-M3, CW-M4, CCW-M4, CW-M5 and CCW-M5
+        threads_factor = {0: 0.5, 1: 0.5, 2: 0.7, 3: 0.7, 4: 0.8, 5: 0.8}
+        # Process the read Z values and
+        for i, screw in enumerate(self.screws):
+            if i == 0:
+                # First screw is the base position used for comparison
+                z_base = positions[i][2]
+                coord_base, name_base = screw
+                # Show the results
+                self.gcode.respond_info("%s (Base): X %.1f, Y %.1f, Z %.5f" %
+                                        (name_base, coord_base[0],
+                                         coord_base[1], z_base))
+            else:
+                # Calculate how knob must me adjusted for other positions
+                z = positions[i][2]
+                coord, name = screw
+                diff = z_base - z
+                if abs(diff) < 0.001:
+                    adjust = 0
+                else:
+                    adjust = diff / threads_factor.get(self.thread, 0.5)
+                if (self.thread & 1) == 1:
+                    sign = "CW" if adjust < 0 else "CCW"
+                else:
+                    sign = "CCW" if adjust < 0 else "CW"
+                full_turns = math.trunc(adjust)
+                decimal_part = adjust - full_turns
+                minutes = round(decimal_part * 60, 0)
+                # Show the results
+                self.gcode.respond_info("%s : X %.1f, Y %.1f, Z %.5f : "
+                                        "Adjust -> %s %02d:%02d" %
+                                        (name, coord[0], coord[1], z, sign,
+                                         abs(full_turns), abs(minutes)))
+
+def load_config(config):
+    return ScrewsTiltAdjust(config)
diff --git a/test/klippy/screws_tilt_adjust.cfg b/test/klippy/screws_tilt_adjust.cfg
new file mode 100644
index 00000000..7b29e6ec
--- /dev/null
+++ b/test/klippy/screws_tilt_adjust.cfg
@@ -0,0 +1,89 @@
+                   # Test config for bed screws tool with bltouch
+[stepper_x]
+step_pin: ar54
+dir_pin: ar55
+enable_pin: !ar38
+step_distance: .0125
+endstop_pin: ^ar3
+position_endstop: 0
+position_max: 200
+homing_speed: 50
+
+[stepper_y]
+step_pin: ar60
+dir_pin: !ar61
+enable_pin: !ar56
+step_distance: .0125
+endstop_pin: ^ar14
+position_endstop: 0
+position_max: 200
+homing_speed: 50
+
+[stepper_z]
+step_pin: ar46
+dir_pin: ar48
+enable_pin: !ar62
+step_distance: .0025
+endstop_pin: probe:z_virtual_endstop
+position_max: 200
+
+[extruder]
+step_pin: ar26
+dir_pin: ar28
+enable_pin: !ar24
+step_distance: .002
+nozzle_diameter: 0.400
+filament_diameter: 1.750
+heater_pin: ar10
+sensor_type: EPCOS 100K B57560G104F
+sensor_pin: analog13
+control: pid
+pid_Kp: 22.2
+pid_Ki: 1.08
+pid_Kd: 114
+min_temp: 0
+max_temp: 250
+
+[heater_bed]
+heater_pin: ar8
+sensor_type: EPCOS 100K B57560G104F
+sensor_pin: analog14
+control: watermark
+min_temp: 0
+max_temp: 130
+
+[bltouch]
+sensor_pin: ar30
+control_pin: ar32
+z_offset: 1.15
+
+[bed_mesh]
+min_point: 10,10
+max_point: 180,180
+
+[mcu]
+serial: /dev/ttyACM0
+pin_map: arduino
+
+[printer]
+kinematics: cartesian
+max_velocity: 300
+max_accel: 3000
+max_z_velocity: 5
+max_z_accel: 100
+
+[screws_tilt_adjust]
+screw1: 10,30
+screw1_name: front left screw
+screw2: 155,30
+screw2_name: front right screw
+screw3: 155,190
+screw3_name: rear right screw
+screw4: 10,190
+screw4_name: read left screw
+horizontal_move_z: 10.
+speed: 50.
+samples: 3
+sample_retract_dist: 2.
+samples_result: median
+screw_thread: CW-M3
diff --git a/test/klippy/screws_tilt_adjust.test b/test/klippy/screws_tilt_adjust.test
new file mode 100644
index 00000000..62b1d53a
--- /dev/null
+++ b/test/klippy/screws_tilt_adjust.test
@@ -0,0 +1,7 @@
+# Test case for bed screws tilt helper tool
+CONFIG screws_tilt_adjust.cfg
+DICTIONARY atmega2560-16mhz.dict
+
+# Simple script to test
+G28
+SCREWS_TILT_CALCULATE