diff --git a/docs/Config_Reference.md b/docs/Config_Reference.md index 549b96ec8a66..66cdca0ef13d 100644 --- a/docs/Config_Reference.md +++ b/docs/Config_Reference.md @@ -1897,6 +1897,82 @@ control_pin: # See the "probe" section for information on these parameters. ``` +### [dockable_probe] + +Certain probes are magnetically coupled to the toolhead and stowed +in a dock when not in use. One should define this section instead +of a probe section if the probe uses magnets to attach and a dock +for storage. See [Dockable Probe Guide](Dockable_Probe.md) +for more detailed information regarding configuration and setup. + +``` +[dockable_probe] +dock_position: 0,0,0 +# The physical position of the probe dock relative to the origin of +# the bed. The coordinates are specified as a comma separated X, Y, Z +# list of values. Certain dock designs are independent of the Z axis. +# If Z is specified the toolhead will move to the Z location before the X, Y +# coordinates. +# This parameter is required. +approach_position: 0,0,0 +# The X, Y, Z position where the toolhead needs to be prior to moving into the +# dock so that the probe is aligned properly for attaching or detaching. +# If Z is specified the toolhead will move to the Z location before the X, Y +# coordinates. +# This parameter is required. +detach_position: 0,0,0 +# Similar to the approach_position, the detach_position is the coordinates +# where the toolhead is moved after the probe has been docked. +# For magnetically coupled probes, this is typically perpendicular to +# the approach_position in a direction that does not cause the tool to +# collide with the printer. +# If Z is specified the toolhead will move to the Z location before the X, Y +# coordinates. +# This parameter is required. +#z_hop: 15.0 +# Distance (in mm) to lift the Z axis prior to attaching/detaching the probe. +# If the Z axis is already homed and the current Z position is less +# than `z_hop`, then this will lift the head to a height of `z_hop`. If +# the Z axis is not already homed the head is lifted by `z_hop`. +# The default is to not implement Z hop. +#dock_retries: +# The number of times to attempt to attach/dock the probe before raising +# an error and aborting probing. +# The default is 0. +#auto_attach_detach: False +# Enable/Disable the automatic attaching/detaching of the probe during +# actions that require the probe. +# The default is True. +#attach_speed: +#detach_speed: +#travel_speed: +# Optional speeds used during moves. +# The default is to use `speed` of `probe` or 5.0. +#check_open_attach: +# The probe status should be verified prior to homing. Setting this option +# to true will check the probe "endstop" is "open" after attaching and +# will abort probing if not, also checking for "triggered" after docking. +# Conversively, setting this to false, the probe should read "triggered" +# after attaching and "open" after docking. If not, probing will abort. +#probe_sense_pin: +# This supplemental pin can be defined to determine an attached state +# instead of check_open_attach. +#dock_sense_pin: +# This supplemental pin can be defined to determine a docked state in +# addition to probe_sense_pin or check_open_attach. +#x_offset: +#y_offset: +#z_offset: +#lift_speed: +#speed: +#samples: +#sample_retract_dist: +#samples_result: +#samples_tolerance: +#samples_tolerance_retries: +# See the "probe" section for information on these parameters. +``` + ### [smart_effector] The "Smart Effector" from Duet3d implements a Z probe using a force diff --git a/docs/Dockable_Probe.md b/docs/Dockable_Probe.md new file mode 100644 index 000000000000..cb2955d185d7 --- /dev/null +++ b/docs/Dockable_Probe.md @@ -0,0 +1,405 @@ +# Dockable Probe + +Dockable probes are typically microswitches mounted to a printed body that +attaches to the toolhead through some means of mechanical coupling. +This coupling is commonly done with magnets though there is support for +a variety of designs including servo and stepper actuated couplings. + +## Basic Configuration + +To use a dockable probe the following options are required at a minimum. +Some users may be transitioning from a macro based set of commands and +many of the options for the `[probe]` config section are the same. +The `[dockable_probe]` module is first and foremost a `[probe]` +but with additional functionality. Any options that can be specified +for `[probe]` are valid for `[dockable_probe]`. + +``` +[dockable_probe] +pin: +z_offset: +sample_retract_dist: +approach_position: +dock_position: +detach_position: +(check_open_attach: OR probe_sense_pin:) AND/OR dock_sense_pin: +``` + +### Attaching and Detaching Positions + +- `dock_position: 300, 295, 0`\ + _Required_\ + This is the XYZ coordinates where the toolhead needs to be positioned + in order to attach the probe. This parameter is X, Y and, Z separated + by commas. + + Many configurations have the dock attached to a moving gantry. This + means that Z axis positioning is irrelevant. However, it may be necessary + to move the gantry clear of the bed or other printer components before + performing docking steps. In this case, specify `z_hop` to force a Z movement. + + Other configurations may have the dock mounted next to the printer bed so + that the Z position _must_ be known prior to attaching the probe. In this + configuration the Z axis parameter _must_ be supplied, and the Z axis + _must_ be homed prior to attaching the probe. + +- `approach_position: 300, 250, 0`\ + _Required_\ + The most common dock designs use a fork or arms that extend out from the dock. + In order to attach the probe to the toolhead, the toolhead must move into and + away from the dock to a particular position so these arms can capture the + probe body. + + As with `dock_position`, a Z position is not required but if specified the + toolhead will be moved to that Z location before the X, Y coordinates. + + For magnetically coupled probes, the `approach_position` should be far enough + away from the probe dock such that the magnets on the probe body are not + attracted to the magnets on the toolhead. + +- `detach_position: 250, 295, 0`\ + _Required_\ + Most probes with magnets require the toolhead to move in a direction that + strips the magnets off with a sliding motion. This is to prevent the magnets + from becoming unseated from repeated pulling and thus affecting probe accuracy. + The `detach_position` is typically defined as a point that is perpendicular to + the dock so that when the toolhead moves, the probe stays docked but cleanly + detaches from the toolhead mount. + + As with `dock_position`, a Z position is not required but if specified the + toolhead will be moved to that Z location before the X, Y coordinates. + + For magnetically coupled probes, the `detach_position` should be far enough + away from the probe dock such that the magnets on the probe body are not + attracted to the magnets on the toolhead. + +- `z_hop: 15.0`\ + _Default Value: None_\ + Distance (in mm) to lift the Z axis prior to attaching/detaching the probe. + If the Z axis is already homed and the current Z position is less + than `z_hop`, then this will lift the head to a height of `z_hop`. If + the Z axis is not already homed the head is lifted by `z_hop`. + The default is to not implement Z hop. + +## Position Examples + +Probe mounted on frame at back of print bed at a fixed Z position. To attach +the probe, the toolhead will move back and then forward. To detach, the toolhead +will move back, and then to the side. + +``` ++--------+ +| p> | +| ^ | +| | ++--------+ +``` + +``` +approach_position: 150, 300, 5 +dock_position: 150, 330, 5 +detach_position: 170, 330 +``` + + +Probe mounted at side of moving gantry with fixed bed. Here the probe is +attachable regardless of the Z position. To attach the probe, the toolhead will +move to the side and back. To detach the toolhead will move to the side and then +forward. + +``` ++--------+ +| | +| p< | +| v | ++--------+ +``` + +``` +approach_position: 50, 150 +dock_position: 10, 150 +detach_position: 10, 130 +``` + + +Probe mounted at side of fixed gantry with bed moving on Z. Probe is attachable +regardless of Z but force Z hop for safety. The toolhead movement is the same +as above. + +``` ++--------+ +| | +| p< | +| v | ++--------+ +``` + +``` +approach_position: 50, 150 +dock_position: 10, 150 +detach_position: 10, 130 +z_hop: 15 +``` + + +Euclid style probe that requires the attach and detach movements to happen in +opposite order. Attach: approach, move to dock, extract. Detach: move to +extract position, move to dock, move to approach position. The approach and +detach positions are the same, as are the extract and insert positions. The +movements can be reordered as necessary by overriding the commands for +extract/insert and using the same coordinates for approach and detach. + +``` +Attach: ++--------+ +| | +| p< | +| v | ++--------+ +Detach: ++--------+ +| | +| p> | +| ^ | ++--------+ +``` + +``` +approach_position: 50, 150 +dock_position: 10, 150 +detach_position: 50, 150 +z_hop: 15 +``` + +``` +[gcode_macro MOVE_TO_EXTRACT_PROBE] +gcode: + G1 X10 Y130 + +[gcode_macro MOVE_TO_INSERT_PROBE] +gcode: + G1 X10 Y130 +``` + +### Homing + +No configuration specific to the dockable probe is required when using +the probe as a virtual endstop, though it's recommended to consider +using `[safe_z_home]` or `[homing_override]`. + +### Probe Attachment Verification + +Given the nature of this type of probe, it is necessary to verify whether or +not it has successfully attached prior to attempting a probing move. Several +methods can be used to verify probe attachment states. + +- `check_open_attach:`\ + _Default Value: None_\ + Certain probes will report `OPEN` when they are attached and `TRIGGERED` + when they are detached in a non-probing state. When `check_open_attach` is + set to `True`, the state of the probe pin is checked after performing a + probe attach or detach maneuver. If the probe does not read `OPEN` + immediately after attaching the probe, an error will be raised and any + further action will be aborted. + + This is intended to prevent crashing the nozzle into the bed since it is + assumed if the probe pin reads `TRIGGERED` prior to probing, the probe is + not attached. + + Setting this to `False` will cause all action to be aborted if the probe + does not read `TRIGGERED` after attaching. + +- `probe_sense_pin:`\ + _Default Value: None_\ + The probe may include a separate pin for attachment verification. This is a + standard pin definition, similar to an endstop pin that defines how to handle + the input from the sensor. Much like the `check_open_attach` option, the check + is done immediately after the tool attaches or detaches the probe. If the + probe is not detected after attempting to attach it, or it remains attached + after attempting to detach it, an error will be raised and further + action aborted. + +- `dock_sense_pin:`\ + _Default Value: None_\ + Docks can have a sensor or switch incorporated into their design in + order to report that the probe is presently located in the dock. A + `dock_sense_pin` can be used to provide verification that the probe is + correctly positioned in the dock. This is a standard pin definition similar + to an endstop pin that defines how to handle the input from the sensor. + Prior to attempting to attach the probe, and after attempting to detach it, + this pin is checked. If the probe is not detected in the dock, an error will + be raised and further action aborted. + +- `dock_retries: 5`\ + _Default Value: 0_\ + A magnetic probe may require repeated attempts to attach or detach. If + `dock_retries` is specified and the probe fails to attach or detach, the + attach/detach action will be repeated until it succeeds. If the retry limit + is reached and the probe is still not in the correct state, an error will be + raised and further action aborted. + +## Tool Velocities + +- `attach_speed: 5.0`\ + _Default Value: Probe `speed` or 5_\ + Movement speed when attaching the probe during `MOVE_TO_DOCK_PROBE`. + +- `detach_speed: 5.0`\ + _Default Value: Probe `speed` or 5_\ + Movement speed when detaching the probe during `MOVE_TO_DETACH_PROBE`. + +- `travel_speed: 5.0`\ + _Default Value: Probe `speed` or 5_\ + Movement speed when approaching the probe during `MOVE_TO_APPROACH_PROBE` + and returning the toolhead to its previous position after attach/detach. + +## Dockable Probe Gcodes + +### General + +`ATTACH_PROBE` + +This command will move the toolhead to the dock, attach the probe, and return +it to its previous position. If the probe is already attached, the command +does nothing. + +This command will call `MOVE_TO_APPROACH_PROBE`, `MOVE_TO_DOCK_PROBE`, +and `MOVE_TO_EXTRACT_PROBE`. + +`DETACH_PROBE` + +This command will move the toolhead to the dock, detach the probe, and return +it to its previous position. If the probe is already detached, the command +will do nothing. + +This command will call `MOVE_TO_APPROACH_PROBE`, `MOVE_TO_DOCK_PROBE`, +and `MOVE_TO_DETACH_PROBE`. + +### Individual Movements + +These commands are useful during setup to prevent the full attach/detach +sequence from crashing into the bed or damaging the probe/dock. + +If your probe has special setup/teardown steps (e.g. moving a servo), +accommodating that could be accomplished by overriding these gcodes. + +`MOVE_TO_APPROACH_PROBE` + +This command will move the toolhead to the `approach_position`. It can be +overridden to move a servo if that's required for attaching your probe. + +`MOVE_TO_DOCK_PROBE` + +This command will move the toolhead to the `dock_position`. + +`MOVE_TO_EXTRACT_PROBE` + +This command will move the toolhead away from the dock after attaching the probe. +By default it's an alias for `MOVE_TO_APPROACH_PROBE`. + +`MOVE_TO_INSERT_PROBE` + +This command will move the toolhead near the dock before detaching the probe. +By default it's an alias for `MOVE_TO_APPROACH_PROBE`. + +`MOVE_TO_DETACH_PROBE` + +This command will move the toolhead to the `detach_position`. It can be +overridden to move a servo if that's required for detaching your probe. + +### Status + +`QUERY_DOCKABLE_PROBE` + +Responds in the gcode terminal with the current probe status. Valid +states are UNKNOWN, ATTACHED, and DOCKED. This is useful during setup +to confirm probe configuration is working as intended. + +`SET_DOCKABLE_PROBE AUTO_ATTACH_DETACH=0|1` + +Enable/Disable the automatic attaching/detaching of the probe during +actions that require the probe. + +This command can be helpful in print-start macros where multiple actions will +be performed with the probe and there's no need to detach the probe. +For example: + +``` +SET_DOCKABLE_PROBE AUTO_ATTACH_DETACH=0 +G28 +ATTACH_PROBE # Explicitly attach the probe +QUAD_GANTRY_LEVEL # Tram the gantry parallel to the bed +BED_MESH_CALIBRATE # Create a bed mesh +DETACH_PROBE # Manually detach the probe +SET_DOCKABLE_PROBE AUTO_ATTACH_DETACH=1 # Make sure the probe is attached in future +``` + +## Typical probe execution flow + +### Probing is Started: + + - A gcode command requiring the use of the probe is executed. + + - This triggers the probe to attach. + + - If configured, the dock sense pin is checked to see if the probe is + presently in the dock. + + - The toolhead position is compared to the dock position. + + - If the toolhead is outside of the minimum safe radius, the toolhead is + commanded to move to the approach vector, that is, a position that is + the minimum safe distance from the dock in line with the dock angle. + (MOVE_TO_APPROACH_PROBE) + + - If the toolhead is inside of the minimum safe radius, the toolhead is + commanded to move to the nearest point on the line of the approach vector. + (MOVE_TO_APPROACH_PROBE) + + - The tool is moved along the approach vector to the dock coordinates. + (MOVE_TO_DOCK_PROBE) + + - The toolhead is commanded to move out of the dock back to the minimum + safe distance in the reverse direction along the dock angle. + (MOVE_TO_EXTRACT_PROBE) + + - If configured, the probe is checked to see if it is attached. + + - If the probe is not attached, the module may retry until it's attached or + an error is raised. + + - If configured, the dock sense pin is checked to see if the probe is still + present, the module may retry until the probe is absent not or an error + is raised. + + - The probe moves to the first probing point and begins probing. + +### Probing is Finished: + + - After the probe is no longer needed, the probe is triggered to detach. + + - The toolhead position is compared to the dock position. + + - If the toolhead is outside of the minimum safe radius, the toolhead is + commanded to move to the approach vector, that is, a position that is + the minimum safe distance from the dock in line with the dock angle. + (MOVE_TO_APPROACH_PROBE) + + - If the toolhead is inside of the minimum safe radius, the toolhead is + commanded to move to the nearest point on the line of the approach vector. + (MOVE_TO_APPROACH_PROBE) + + - The toolhead is moved along the approach vector to the dock coordinates. + (MOVE_TO_DOCK_PROBE) + + - The toolhead is commanded to move along the detach vector if supplied or a + calculated direction based on axis parameters. (MOVE_TO_DETACH_PROBE) + + - If configured, the probe is checked to see if it detached. + + - If the probe did not detach, the module moves the toolhead back to the + approach vector and may retry until it detaches or an error is raised. + + - If configured, the dock sense pin is checked to see if the probe is + present in the dock. If it is not the module moves the toolhead back to + the approach vector and may retry until it detaches or an error is raised. diff --git a/docs/G-Codes.md b/docs/G-Codes.md index a6d056bf2e56..0ad39f529085 100644 --- a/docs/G-Codes.md +++ b/docs/G-Codes.md @@ -303,6 +303,30 @@ Also provided is the following extended G-Code command: setting the supplied `MSG` as the current display message. If `MSG` is omitted the display will be cleared. +## [dockable_probe] + +In addition to the normal commands available for a `[probe]`, the following +commands are available when a +[dockable_probe config section](Config_Reference.md#dockable_probe) is enabled +(also see the [Dockable Probe guide](Dockable_Probe.md)): + +- `ATTACH_PROBE`: Move to dock and attach probe to the toolhead, the toolhead + will return to its previous position after attaching. +- `DETACH_PROBE`: Move to dock and detach probe from the toolhead, the toolhead + will return to its previous position after detaching. +- `QUERY_DOCKABLE_PROBE`: Respond with current probe state. This is useful for + verifying configuration settings are working as intended. +- `SET_DOCKABLE_PROBE AUTO_ATTACH_DETACH=0|1`: Enable/Disable the automatic + attaching/detaching of the probe during actions that require the probe. +- `MOVE_TO_APPROACH_PROBE`: Move to approach the probe dock. +- `MOVE_TO_DOCK_PROBE`: Move to the probe dock (this should trigger the probe + to attach). +- `MOVE_TO_EXTRACT_PROBE`: Move to leave the dock with the probe attached. +- `MOVE_TO_INSERT_PROBE`: Move to insert position near the dock + with the probe attached. +- `MOVE_TO_DETACH_PROBE`: Move away from the dock to disconnect the probe + from the toolhead. + ### [dual_carriage] The following command is available when the diff --git a/docs/Status_Reference.md b/docs/Status_Reference.md index bff64d180fac..773075b44edb 100644 --- a/docs/Status_Reference.md +++ b/docs/Status_Reference.md @@ -68,6 +68,16 @@ The following information is available in the `display_status` object `virtual_sdcard.progress` if no recent `M73` received). - `message`: The message contained in the last `M117` G-Code command. +## dockable_probe + +The following information is available in the +[dockable_probe](Config_Reference.md#dockable_probe): +- `last_status`: The UNKNOWN/ATTACHED/DOCKED status of the probbe as + reported during the last QUERY_DOCKABLE_PROBE command. Note, if + this is used in a macro, due to the order of template expansion, + the QUERY_DOCKABLE_PROBE command must be run prior to the macro + containing this reference. + ## endstop_phase The following information is available in the diff --git a/klippy/extras/dockable_probe.py b/klippy/extras/dockable_probe.py new file mode 100644 index 000000000000..817dc6182e7c --- /dev/null +++ b/klippy/extras/dockable_probe.py @@ -0,0 +1,646 @@ +# Dockable Probe +# This provides support for probes that are magnetically coupled +# to the toolhead and stowed in a dock when not in use and +# +# Copyright (C) 2018-2023 Kevin O'Connor +# Copyright (C) 2021 Paul McGowan +# Copyright (C) 2023 Alan Smith +# +# This file may be distributed under the terms of the GNU GPLv3 license. +from . import probe +from mcu import MCU_endstop +from math import sin, cos, atan2, pi, sqrt + +PROBE_VERIFY_DELAY = .1 + +PROBE_UNKNOWN = 0 +PROBE_ATTACHED = 1 +PROBE_DOCKED = 2 + +MULTI_OFF = 0 +MULTI_FIRST = 1 +MULTI_ON = 2 + +HINT_VERIFICATION_ERROR = """ +{0}: A probe attachment verification method +was not provided. A method to verify the probes attachment +state must be specified to prevent unintended behavior. + +At least one of the following must be specified: +'check_open_attach', 'probe_sense_pin', 'dock_sense_pin' + +Please see {0}.md and config_Reference.md. +""" + +HINT_VIRTUAL_ENDSTOP_ERROR = """ +{0}: Using a 'probe:z_virtual_endstop' Z endstop is +incompatible with 'approach_position'/'dock_position' +containing a Z coordinate. + +If the toolhead doesn't need to move in Z to reach the +dock then no Z coordinate should be specified in +'approach_position'/'dock_position'. + +Please see {0}.md and config_Reference.md. +""" + +# Helper class to handle polling pins for probe attachment states +class PinPollingHelper: + def __init__(self, config, endstop): + self.printer = config.get_printer() + self.query_endstop = endstop + self.last_verify_time = 0 + self.last_verify_state = None + + def query_pin(self, curtime): + if (curtime > (self.last_verify_time + PROBE_VERIFY_DELAY) + or self.last_verify_state is None): + self.last_verify_time = curtime + toolhead = self.printer.lookup_object('toolhead') + query_time = toolhead.get_last_move_time() + self.last_verify_state = not not self.query_endstop(query_time) + return self.last_verify_state + + def query_pin_inv(self, curtime): + return not self.query_pin(curtime) + +# Helper class to verify probe attachment status +class ProbeState: + def __init__(self, config, aProbe): + self.printer = config.get_printer() + + if (not config.fileconfig.has_option(config.section, + 'check_open_attach') + and not config.fileconfig.has_option(config.section, + 'probe_sense_pin') + and not config.fileconfig.has_option(config.section, + 'dock_sense_pin')): + raise self.printer.config_error(HINT_VERIFICATION_ERROR.format( + aProbe.name)) + + self.printer.register_event_handler('klippy:ready', + self._handle_ready) + + # Configure sense pins as endstops so they + # can be polled at specific times + ppins = self.printer.lookup_object('pins') + def configEndstop(pin): + mcu_endstop = ppins.setup_pin('endstop', pin) + helper = PinPollingHelper(config, mcu_endstop.query_endstop) + return helper + + probe_sense_helper = None + dock_sense_helper = None + + # Setup sensor pins, if configured, otherwise use probe endstop + # as a dummy sensor. + ehelper = PinPollingHelper(config, aProbe.query_endstop) + + # Probe sense pin is optional + probe_sense_pin = config.get('probe_sense_pin', None) + if probe_sense_pin is not None: + probe_sense_helper = configEndstop(probe_sense_pin) + self.probe_sense_pin = probe_sense_helper.query_pin + else: + self.probe_sense_pin = ehelper.query_pin_inv + + # If check_open_attach is specified, it takes precedence + # over probe_sense_pin + check_open_attach = None + if config.fileconfig.has_option(config.section, 'check_open_attach'): + check_open_attach = config.getboolean('check_open_attach') + + if check_open_attach: + self.probe_sense_pin = ehelper.query_pin_inv + else: + self.probe_sense_pin = ehelper.query_pin + + # Dock sense pin is optional + self.dock_sense_pin = None + dock_sense_pin = config.get('dock_sense_pin', None) + if dock_sense_pin is not None: + dock_sense_helper = configEndstop(dock_sense_pin) + self.dock_sense_pin = dock_sense_helper.query_pin + + def _handle_ready(self): + self.last_verify_time = 0 + self.last_verify_state = PROBE_UNKNOWN + + def get_probe_state(self): + curtime = self.printer.get_reactor().monotonic() + return self.get_probe_state_with_time(curtime) + + def get_probe_state_with_time(self, curtime): + if (self.last_verify_state == PROBE_UNKNOWN + or curtime > self.last_verify_time + PROBE_VERIFY_DELAY): + self.last_verify_time = curtime + self.last_verify_state = PROBE_UNKNOWN + + a = self.probe_sense_pin(curtime) + + if self.dock_sense_pin is not None: + d = self.dock_sense_pin(curtime) + + if a and not d: + self.last_verify_state = PROBE_ATTACHED + elif d and not a: + self.last_verify_state = PROBE_DOCKED + else: + if a: + self.last_verify_state = PROBE_ATTACHED + elif not a: + self.last_verify_state = PROBE_DOCKED + return self.last_verify_state + +class DockableProbe: + def __init__(self, config): + self.printer = config.get_printer() + self.gcode = self.printer.lookup_object('gcode') + self.name = config.get_name() + + # Configuration Options + self.position_endstop = config.getfloat('z_offset') + self.x_offset = config.getfloat('x_offset', 0.) + self.y_offset = config.getfloat('y_offset', 0.) + self.speed = config.getfloat('speed', 5.0, above=0.) + self.lift_speed = config.getfloat('lift_speed', + self.speed, above=0.) + self.dock_retries = config.getint('dock_retries', 0) + self.auto_attach_detach = config.getboolean('auto_attach_detach', + True) + self.travel_speed = config.getfloat('travel_speed', + self.speed, above=0.) + self.attach_speed = config.getfloat('attach_speed', + self.travel_speed, above=0.) + self.detach_speed = config.getfloat('detach_speed', + self.travel_speed, above=0.) + self.sample_retract_dist = config.getfloat('sample_retract_dist', + 2., above=0.) + + # Positions (approach, detach, etc) + self.approach_position = self._parse_coord(config, 'approach_position') + self.detach_position = self._parse_coord(config, 'detach_position') + self.dock_position = self._parse_coord(config, 'dock_position') + self.z_hop = config.getfloat('z_hop', 0., above=0.) + + self.dock_requires_z = (len(self.approach_position) > 2 + or len(self.dock_position) > 2) + + self.dock_angle, self.approach_distance = self._get_vector( + self.dock_position, + self.approach_position) + self.detach_angle, self.detach_distance = self._get_vector( + self.dock_position, + self.detach_position) + + # Pins + ppins = self.printer.lookup_object('pins') + self.mcu_endstop = ppins.setup_pin('endstop', config.get('pin')) + # github.com/protoloft/klipper_z_calibration expects any probe + # implementation to have the below variable: + self.mcu_probe = self.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_wait = self.mcu_endstop.home_wait + self.query_endstop = self.mcu_endstop.query_endstop + self.finish_home_complete = self.wait_trigger_complete = None + + # Common probe implementation helpers + self.cmd_helper = probe.ProbeCommandHelper( + config, self, self.mcu_endstop.query_endstop) + self.probe_offsets = probe.ProbeOffsetsHelper(config) + if hasattr(probe, 'ProbeParameterHelper'): + self.param_helper = probe.ProbeParameterHelper(config) + self.homing_helper = probe.HomingViaProbeHelper(config, self, self.param_helper) + self.probe_session = probe.ProbeSessionHelper( + config, self.param_helper, self.homing_helper.start_probe_session) + else: + self.probe_session = probe.ProbeSessionHelper(config, self) + + # State + self.last_z = -9999 + self.multi = MULTI_OFF + self._last_homed = None + + pstate = ProbeState(config, self) + self.get_probe_state = pstate.get_probe_state + self.last_probe_state = PROBE_UNKNOWN + + self.probe_states = { + PROBE_ATTACHED: 'ATTACHED', + PROBE_DOCKED: 'DOCKED', + PROBE_UNKNOWN: 'UNKNOWN' + } + + # Gcode Commands + self.gcode.register_command('QUERY_DOCKABLE_PROBE', + self.cmd_QUERY_DOCKABLE_PROBE, + desc=self.cmd_QUERY_DOCKABLE_PROBE_help) + + self.gcode.register_command('MOVE_TO_APPROACH_PROBE', + self.cmd_MOVE_TO_APPROACH_PROBE, + desc=self.cmd_MOVE_TO_APPROACH_PROBE_help) + self.gcode.register_command('MOVE_TO_DOCK_PROBE', + self.cmd_MOVE_TO_DOCK_PROBE, + desc=self.cmd_MOVE_TO_DOCK_PROBE_help) + self.gcode.register_command('MOVE_TO_EXTRACT_PROBE', + self.cmd_MOVE_TO_EXTRACT_PROBE, + desc=self.cmd_MOVE_TO_EXTRACT_PROBE_help) + self.gcode.register_command('MOVE_TO_INSERT_PROBE', + self.cmd_MOVE_TO_INSERT_PROBE, + desc=self.cmd_MOVE_TO_INSERT_PROBE_help) + self.gcode.register_command('MOVE_TO_DETACH_PROBE', + self.cmd_MOVE_TO_DETACH_PROBE, + desc=self.cmd_MOVE_TO_DETACH_PROBE_help) + + self.gcode.register_command('SET_DOCKABLE_PROBE', + self.cmd_SET_DOCKABLE_PROBE, + desc=self.cmd_SET_DOCKABLE_PROBE_help) + self.gcode.register_command('ATTACH_PROBE', + self.cmd_ATTACH_PROBE, + desc=self.cmd_ATTACH_PROBE_help) + self.gcode.register_command('DETACH_PROBE', + self.cmd_DETACH_PROBE, + desc=self.cmd_DETACH_PROBE_help) + + # Event Handlers + self.printer.register_event_handler('klippy:connect', + self._handle_connect) + + # Parse a string coordinate representation from the config + # and return a list of numbers. + # + # e.g. "233, 10, 0" -> [233, 10, 0] + def _parse_coord(self, config, name, expected_dims=3): + val = config.get(name) + error_msg = "Unable to parse {0} in {1}: {2}" + if not val: + return None + try: + vals = [float(x.strip()) for x in val.split(',')] + except Exception as e: + raise config.error(error_msg.format(name, self.name, str(e))) + supplied_dims = len(vals) + if not 2 <= supplied_dims <= expected_dims: + raise config.error(error_msg.format(name, self.name, + "Invalid number of coordinates")) + p = [None] * supplied_dims + p[:supplied_dims] = vals + return p + + def get_probe_params(self, gcmd=None): + if hasattr(self, 'param_helper'): + return self.param_helper.get_probe_params(gcmd) + return self.probe_session.get_probe_params(gcmd) + def get_offsets(self): + return self.probe_offsets.get_offsets() + def get_status(self, eventtime): + status = self.cmd_helper.get_status(eventtime) + status['last_status'] = self.last_probe_state + return status + def start_probe_session(self, gcmd): + return self.probe_session.start_probe_session(gcmd) + + def _handle_connect(self): + self.toolhead = self.printer.lookup_object('toolhead') + + # If neither position config options contain a Z coordinate return early + if not self.dock_requires_z: + return + + query_endstops = self.printer.lookup_object('query_endstops') + for endstop, name in query_endstops.endstops: + if name == 'z': + # Check for probe being used as virtual endstop + if not isinstance(endstop, MCU_endstop): + raise self.printer.config_error( + HINT_VIRTUAL_ENDSTOP_ERROR.format(self.name)) + + ####################################################################### + # GCode Commands + ####################################################################### + + cmd_QUERY_DOCKABLE_PROBE_help = ("Prints the current probe state," + + " valid probe states are UNKNOWN, ATTACHED, and DOCKED") + def cmd_QUERY_DOCKABLE_PROBE(self, gcmd): + self.last_probe_state = self.get_probe_state() + state = self.probe_states[self.last_probe_state] + + gcmd.respond_info('Probe Status: %s' % (state)) + + cmd_MOVE_TO_APPROACH_PROBE_help = "Move close to the probe dock" \ + "before attaching" + def cmd_MOVE_TO_APPROACH_PROBE(self, gcmd): + self._align_z() + + if self._check_distance(dist=self.approach_distance): + self._align_to_vector(self.dock_angle) + else: + self._move_to_vector(self.dock_angle) + + if len(self.approach_position) > 2: + self.toolhead.manual_move([None, None, self.approach_position[2]], + self.travel_speed) + + self.toolhead.manual_move( + [self.approach_position[0], self.approach_position[1], None], + self.travel_speed) + + cmd_MOVE_TO_DOCK_PROBE_help = "Move to connect the toolhead/dock" \ + "to the probe" + def cmd_MOVE_TO_DOCK_PROBE(self, gcmd): + if len(self.dock_position) > 2: + self.toolhead.manual_move([None, None, self.dock_position[2]], + self.attach_speed) + + self.toolhead.manual_move( + [self.dock_position[0], self.dock_position[1], None], + self.attach_speed) + + cmd_MOVE_TO_EXTRACT_PROBE_help = "Move away from the dock with the" \ + "probe attached" + def cmd_MOVE_TO_EXTRACT_PROBE(self, gcmd): + self.cmd_MOVE_TO_APPROACH_PROBE(gcmd) + + cmd_MOVE_TO_INSERT_PROBE_help = "Move near the dock with the" \ + "probe attached before detaching" + def cmd_MOVE_TO_INSERT_PROBE(self, gcmd): + self.cmd_MOVE_TO_APPROACH_PROBE(gcmd) + + cmd_MOVE_TO_DETACH_PROBE_help = "Move away from the dock to detach" \ + "the probe" + def cmd_MOVE_TO_DETACH_PROBE(self, gcmd): + if len(self.detach_position) > 2: + self.toolhead.manual_move([None, None, self.detach_position[2]], + self.detach_speed) + + self.toolhead.manual_move( + [self.detach_position[0], self.detach_position[1], None], + self.detach_speed) + + cmd_SET_DOCKABLE_PROBE_help = "Set probe parameters" + def cmd_SET_DOCKABLE_PROBE(self, gcmd): + auto = gcmd.get('AUTO_ATTACH_DETACH', None) + if auto is None: + return + + if int(auto) == 1: + self.auto_attach_detach = True + else: + self.auto_attach_detach = False + + cmd_ATTACH_PROBE_help = "Check probe status and attach probe using" \ + "the movement gcodes" + def cmd_ATTACH_PROBE(self, gcmd): + return_pos = self.toolhead.get_position() + self.attach_probe(return_pos) + + cmd_DETACH_PROBE_help = "Check probe status and detach probe using" \ + "the movement gcodes" + def cmd_DETACH_PROBE(self, gcmd): + return_pos = self.toolhead.get_position() + self.detach_probe(return_pos) + + def attach_probe(self, return_pos=None): + retry = 0 + while (self.get_probe_state() != PROBE_ATTACHED + and retry < self.dock_retries + 1): + if self.get_probe_state() != PROBE_DOCKED: + raise self.printer.command_error( + 'Attach Probe: Probe not detected in dock, aborting') + # Call these gcodes as a script because we don't have enough + # structs/data to call the cmd_...() funcs and supply 'gcmd'. + # This method also has the advantage of calling user-written gcodes + # if they've been defined. + self.gcode.run_script_from_command(""" + MOVE_TO_APPROACH_PROBE + MOVE_TO_DOCK_PROBE + MOVE_TO_EXTRACT_PROBE + """) + + retry += 1 + + if self.get_probe_state() != PROBE_ATTACHED: + raise self.printer.command_error('Probe attach failed!') + + if return_pos: + if not self._check_distance(return_pos, self.approach_distance): + self.toolhead.manual_move( + [return_pos[0], return_pos[1], None], + self.travel_speed) + # Do NOT return to the original Z position after attach + # as the probe might crash into the bed. + + def detach_probe(self, return_pos=None): + retry = 0 + while (self.get_probe_state() != PROBE_DOCKED + and retry < self.dock_retries + 1): + # Call these gcodes as a script because we don't have enough + # structs/data to call the cmd_...() funcs and supply 'gcmd'. + # This method also has the advantage of calling user-written gcodes + # if they've been defined. + self.gcode.run_script_from_command(""" + MOVE_TO_INSERT_PROBE + MOVE_TO_DOCK_PROBE + MOVE_TO_DETACH_PROBE + """) + + retry += 1 + + if self.get_probe_state() != PROBE_DOCKED: + raise self.printer.command_error('Probe detach failed!') + + if return_pos: + if not self._check_distance(return_pos, self.detach_distance): + self.toolhead.manual_move( + [return_pos[0], return_pos[1], None], + self.travel_speed) + # Return to original Z position after detach as + # there's no chance of the probe crashing into the bed. + self.toolhead.manual_move( + [None, None, return_pos[2]], + self.travel_speed) + + def auto_detach_probe(self, return_pos=None): + if self.get_probe_state() == PROBE_DOCKED: + return + if self.auto_attach_detach: + self.detach_probe(return_pos) + + def auto_attach_probe(self, return_pos=None): + if self.get_probe_state() == PROBE_ATTACHED: + return + if not self.auto_attach_detach: + raise self.printer.command_error("Cannot probe, probe is not " \ + "attached and auto-attach is disabled") + self.attach_probe(return_pos) + + ####################################################################### + # Functions for calculating points and moving the toolhead + ####################################################################### + + # Move the toolhead to minimum safe distance aligned with angle + def _move_to_vector(self, angle): + x, y = self._get_point_on_vector(self.dock_position[:2], + angle, + self.approach_distance) + self.toolhead.manual_move([x,y,None], self.travel_speed) + + # Move the toolhead to angle within minimium safe distance + def _align_to_vector(self, angle): + approach = self._get_intercept(self.toolhead.get_position(), + angle + (pi/2), + self.dock_position, + angle) + self.toolhead.manual_move([approach[0], approach[1], None], + self.attach_speed) + + # Determine toolhead distance to dock coordinates + def _check_distance(self, pos=None, dist=None): + if not pos: + pos = self.toolhead.get_position() + dock = self.dock_position + + if dist > sqrt((pos[0]-dock[0])**2 + + (pos[1]-dock[1])**2 ): + return True + else: + return False + + # Find a point on a vector line at a specific distance + def _get_point_on_vector(self, point, angle, magnitude=1): + x = point[0] - magnitude * cos(angle) + y = point[1] - magnitude * sin(angle) + return (x, y) + + # Locate the intersection of two vectors + def _get_intercept(self, point1, angle1, point2, angle2): + x1, y1 = point1[:2] + x2, y2 = self._get_point_on_vector(point1, angle1, 10.0) + x3, y3 = point2[:2] + x4, y4 = self._get_point_on_vector(point2, angle2, 10.0) + det1 = ((x1 * y2) - (y1 * x2)) + det2 = ((x3 * y4) - (y3 * x4)) + d = ((x1 - x2) * (y3 - y4)) - ((y1 - y2) * (x3 - x4)) + x = float((det1 * (x3 - x4)) - ((x1 - x2) * det2)) / d + y = float((det1 * (y3 - y4)) - ((y1 - y2) * det2)) / d + return (x, y) + + # Determine the vector of two points + def _get_vector(self, point1, point2): + x1, y1 = point1[:2] + x2, y2 = point2[:2] + magnitude = sqrt((x2-x1)**2 + (y2-y1)**2 ) + angle = atan2(y2-y1, x2-x1) + pi + + return angle, magnitude + + # Align z axis to prevent crashes + def _align_z(self): + curtime = self.printer.get_reactor().monotonic() + homed_axes = self.toolhead.get_status(curtime)['homed_axes'] + self._last_homed = homed_axes + + if self.dock_requires_z: + self._align_z_required() + + if self.z_hop > 0.0: + if 'z' in self._last_homed: + tpos = self.toolhead.get_position() + if tpos[2] < self.z_hop: + self.toolhead.manual_move([None, None, self.z_hop], + self.lift_speed) + else: + self._force_z_hop() + + def _align_z_required(self): + if 'z' not in self._last_homed: + raise self.printer.command_error( + "Cannot attach/detach probe, must home Z axis first") + + self.toolhead.manual_move([None, None, self.approach_position[2]], + self.lift_speed) + + # Hop z and return to un-homed state + def _force_z_hop(self): + this_z = self.toolhead.get_position()[2] + if self.last_z == this_z: + return + + tpos = self.toolhead.get_position() + self.toolhead.set_position([tpos[0], tpos[1], 0.0, tpos[3]], + homing_axes=[2]) + self.toolhead.manual_move([None, None, self.z_hop], + self.lift_speed) + kin = self.toolhead.get_kinematics() + kin.note_z_not_homed() + self.last_z = self.toolhead.get_position()[2] + + ####################################################################### + # Probe Wrappers + ####################################################################### + + def multi_probe_begin(self): + self.multi = MULTI_FIRST + + # Attach probe before moving to the first probe point and + # return to current position. Move because this can be called + # before a multi _point_ probe and a multi probe at the same + # point but for the latter the toolhead is already in position. + # If the toolhead is not returned to the current position it + # will complete the probing next to the dock. + return_pos = self.toolhead.get_position() + self.auto_attach_probe(return_pos) + + def multi_probe_end(self): + self.multi = MULTI_OFF + + return_pos = self.toolhead.get_position() + # Move away from the bed to ensure the probe isn't triggered, + # preventing detaching in the event there's no probe/dock sensor. + self.toolhead.manual_move([None, None, return_pos[2]+2], + self.travel_speed) + self.auto_detach_probe(return_pos) + + def probe_prepare(self, hmove): + if self.multi == MULTI_OFF or self.multi == MULTI_FIRST: + return_pos = self.toolhead.get_position() + self.auto_attach_probe(return_pos) + if self.multi == MULTI_FIRST: + self.multi = MULTI_ON + + def probe_finish(self, hmove): + self.wait_trigger_complete.wait() + if self.multi == MULTI_OFF: + return_pos = self.toolhead.get_position() + # Move away from the bed to ensure the probe isn't triggered, + # preventing detaching in the event there's no probe/dock sensor. + self.toolhead.manual_move([None, None, return_pos[2]+2], + self.travel_speed) + self.auto_detach_probe(return_pos) + + def home_start(self, print_time, sample_time, sample_count, rest_time, + triggered=True): + self.finish_home_complete = self.mcu_endstop.home_start( + print_time, sample_time, sample_count, rest_time, triggered) + r = self.printer.get_reactor() + self.wait_trigger_complete = r.register_callback(self.wait_for_trigger) + return self.finish_home_complete + + def wait_for_trigger(self, eventtime): + self.finish_home_complete.wait() + + def get_position_endstop(self): + return self.position_endstop + + def probing_move(self, pos, speed): + phoming = self.printer.lookup_object('homing') + return phoming.probing_move(self, pos, speed) + +def load_config(config): + dockable_probe = DockableProbe(config) + config.get_printer().add_object('probe', dockable_probe) + return dockable_probe