From 60372fd0cf58a61dc3ced5efca0ba9b9c43fcb27 Mon Sep 17 00:00:00 2001 From: Arksine <9563098+Arksine@users.noreply.github.com> Date: Sat, 3 Apr 2021 08:43:45 -0400 Subject: [PATCH] bed_mesh: introduce "faulty_regions" option Users may define "faulty regions", locations within the mesh where a probed value is unreliable. When bed mesh generates points it will substitute points in faulty regions with up to 4 points nearest to the region. After calibration is complete the Z values at these points will be averaged and assigned to the original value inside the faulty region. Signed-off-by: Eric Callahan --- klippy/extras/bed_mesh.py | 172 ++++++++++++++++++++++++++++++++++++-- 1 file changed, 167 insertions(+), 5 deletions(-) diff --git a/klippy/extras/bed_mesh.py b/klippy/extras/bed_mesh.py index 1c25a1ea..11a8a748 100644 --- a/klippy/extras/bed_mesh.py +++ b/klippy/extras/bed_mesh.py @@ -21,6 +21,12 @@ class BedMeshError(Exception): def isclose(a, b, rel_tol=1e-09, abs_tol=0.0): return abs(a-b) <= max(rel_tol * max(abs(a), abs(b)), abs_tol) +# return true if a coordinate is within the region +# specified by min_c and max_c +def within(coord, min_c, max_c, tol=0.0): + return (max_c[0] + tol) >= coord[0] >= (min_c[0] - tol) and \ + (max_c[1] + tol) >= coord[1] >= (min_c[1] - tol) + # Constrain value between min and max def constrain(val, min_val, max_val): return min(max_val, max(min_val, val)) @@ -240,6 +246,8 @@ class BedMeshCalibrate: self.mesh_min = self.mesh_max = (0., 0.) self.relative_reference_index = config.getint( 'relative_reference_index', None) + self.faulty_regions = [] + self.substituted_indices = collections.OrderedDict() self.orig_config['rri'] = self.relative_reference_index self.bedmesh = bedmesh self.mesh_config = collections.OrderedDict() @@ -247,7 +255,7 @@ class BedMeshCalibrate: self._generate_points(config.error) self.orig_points = self.points self.probe_helper = probe.ProbePointsHelper( - config, self.probe_finalize, self.points) + config, self.probe_finalize, self._get_adjusted_points()) self.probe_helper.minimum_points(3) self.probe_helper.use_xy_offsets(True) self.gcode = self.printer.lookup_object('gcode') @@ -297,6 +305,45 @@ class BedMeshCalibrate: (self.origin[0] + pos_x, self.origin[1] + pos_y)) pos_y += y_dist self.points = points + if not self.faulty_regions: + return + # Check to see if any points fall within faulty regions + last_y = self.points[0][1] + is_reversed = False + for i, coord in enumerate(self.points): + if not isclose(coord[1], last_y): + is_reversed = not is_reversed + last_y = coord[1] + adj_coords = [] + for min_c, max_c in self.faulty_regions: + if within(coord, min_c, max_c, tol=.00001): + # Point lies within a faulty region + adj_coords = [ + (min_c[0], coord[1]), (coord[0], min_c[1]), + (coord[0], max_c[1]), (max_c[0], coord[1])] + if is_reversed: + # Swap first and last points for zig-zag pattern + first = adj_coords[0] + adj_coords[0] = adj_coords[-1] + adj_coords[-1] = first + break + if not adj_coords: + # coord is not located within a faulty region + continue + valid_coords = [] + for ac in adj_coords: + # make sure that coordinates are within the mesh boundary + if self.radius is None: + if within(ac, (min_x, min_y), (max_x, max_y), .000001): + valid_coords.append(ac) + else: + dist_from_origin = math.sqrt(ac[0]*ac[0] + ac[1]*ac[1]) + if dist_from_origin <= self.radius: + valid_coords.append(ac) + if not valid_coords: + raise error("bed_mesh: Unable to generate coordinates" + " for faulty region at index: %d" % (i)) + self.substituted_indices[i] = valid_coords def print_generated_points(self, print_func): x_offset = y_offset = 0. probe = self.printer.lookup_object('probe', None) @@ -314,6 +361,12 @@ class BedMeshCalibrate: print_func( "bed_mesh: relative_reference_index %d is (%.2f, %.2f)" % (rri, self.points[rri][0], self.points[rri][1])) + if self.substituted_indices: + print_func("bed_mesh: faulty region points") + for i, v in self.substituted_indices.items(): + pt = self.points[i] + print_func("%d (%.2f, %.2f), substituted points: %s" + % (i, pt[0], pt[1], repr(v))) def _init_mesh_config(self, config): mesh_cfg = self.mesh_config orig_cfg = self.orig_config @@ -352,6 +405,41 @@ class BedMeshCalibrate: config.get('algorithm', 'lagrange').strip().lower() orig_cfg['tension'] = mesh_cfg['tension'] = config.getfloat( 'bicubic_tension', .2, minval=0., maxval=2.) + for i in list(range(1, 100, 1)): + min_opt = "faulty_region_%d_min" % (i,) + max_opt = "faulty_region_%d_max" % (i,) + if config.get(min_opt, None) is None: + break + start = parse_pair(config, (min_opt,)) + end = parse_pair(config, (max_opt,)) + # Validate the corners. If necessary reorganize them. + # c1 = min point, c3 = max point + # c4 ---- c3 + # | | + # c1 ---- c2 + c1 = [min([s, e]) for s, e in zip(start, end)] + c3 = [max([s, e]) for s, e in zip(start, end)] + c2 = [c1[0], c3[1]] + c4 = [c3[0], c1[1]] + # Check for overlapping regions + for j, (prev_c1, prev_c3) in enumerate(self.faulty_regions): + prev_c2 = [prev_c1[0], prev_c3[1]] + prev_c4 = [prev_c3[0], prev_c1[1]] + # Validate that no existing corner is within the new region + for coord in [prev_c1, prev_c2, prev_c3, prev_c4]: + if within(coord, c1, c3): + raise config.error( + "bed_mesh: Existing faulty_region_%d %s overlaps " + "added faulty_region_%d %s" + % (j, repr([prev_c1, prev_c3]), i, repr([c1, c3]))) + # Validate that no new corner is within an existing region + for coord in [c1, c2, c3, c4]: + if within(coord, prev_c1, prev_c3): + raise config.error( + "bed_mesh: Added faulty_region_%d %s overlaps " + "existing faulty_region_%d %s" + % (i, repr([c1, c3]), j, repr([prev_c1, prev_c3]))) + self.faulty_regions.append((c1, c3)) self._verify_algorithm(config.error) def _verify_algorithm(self, error): params = self.mesh_config @@ -445,7 +533,8 @@ class BedMeshCalibrate: self._generate_points(gcmd.error) gcmd.respond_info("Generating new points...") self.print_generated_points(gcmd.respond_info) - self.probe_helper.update_probe_points(self.points, 3) + pts = self._get_adjusted_points() + self.probe_helper.update_probe_points(pts, 3) msg = "relative_reference_index: %s\n" % \ (self.relative_reference_index) msg += "\n".join(["%s: %s" % (k, v) for k, v @@ -453,8 +542,21 @@ class BedMeshCalibrate: logging.info("Updated Mesh Configuration:\n" + msg) else: self.points = self.orig_points - self.probe_helper.update_probe_points(self.points, 3) - + pts = self._get_adjusted_points() + self.probe_helper.update_probe_points(pts, 3) + def _get_adjusted_points(self): + if not self.substituted_indices: + return self.points + adj_pts = [] + last_index = 0 + for i, pts in self.substituted_indices.items(): + adj_pts.extend(self.points[last_index:i]) + adj_pts.extend(pts) + # Add one to the last index to skip the point + # we are replacing + last_index = i + 1 + adj_pts.extend(self.points[last_index:]) + return adj_pts cmd_BED_MESH_CALIBRATE_help = "Perform Mesh Bed Leveling" def cmd_BED_MESH_CALIBRATE(self, gcmd): self.bedmesh.set_mesh(None) @@ -462,7 +564,7 @@ class BedMeshCalibrate: self.probe_helper.start_probe(gcmd) def probe_finalize(self, offsets, positions): x_offset, y_offset, z_offset = offsets - positions = [(round(p[0], 2), round(p[1], 2), p[2]) + positions = [[round(p[0], 2), round(p[1], 2), p[2]] for p in positions] params = dict(self.mesh_config) params['min_x'] = min(positions, key=lambda p: p[0])[0] + x_offset @@ -472,6 +574,48 @@ class BedMeshCalibrate: x_cnt = params['x_count'] y_cnt = params['y_count'] + if self.substituted_indices: + # Replace substituted points with the original generated + # point. Its Z Value is the average probed Z of the + # substituted points. + corrected_pts = [] + idx_offset = 0 + start_idx = 0 + for i, pts in self.substituted_indices.items(): + fpt = [p - o for p, o in zip(self.points[i], offsets[:2])] + # offset the index to account for additional samples + idx = i + idx_offset + # Add "normal" points + corrected_pts.extend(positions[start_idx:idx]) + avg_z = sum([p[2] for p in positions[idx:idx+len(pts)]]) \ + / len(pts) + idx_offset += len(pts) - 1 + start_idx = idx + len(pts) + fpt.append(avg_z) + logging.info( + "bed_mesh: Replacing value at faulty index %d" + " (%.4f, %.4f): avg value = %.6f, avg w/ z_offset = %.6f" + % (i, fpt[0], fpt[1], avg_z, avg_z - z_offset)) + corrected_pts.append(fpt) + corrected_pts.extend(positions[start_idx:]) + # validate corrected positions + if len(self.points) != len(corrected_pts): + self._dump_points(positions, corrected_pts, offsets) + raise self.gcode.error( + "bed_mesh: invalid position list size, " + "generated count: %d, probed count: %d" + % (len(self.points), len(corrected_pts))) + for gen_pt, probed in zip(self.points, corrected_pts): + off_pt = [p - o for p, o in zip(gen_pt, offsets[:2])] + if not isclose(off_pt[0], probed[0], abs_tol=.1) or \ + not isclose(off_pt[1], probed[1], abs_tol=.1): + self._dump_points(positions, corrected_pts, offsets) + raise self.gcode.error( + "bed_mesh: point mismatch, orig = (%.2f, %.2f)" + ", probed = (%.2f, %.2f)" + % (off_pt[0], off_pt[1], probed[0], probed[1])) + positions = corrected_pts + if self.relative_reference_index is not None: # zero out probe z offset and # set offset relative to reference index @@ -536,6 +680,24 @@ class BedMeshCalibrate: self.bedmesh.set_mesh(z_mesh) self.gcode.respond_info("Mesh Bed Leveling Complete") self.bedmesh.save_profile("default") + def _dump_points(self, probed_pts, corrected_pts, offsets): + # logs generated points with offset applied, points received + # from the finalize callback, and the list of corrected points + max_len = max([len(self.points), len(probed_pts), len(corrected_pts)]) + logging.info( + "bed_mesh: calibration point dump\nIndex | %-17s| %-25s|" + " Corrected Point" % ("Generated Point", "Probed Point")) + for i in list(range(max_len)): + gen_pt = probed_pt = corr_pt = "" + if i < len(self.points): + off_pt = [p - o for p, o in zip(self.points[i], offsets[:2])] + gen_pt = "(%.2f, %.2f)" % tuple(off_pt) + if i < len(probed_pts): + probed_pt = "(%.2f, %.2f, %.4f)" % tuple(probed_pts[i]) + if i < len(corrected_pts): + corr_pt = "(%.2f, %.2f, %.4f)" % tuple(corrected_pts[i]) + logging.info( + " %-4d| %-17s| %-25s| %s" % (i, gen_pt, probed_pt, corr_pt)) class MoveSplitter: