klipper-dgus/klippy/extras/probe.py

# Z-Probe support
#
# Copyright (C) 2017-2018  Kevin O'Connor <kevin@koconnor.net>
#
# This file may be distributed under the terms of the GNU GPLv3 license.
import pins, homing

HINT_TIMEOUT = """
Make sure to home the printer before probing. If the probe
did not move far enough to trigger, then consider reducing
the Z axis minimum position so the probe can travel further
(the Z minimum position can be negative).
"""

class PrinterProbe:
    def __init__(self, config):
        self.printer = config.get_printer()
        self.speed = config.getfloat('speed', 5.0)
        self.x_offset = config.getfloat('x_offset', 0.)
        self.y_offset = config.getfloat('y_offset', 0.)
        self.z_offset = config.getfloat('z_offset')
        # Infer Z position to move to during a probe
        if config.has_section('stepper_z'):
            zconfig = config.getsection('stepper_z')
            self.z_position = zconfig.getfloat('position_min', 0.)
        else:
            pconfig = config.getsection('printer')
            self.z_position = pconfig.getfloat('minimum_z_position', 0.)
        # 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']
        mcu.register_config_callback(self.build_config)
        self.mcu_probe = mcu.setup_pin('endstop', pin_params)
        if (config.get('activate_gcode', None) is not None or
            config.get('deactivate_gcode', None) is not None):
            self.mcu_probe = ProbeEndstopWrapper(config, self.mcu_probe)
        # Create z_virtual_endstop pin
        ppins.register_chip('probe', self)
        # Register PROBE/QUERY_PROBE commands
        self.gcode = self.printer.lookup_object('gcode')
        self.gcode.register_command(
            'PROBE', self.cmd_PROBE, desc=self.cmd_PROBE_help)
        self.gcode.register_command(
            'QUERY_PROBE', self.cmd_QUERY_PROBE, desc=self.cmd_QUERY_PROBE_help)
    def build_config(self):
        kin = self.printer.lookup_object('toolhead').get_kinematics()
        for stepper in kin.get_steppers('Z'):
            stepper.add_to_endstop(self.mcu_probe)
    def setup_pin(self, pin_type, pin_params):
        if pin_type != 'endstop' or pin_params['pin'] != 'z_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 ProbeVirtualEndstop(self, self.mcu_probe)
    def get_offsets(self):
        return self.x_offset, self.y_offset, self.z_offset
    cmd_PROBE_help = "Probe Z-height at current XY position"
    def cmd_PROBE(self, params):
        toolhead = self.printer.lookup_object('toolhead')
        homing_state = homing.Homing(toolhead)
        pos = toolhead.get_position()
        pos[2] = self.z_position
        try:
            homing_state.probing_move(pos, self.mcu_probe, self.speed)
        except homing.EndstopError as e:
            reason = str(e)
            if "Timeout during endstop homing" in reason:
                reason += HINT_TIMEOUT
            raise self.gcode.error(reason)
        pos = toolhead.get_position()
        self.gcode.respond_info("probe at %.3f,%.3f is z=%.6f" % (
            pos[0], pos[1], pos[2]))
        self.gcode.reset_last_position()
    cmd_QUERY_PROBE_help = "Return the status of the z-probe"
    def cmd_QUERY_PROBE(self, params):
        toolhead = self.printer.lookup_object('toolhead')
        print_time = toolhead.get_last_move_time()
        self.mcu_probe.query_endstop(print_time)
        res = self.mcu_probe.query_endstop_wait()
        self.gcode.respond_info(
            "probe: %s" % (["open", "TRIGGERED"][not not res],))

# Endstop wrapper that enables running g-code scripts on setup
class ProbeEndstopWrapper:
    def __init__(self, config, mcu_endstop):
        self.mcu_endstop = mcu_endstop
        self.gcode = config.get_printer().lookup_object('gcode')
        self.activate_gcode = config.get('activate_gcode', "")
        self.deactivate_gcode = config.get('deactivate_gcode', "")
        # 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
        self.query_endstop_wait = self.mcu_endstop.query_endstop_wait
        self.TimeoutError = self.mcu_endstop.TimeoutError
    def home_prepare(self):
        self.gcode.run_script_from_command(self.activate_gcode)
        self.mcu_endstop.home_prepare()
    def home_finalize(self):
        self.gcode.run_script_from_command(self.deactivate_gcode)
        self.mcu_endstop.home_finalize()

# Wrapper for probe:z_virtual_endstop handling
class ProbeVirtualEndstop:
    def __init__(self, probe, mcu_endstop):
        self.probe = probe
        self.mcu_endstop = mcu_endstop
        # 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.home_finalize = self.mcu_endstop.home_finalize
        self.query_endstop = self.mcu_endstop.query_endstop
        self.query_endstop_wait = self.mcu_endstop.query_endstop_wait
        self.home_prepare = self.mcu_endstop.home_prepare
        self.TimeoutError = self.mcu_endstop.TimeoutError
    def get_position_endstop(self):
        return self.probe.get_offsets()[2]

# Helper code that can probe a series of points and report the
# position at each point.
class ProbePointsHelper:
    def __init__(self, config, finalize_callback, default_points=None):
        self.printer = config.get_printer()
        self.finalize_callback = finalize_callback
        self.probe_points = default_points
        # Read config settings
        if default_points is None or config.get('points', None) is not None:
            points = config.get('points').split('\n')
            try:
                points = [line.split(',', 1) for line in points if line.strip()]
                self.probe_points = [(float(p[0].strip()), float(p[1].strip()))
                                     for p in points]
            except:
                raise config.error("Unable to parse probe points in %s" % (
                    config.get_name()))
        if len(self.probe_points) < 3:
            raise config.error("Need at least 3 probe points for %s" % (
                config.get_name()))
        self.horizontal_move_z = config.getfloat('horizontal_move_z', 5.)
        self.speed = self.lift_speed = config.getfloat('speed', 50., above=0.)
        self.probe_offsets = (0., 0., 0.)
        self.samples = config.getint('samples', 1, minval=1)
        self.sample_retract_dist = config.getfloat(
            'sample_retract_dist', 2., above=0.)
        # Internal probing state
        self.results = []
        self.busy = False
        self.gcode = self.toolhead = None
    def get_lift_speed(self):
        return self.lift_speed
    def _lift_z(self, z_pos, add=False, speed=None):
        # Lift toolhead
        curpos = self.toolhead.get_position()
        if add:
            curpos[2] += z_pos
        else:
            curpos[2] = z_pos
        if speed is None:
            speed = self.lift_speed
        try:
            self.toolhead.move(curpos, speed)
        except homing.EndstopError as e:
            self._finalize(False)
            raise self.gcode.error(str(e))
    def _move_next(self):
        # Lift toolhead
        self._lift_z(self.horizontal_move_z)
        # Check if done probing
        if len(self.results) >= len(self.probe_points):
            self.toolhead.get_last_move_time()
            self._finalize(True)
            return
        # Move to next XY probe point
        x, y = self.probe_points[len(self.results)]
        curpos = self.toolhead.get_position()
        curpos[0] = x
        curpos[1] = y
        curpos[2] = self.horizontal_move_z
        try:
            self.toolhead.move(curpos, self.speed)
        except homing.EndstopError as e:
            self._finalize(False)
            raise self.gcode.error(str(e))
        self.gcode.reset_last_position()
    def _automatic_probe_point(self):
        positions = []
        for i in range(self.samples):
            try:
                self.gcode.run_script_from_command("PROBE")
            except self.gcode.error as e:
                self._finalize(False)
                raise
            positions.append(self.toolhead.get_position())
            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)
    def start_probe(self, params):
        # Lookup objects
        self.toolhead = self.printer.lookup_object('toolhead')
        self.gcode = self.printer.lookup_object('gcode')
        probe = self.printer.lookup_object('probe', None)
        method = self.gcode.get_str('METHOD', params, 'automatic').lower()
        if probe is not None and method == 'automatic':
            self.lift_speed = min(self.speed, probe.speed)
            self.probe_offsets = probe.get_offsets()
            if self.horizontal_move_z < self.probe_offsets[2]:
                raise self.gcode.error("horizontal_move_z can't be less than"
                                       " probe's z_offset")
        else:
            probe = None
            self.lift_speed = self.speed
            self.probe_offsets = (0., 0., 0.)
        # Start probe
        self.results = []
        self.busy = True
        self._lift_z(self.horizontal_move_z, speed=self.speed)
        self._move_next()
        if probe is None:
            # Setup for manual probing
            self.gcode.register_command('NEXT', None)
            self.gcode.register_command('NEXT', self.cmd_NEXT,
                                        desc=self.cmd_NEXT_help)
        else:
            # Perform automatic probing
            while self.busy:
                self._automatic_probe_point()
                self._move_next()
    cmd_NEXT_help = "Move to the next XY position to probe"
    def cmd_NEXT(self, params):
        # Record current position for manual probe
        self.toolhead.get_last_move_time()
        self.results.append(self.toolhead.get_kinematics().calc_position())
        # Move to next position
        self._move_next()
    def _finalize(self, success):
        self.busy = False
        self.gcode.reset_last_position()
        self.gcode.register_command('NEXT', None)
        if success:
            self.finalize_callback(self.probe_offsets, self.results)

def load_config(config):
    return PrinterProbe(config)