bed_level: improve screw detection (#908)

* Use max_distance for fitting screws to buttons

The max distance is at most 1/3 of the diagonal,
depending on how many screws are there in horizontal or
vertical orientation.

This is a safe guard to prevent assigning screws to buttons
that would confuse the users, for very skewed screw configs.

use a smaller button scale fixes #891

Co-authored-by: alfrix <alfredomonclus@gmail.com>

Co-authored-by: czaky <jendrzej.w@lczak.net>

panels/bed_level.py

@@ -1,5 +1,6 @@
 import logging
 import re
+import math
 
 import gi
 
@@ -14,6 +15,29 @@ def create_panel(*args):
     return BedLevelPanel(*args)
 
 
+# Find the screw closest to the point,
+# but return None if the distance is above max_distance.
+# If remove is set to true, the screw is also removed
+# from the list of passed in screws.
+def find_closest(screws, point, max_distance, remove=False):
+    if len(screws) == 0:
+        return None
+    closest = screws[0]
+    min_distance = math.dist(closest, point)
+    for screw in screws[1:]:
+        distance = math.dist(screw, point)
+        if distance < min_distance:
+            closest = screw
+            min_distance = distance
+
+    if min_distance > max_distance:
+        return None
+
+    if remove:
+        screws.remove(closest)
+    return closest
+
+
 class BedLevelPanel(ScreenPanel):
 
     def __init__(self, screen, title):
@@ -49,7 +73,8 @@ class BedLevelPanel(ScreenPanel):
                     self.y_offset = round(float(probe['y_offset']), 1)
                 logging.debug(f"offset X: {self.x_offset} Y: {self.y_offset}")
             # bed_screws uses NOZZLE positions
-            # screws_tilt_adjust uses PROBE positions and to be offseted for the buttons to work equal to bed_screws
+            # screws_tilt_adjust uses PROBE positions and
+            # to be offseted for the buttons to work equal to bed_screws
             new_screws = [
                 [round(screw[0] + self.x_offset, 1), round(screw[1] + self.y_offset, 1)]
                 for screw in self.screws
@@ -72,35 +97,96 @@ class BedLevelPanel(ScreenPanel):
         max_x = max(x_positions)
         min_y = min(y_positions)
         max_y = max(y_positions)
+        max_distance = math.ceil(
+            math.dist((min_x, min_y), (max_x, max_y))
+            / min(self.x_cnt, self.y_cnt, 3)
+        )
 
-        fl = [min_x, min_y]
+        logging.debug(f"Using max_distance: {max_distance} to fit: {len(self.screws)} screws.")
-        bl = [min_x, max_y]
+
-        br = [max_x, max_y]
+        remaining_screws = self.screws[:]
-        fr = [max_x, min_y]
+        remaining_positions = []
+
+        fl = find_closest(remaining_screws, (min_x, min_y), max_distance, remove=True)
+        bl = find_closest(remaining_screws, (min_x, max_y), max_distance, remove=True)
+        br = find_closest(remaining_screws, (max_x, max_y), max_distance, remove=True)
+        fr = find_closest(remaining_screws, (max_x, min_y), max_distance, remove=True)
 
         if self.x_cnt == 3:
             mid_x = [x for x in list(zip(*self.screws))[0] if x not in (min_x, max_x)][0]
-            fm = [mid_x, min_y]
+            fm = find_closest(remaining_screws, (mid_x, min_y), max_distance, remove=True)
-            bm = [mid_x, max_y]
+            bm = find_closest(remaining_screws, (mid_x, max_y), max_distance, remove=True)
+            fmp = bmp = None
         else:
+            mid_x = round((min_x + max_x) / 2)
+            fmp = (mid_x, min_y)
+            bmp = (mid_x, max_y)
+            remaining_positions.extend([fmp, bmp])
             fm = bm = None
+
         if self.y_cnt == 3:
             mid_y = [y for y in list(zip(*self.screws))[1] if y not in (min_y, max_y)][0]
-            lm = [min_x, mid_y]
+            lm = find_closest(remaining_screws, (min_x, mid_y), max_distance, remove=True)
-            rm = [max_x, mid_y]
+            rm = find_closest(remaining_screws, (max_x, mid_y), max_distance, remove=True)
+            lmp = rmp = None
         else:
+            mid_y = round((min_y + max_y) / 2)
+            lmp = (min_x, mid_y)
+            rmp = (max_x, mid_y)
+            remaining_positions.extend([lmp, rmp])
             lm = rm = None
 
-        logging.debug(f"Using {len(self.screws)}-screw locations [x,y] [{self.x_cnt}x{self.y_cnt}]")
+        remaining_count = len(remaining_screws)
+        while remaining_count > 0:
+            logging.debug(f"Screws remaining: {remaining_screws}")
+            logging.debug(f"Positions remaining: {remaining_positions}")
+            for screw in remaining_screws:
+                pos = find_closest(remaining_positions, screw, max_distance)
+                closest = find_closest(remaining_screws, pos, max_distance)
+                if closest != screw:
+                    continue
+                elif pos == fmp:
+                    fm = screw
+                elif pos == bmp:
+                    bm = screw
+                elif pos == lmp:
+                    lm = screw
+                elif pos == rmp:
+                    rm = screw
 
-        self.buttons['bl'] = self._gtk.Button("bed-level-t-l", scale=2.5)
+                logging.debug(f"Fitted screw {screw} close to {pos}")
-        self.buttons['br'] = self._gtk.Button("bed-level-t-r", scale=2.5)
+                remaining_positions.remove(pos)
-        self.buttons['fl'] = self._gtk.Button("bed-level-b-l", scale=2.5)
+                remaining_screws.remove(screw)
-        self.buttons['fr'] = self._gtk.Button("bed-level-b-r", scale=2.5)
+
-        self.buttons['lm'] = self._gtk.Button("bed-level-l-m", scale=2.5)
+            if remaining_count == len(remaining_screws):
-        self.buttons['rm'] = self._gtk.Button("bed-level-r-m", scale=2.5)
+                logging.warning(
-        self.buttons['fm'] = self._gtk.Button("bed-level-b-m", scale=2.5)
+                    f"Remaining screws: {remaining_screws}" +
-        self.buttons['bm'] = self._gtk.Button("bed-level-t-m", scale=2.5)
+                    f" don't fit to positions: {remaining_positions}")
+                break
+            remaining_count = len(remaining_screws)
+
+        if remaining_count == 0:
+            # All screws fitted.
+            if self.x_cnt > 3:
+                self.x_cnt = 3
+            if self.y_cnt > 3:
+                self.y_cnt = 3
+        else:
+            logging.debug(f"Screws not used: {remaining_screws}")
+
+        used_screw_cnt = len(self.screws) - len(remaining_screws)
+        logging.debug(f"Using {used_screw_cnt}-screw locations [x,y] [{self.x_cnt}x{self.y_cnt}]")
+
+        button_scale = 2
+
+        self.buttons['bl'] = self._gtk.Button("bed-level-t-l", scale=button_scale)
+        self.buttons['br'] = self._gtk.Button("bed-level-t-r", scale=button_scale)
+        self.buttons['fl'] = self._gtk.Button("bed-level-b-l", scale=button_scale)
+        self.buttons['fr'] = self._gtk.Button("bed-level-b-r", scale=button_scale)
+        self.buttons['lm'] = self._gtk.Button("bed-level-l-m", scale=button_scale)
+        self.buttons['rm'] = self._gtk.Button("bed-level-r-m", scale=button_scale)
+        self.buttons['fm'] = self._gtk.Button("bed-level-b-m", scale=button_scale)
+        self.buttons['bm'] = self._gtk.Button("bed-level-t-m", scale=button_scale)
 
         valid_positions = True
         if self.ks_printer_cfg is not None:
@@ -261,9 +347,16 @@ class BedLevelPanel(ScreenPanel):
         for key, value in self.screw_dict.items():
             self.buttons[key].set_label(f"{value}")
 
-    def go_to_position(self, widget, position):
+    def home(self):
+        # Test if all axes have been homed. Home if necessary.
         if self._printer.get_stat("toolhead", "homed_axes") != "xyz":
             self._screen._ws.klippy.gcode_script(KlippyGcodes.HOME)
+            # do Z_TILT_CALIBRATE if applicable.
+            if self._printer.config_section_exists("z_tilt"):
+                self._screen._ws.klippy.gcode_script(KlippyGcodes.Z_TILT)
+
+    def go_to_position(self, widget, position):
+        self.home()
         logging.debug(f"Going to position: {position}")
         script = [
             f"{KlippyGcodes.MOVE_ABSOLUTE}",
@@ -284,7 +377,8 @@ class BedLevelPanel(ScreenPanel):
     def process_busy(self, busy):
         for button in self.buttons:
             if button == "screws":
-                self.buttons[button].set_sensitive(self._printer.config_section_exists("screws_tilt_adjust")
+                self.buttons[button].set_sensitive(
+                    self._printer.config_section_exists("screws_tilt_adjust")
                     and (not busy))
                 continue
             self.buttons[button].set_sensitive((not busy))
@@ -347,8 +441,7 @@ class BedLevelPanel(ScreenPanel):
         return sorted(screws, key=lambda s: (float(s[1]), float(s[0])))
 
     def screws_tilt_calculate(self, widget):
-        if self._printer.get_stat("toolhead", "homed_axes") != "xyz":
+        self.home()
-            self._screen._ws.klippy.gcode_script(KlippyGcodes.HOME)
         self.response_count = 0
         self.buttons['screws'].set_sensitive(False)
         self._screen._ws.klippy.gcode_script("SCREWS_TILT_CALCULATE")