add X-Y probing module (PRELIMINARY IMPLEMENTATION!)

config/printer-rf1000-mill.cfg

@@ -178,7 +178,8 @@ gcode:
   SET_KINEMATIC_POSITION Z={z_clearance}
 
   BED_MESH_CALIBRATE MESH_MIN={svv.workpart_start_x},{svv.workpart_start_y} MESH_MAX={svv.workpart_end_x},{svv.workpart_end_y} PROBE_COUNT={nx},{ny}
-  RESTORE_GCODE_STATE NAME=workpart_scan_state
+  SAVE_CONFIG
+  #RESTORE_GCODE_STATE NAME=workpart_scan_state
 
 
 [gcode_macro edge_clear]
@@ -280,7 +281,7 @@ gcode:
   {% set svv = printer.save_variables.variables %}
   {% set z_clearance = svv.z_clearance - printer.toolhead.position.z + printer.gcode_move.gcode_position.z %}
   SAVE_GCODE_STATE NAME=do_probe_xy_state
-  CLEAR_WORKPART
+  #CLEAR_WORKPART
   G90
   {% for idx in range(0,svv.edge_nx) %}
     G0 Z{z_clearance}

klippy/extras/probe_xy.py

@@ -0,0 +1,291 @@
+# X/Y-Probe support
+#
+# Copyright (C) 2017-2021  Kevin O'Connor <kevin@koconnor.net>
+# Copyright (C) 2022  Martin Hierholzer <martin@hierholzer.info>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+import logging
+import pins
+
+HINT_TIMEOUT = """
+If the probe did not move far enough to trigger, then
+consider reducing/increasing the axis minimum/maximum
+position so the probe can travel further (the minimum
+position can be negative).
+"""
+
+direction_types = {'x+': [0,+1],'x-': [0,-1],'y+': [1,+1],'y-': [1,-1]}
+
+
+class PrinterProbeXY:
+    def __init__(self, config, mcu_probe_x, mcu_probe_y):
+        self.printer = config.get_printer()
+        self.name = config.get_name()
+        self.mcu_probe = [mcu_probe_x, mcu_probe_y]
+        self.speed = config.getfloat('speed', 5.0, above=0.)
+        self.lift_speed = config.getfloat('lift_speed', self.speed, above=0.)
+        self.last_state = False
+        self.last_result = [0., 0., 0.]
+        self.gcode_move = self.printer.load_object(config, "gcode_move")
+
+        xconfig = config.getsection('stepper_x')
+        yconfig = config.getsection('stepper_y')
+        # Note: This may not work for all kinematics (delta)...
+        self.axis_range = [ { -1: xconfig.getfloat('position_min', 0.),
+                              +1: xconfig.getfloat('position_max')      },
+                            { -1: yconfig.getfloat('position_min', 0.),
+                              +1: yconfig.getfloat('position_max')      } ]
+
+        # Multi-sample support (for improved accuracy)
+        self.sample_count = config.getint('samples', 1, minval=1)
+        self.sample_retract_dist = config.getfloat('sample_retract_dist', 2.,
+                                                   above=0.)
+        atypes = {'median': 'median', 'average': 'average'}
+        self.samples_result = config.getchoice('samples_result', atypes,
+                                               'average')
+        self.samples_tolerance = config.getfloat('samples_tolerance', 0.100,
+                                                 minval=0.)
+        self.samples_retries = config.getint('samples_tolerance_retries', 0,
+                                             minval=0)
+        # Register xy_virtual_endstop pin
+        self.printer.lookup_object('pins').register_chip('probe_xy', self)
+        # Register homing event handlers
+        #self.printer.register_event_handler("homing:homing_move_begin",
+        #                                    self._handle_homing_move_begin)
+        #self.printer.register_event_handler("homing:homing_move_end",
+        #                                    self._handle_homing_move_end)
+        self.printer.register_event_handler("homing:home_rails_begin",
+                                            self._handle_home_rails_begin)
+        self.printer.register_event_handler("homing:home_rails_end",
+                                            self._handle_home_rails_end)
+        # Register PROBE/QUERY_PROBE commands
+        self.gcode = self.printer.lookup_object('gcode')
+        self.gcode.register_command('PROBE_XY', self.cmd_PROBE,
+                                    desc=self.cmd_PROBE_help)
+        self.gcode.register_command('QUERY_PROBE_XY', self.cmd_QUERY_PROBE,
+                                    desc=self.cmd_QUERY_PROBE_help)
+        self.gcode.register_command('PROBE_XY_ACCURACY',
+                                    self.cmd_PROBE_ACCURACY,
+                                    desc=self.cmd_PROBE_ACCURACY_help)
+    #def _handle_homing_move_begin(self, hmove):
+    #    if self.mcu_probe[0] in hmove.get_mcu_endstops():
+    #        self.mcu_probe[0].probe_prepare(hmove)
+    #    if self.mcu_probe1[1] in hmove.get_mcu_endstops():
+    #        self.mcu_probe[1].probe_prepare(hmove)
+    #def _handle_homing_move_end(self, hmove):
+    #    if self.mcu_probe[0] in hmove.get_mcu_endstops():
+    #        self.mcu_probe[0].probe_finish(hmove)
+    #    if self.mcu_probe[1] in hmove.get_mcu_endstops():
+    #        self.mcu_probe[1].probe_finish(hmove)
+    def _handle_home_rails_begin(self, homing_state, rails):
+        endstops = [es for rail in rails for es, name in rail.get_endstops()]
+    def _handle_home_rails_end(self, homing_state, rails):
+        endstops = [es for rail in rails for es, name in rail.get_endstops()]
+    def setup_pin(self, pin_type, pin_params):
+        if pin_type != 'endstop' or pin_params['pin'] != 'xy_virtual_endstop':
+            raise pins.error("Probe virtual endstop only useful as endstop pin")
+        if pin_params['invert'] or pin_params['pullup']:
+            raise pins.error("Can not pullup/invert probe virtual endstop")
+        return self.mcu_probe
+    def get_lift_speed(self, gcmd=None):
+        if gcmd is not None:
+            return gcmd.get_float("LIFT_SPEED", self.lift_speed, above=0.)
+        return self.lift_speed
+    def _probe(self, speed, axis, sense):
+        toolhead = self.printer.lookup_object('toolhead')
+        curtime = self.printer.get_reactor().monotonic()
+        if 'x' not in toolhead.get_status(curtime)['homed_axes'] or            \
+           'y' not in toolhead.get_status(curtime)['homed_axes']:
+            raise self.printer.command_error("Must home before probe")
+        phoming = self.printer.lookup_object('homing')
+        pos = toolhead.get_position()
+        pos[axis] = self.axis_range[axis][sense]
+        try:
+            epos = phoming.probing_move(self.mcu_probe[axis], pos, speed)
+        except self.printer.command_error as e:
+            reason = str(e)
+            if "Timeout during endstop homing" in reason:
+                reason += HINT_TIMEOUT
+            raise self.printer.command_error(reason)
+        self.gcode.respond_info("probe at %.3f,%.3f is z=%.6f"
+                                % (epos[0], epos[1], epos[2]))
+        return epos[:3]
+    def _move(self, coord, speed):
+        self.printer.lookup_object('toolhead').manual_move(coord, speed)
+    def _calc_mean(self, positions):
+        count = float(len(positions))
+        return [sum([pos[i] for pos in positions]) / count
+                for i in range(3)]
+    def _calc_median(self, positions, axis):
+        axis_sorted = sorted(positions, key=(lambda p: p[axis]))
+        middle = len(positions) // 2
+        if (len(positions) & 1) == 1:
+            # odd number of samples
+            return axis_sorted[middle]
+        # even number of samples
+        return self._calc_mean(axis_sorted[middle-1:middle+1])
+    def run_probe(self, gcmd):
+        speed = gcmd.get_float("PROBE_SPEED", self.speed, above=0.)
+        direction = gcmd.get("DIRECTION").lower()
+        if direction not in direction_types:
+          raise self.printer.command_error("Wrong value for DIRECTION.")
+
+        logging.info("run_probe direction = "+str(direction))
+
+        (axis,sense) = direction_types[direction]
+
+        logging.info("run_probe axis = %d, sense = %d" % (axis,sense))
+
+        lift_speed = self.get_lift_speed(gcmd)
+        sample_count = gcmd.get_int("SAMPLES", self.sample_count, minval=1)
+        sample_retract_dist = gcmd.get_float("SAMPLE_RETRACT_DIST",
+                                             self.sample_retract_dist, above=0.)
+        samples_tolerance = gcmd.get_float("SAMPLES_TOLERANCE",
+                                           self.samples_tolerance, minval=0.)
+        samples_retries = gcmd.get_int("SAMPLES_TOLERANCE_RETRIES",
+                                       self.samples_retries, minval=0)
+        samples_result = gcmd.get("SAMPLES_RESULT", self.samples_result)
+        probe_start = self.printer.lookup_object('toolhead').get_position()
+        retries = 0
+        positions = []
+        while len(positions) < sample_count:
+            # Probe position
+            pos = self._probe(speed, axis, sense)
+            positions.append(pos)
+            # Check samples tolerance
+            axis_positions = [p[axis] for p in positions]
+            if max(axis_positions) - min(axis_positions) > samples_tolerance:
+                if retries >= samples_retries:
+                    raise gcmd.error("Probe samples exceed samples_tolerance")
+                gcmd.respond_info("Probe samples exceed tolerance. Retrying...")
+                retries += 1
+                positions = []
+            # Retract
+            if len(positions) < sample_count:
+                liftpos = probe_start
+                liftpos[axis] = pos[axis] - sense*sample_retract_dist
+                self._move(liftpos, lift_speed)
+        # Calculate and return result
+        if samples_result == 'median':
+            return self._calc_median(positions, axis)
+        return self._calc_mean(positions)
+    cmd_PROBE_help = "Probe Z-height at current XY position"
+    def cmd_PROBE(self, gcmd):
+        pos = self.run_probe(gcmd)
+        gcmd.respond_info("Result is x,y,z=%.6f,%.6f,%.6f" % (pos[0],pos[1],pos[2]))
+        self.last_result = pos
+        self._move(pos, self.get_lift_speed(gcmd))
+    cmd_QUERY_PROBE_help = "Return the status of the xy-probe"
+    def cmd_QUERY_PROBE(self, gcmd):
+        toolhead = self.printer.lookup_object('toolhead')
+        print_time = toolhead.get_last_move_time()
+        res = self.mcu_probe[0].query_endstop(print_time)
+        self.last_state = res
+        gcmd.respond_info("probe: %s" % (["open", "TRIGGERED"][not not res],))
+    def get_status(self, eventtime):
+        return {'last_query': self.last_state,
+                'last_result': self.last_result}
+    cmd_PROBE_ACCURACY_help = "Probe Z-height accuracy at current XY position"
+    def cmd_PROBE_ACCURACY(self, gcmd):
+        speed = gcmd.get_float("PROBE_SPEED", self.speed, above=0.)
+        direction = gcmd.get("DIRECTION")
+        if direction not in direction_types:
+          raise self.printer.command_error("Wrong value for DIRECTION.")
+        (axis,sense) = direction_types[direction]
+        lift_speed = self.get_lift_speed(gcmd)
+        sample_count = gcmd.get_int("SAMPLES", 10, minval=1)
+        sample_retract_dist = gcmd.get_float("SAMPLE_RETRACT_DIST",
+                                             self.sample_retract_dist, above=0.)
+        toolhead = self.printer.lookup_object('toolhead')
+        pos = toolhead.get_position()
+        gcmd.respond_info("PROBE_ACCURACY at X:%.3f Y:%.3f Z:%.3f"
+                          " (samples=%d retract=%.3f"
+                          " speed=%.1f lift_speed=%.1f)\n"
+                          % (pos[0], pos[1], pos[2],
+                             sample_count, sample_retract_dist,
+                             speed, lift_speed))
+        # Probe bed sample_count times
+        positions = []
+        while len(positions) < sample_count:
+            # Probe position
+            pos = self._probe(speed, axis, sense)
+            positions.append(pos)
+            # Retract
+            liftpos = toolhead.get_position()
+            liftpos[axis] = pos[axis] - sense*sample_retract_dist
+            self._move(liftpos, lift_speed)
+        # Calculate maximum, minimum and average values
+        max_value = max([p[axis] for p in positions])
+        min_value = min([p[axis] for p in positions])
+        range_value = max_value - min_value
+        avg_value = self._calc_mean(positions)[axis]
+        median = self._calc_median(positions, axis)[axis]
+        # calculate the standard deviation
+        deviation_sum = 0
+        for i in range(len(positions)):
+            deviation_sum += pow(positions[i][axis] - avg_value, 2.)
+        sigma = (deviation_sum / len(positions)) ** 0.5
+        # Show information
+        gcmd.respond_info(
+            "probe accuracy results: maximum %.6f, minimum %.6f, range %.6f, "
+            "average %.6f, median %.6f, standard deviation %.6f" % (
+            max_value, min_value, range_value, avg_value, median, sigma))
+
+# Endstop wrapper that enables probe specific features
+class ProbeXEndstopWrapper:
+    def __init__(self, config):
+        self.printer = config.get_printer()
+        # Create an "endstop" object to handle the probe pin
+        ppins = self.printer.lookup_object('pins')
+        pin = config.get('pin')
+        ppins.allow_multi_use_pin(pin.replace('^','').replace('!',''))
+        pin_params = ppins.lookup_pin(pin, can_invert=True, can_pullup=True)
+        mcu = pin_params['chip']
+        self.mcu_endstop = mcu.setup_pin('endstop', pin_params)
+        self.printer.register_event_handler('klippy:mcu_identify',
+                                            self._handle_mcu_identify)
+        # Wrappers
+        self.get_mcu = self.mcu_endstop.get_mcu
+        self.add_stepper = self.mcu_endstop.add_stepper
+        self.get_steppers = self.mcu_endstop.get_steppers
+        self.home_start = self.mcu_endstop.home_start
+        self.home_wait = self.mcu_endstop.home_wait
+        self.query_endstop = self.mcu_endstop.query_endstop
+    def _handle_mcu_identify(self):
+        kin = self.printer.lookup_object('toolhead').get_kinematics()
+        for stepper in kin.get_steppers():
+            if stepper.is_active_axis('x'):
+                self.add_stepper(stepper)
+    def get_position_endstop(self):
+        return self.position_endstop
+
+# Endstop wrapper that enables probe specific features
+class ProbeYEndstopWrapper:
+    def __init__(self, config):
+        self.printer = config.get_printer()
+        # Create an "endstop" object to handle the probe pin
+        ppins = self.printer.lookup_object('pins')
+        pin = config.get('pin')
+        pin_params = ppins.lookup_pin(pin, can_invert=True, can_pullup=True)
+        mcu = pin_params['chip']
+        self.mcu_endstop = mcu.setup_pin('endstop', pin_params)
+        self.printer.register_event_handler('klippy:mcu_identify',
+                                            self._handle_mcu_identify)
+        # Wrappers
+        self.get_mcu = self.mcu_endstop.get_mcu
+        self.add_stepper = self.mcu_endstop.add_stepper
+        self.get_steppers = self.mcu_endstop.get_steppers
+        self.home_start = self.mcu_endstop.home_start
+        self.home_wait = self.mcu_endstop.home_wait
+        self.query_endstop = self.mcu_endstop.query_endstop
+    def _handle_mcu_identify(self):
+        kin = self.printer.lookup_object('toolhead').get_kinematics()
+        for stepper in kin.get_steppers():
+            if stepper.is_active_axis('y'):
+                self.add_stepper(stepper)
+    def get_position_endstop(self):
+        return self.position_endstop
+
+
+def load_config(config):
+    return PrinterProbeXY(config, ProbeXEndstopWrapper(config), ProbeYEndstopWrapper(config))