From b3e67e92b00a53e3646eb37012a611b959f1f33a Mon Sep 17 00:00:00 2001 From: Brandon Date: Sun, 27 Aug 2023 13:39:18 -0400 Subject: [PATCH] CloakedCode/Mental Dockable Probe (#43) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Dockable Probe annexed_probe: added dock_retries configuration option annexed_probe: renamed decouple_speed to detach_speed for consistency Annexed_Probe.md: initial commit of documentation Config_Reference.md: added annexed_probe config section annexed_probe: retry attempts must be greater than 1 annexed_probe: redefine default speeds annexed_probe: changed delayed detach to optional Annexed_Probe.md: added section for delayed_detach option Annexed_Probe.md: (fixed invalid characters and whitespace errors Annexed_Probe.md: Added additional documentation and gcode reference Config_Reference.md: Removed extra spaces for annexed_probe and added allow_delayed_detach parameter G-Codes.md: added annexed_probe gcode reference annexed_probe.py: changed initial position execution to after docking completed annexed_probe: rename to dockable_probe dockable_probe: remove ability to set manual_probe_state dockable_probe: change docking points to discrete coordinates instead of vectors Annexed_Probe.md: fixed typo in tool velocities dockable_probe: corrected typos dockable_probe.py - Corrected typo in _do_detach() dockable_probe: Python3 compatibility and misc fixes in parseCoord - map() returns a iterable which is not sliceable. - Fix a bug where the coordinate vector could be resized. - Better error reporting. Signed-off-by: Maël Kerbiriou * black --------- Signed-off-by: Maël Kerbiriou Co-authored-by: Paul McGowan --- docs/Config_Reference.md | 78 ++++ docs/Dockable_Probe.md | 405 +++++++++++++++++++ docs/G-Codes.md | 24 ++ docs/Status_Reference.md | 10 + klippy/extras/dockable_probe.py | 691 ++++++++++++++++++++++++++++++++ 5 files changed, 1208 insertions(+) create mode 100644 docs/Dockable_Probe.md create mode 100644 klippy/extras/dockable_probe.py diff --git a/docs/Config_Reference.md b/docs/Config_Reference.md index f609661f0..6cbd4be37 100644 --- a/docs/Config_Reference.md +++ b/docs/Config_Reference.md @@ -1910,6 +1910,84 @@ 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: +#activate_gcode: +#deactivate_gcode: +# 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 000000000..cb2955d18 --- /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 d8e71e2f2..2416fb4ef 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 13139dd02..c17eb2924 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 000000000..b4f09d009 --- /dev/null +++ b/klippy/extras/dockable_probe.py @@ -0,0 +1,691 @@ +# 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 math import sin, cos, atan2, pi, sqrt + +PROBE_VERIFY_DELAY = 0.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. +""" + +# 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): + pin_params = ppins.lookup_pin(pin, can_invert=True, can_pullup=True) + mcu = pin_params["chip"] + mcu_endstop = mcu.setup_pin("endstop", pin_params) + 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.0) + self.y_offset = config.getfloat("y_offset", 0.0) + self.speed = config.getfloat("speed", 5.0, above=0.0) + self.lift_speed = config.getfloat("lift_speed", self.speed, above=0.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.0 + ) + self.attach_speed = config.getfloat( + "attach_speed", self.travel_speed, above=0.0 + ) + self.detach_speed = config.getfloat( + "detach_speed", self.travel_speed, above=0.0 + ) + self.sample_retract_dist = config.getfloat( + "sample_retract_dist", 2.0, above=0.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.0, above=0.0) + + self.dock_requires_z = ( + self.approach_position[2] is not None + or self.dock_position[2] is not None + ) + + 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") + 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_endstop = mcu.setup_pin("endstop", pin_params) + + # 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 + + # 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] * 3 + p[:supplied_dims] = vals + return p + + def _build_config(self): + kin = self.printer.lookup_object("toolhead").get_kinematics() + for stepper in kin.get_steppers(): + if stepper.is_active_axis("z"): + self.add_stepper(stepper) + + def _handle_connect(self): + self.toolhead = self.printer.lookup_object("toolhead") + + ####################################################################### + # 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)) + + def get_status(self, curtime): + # Use last_'status' here to be consistent with QUERY_PROBE_'STATUS'. + return { + "last_status": self.last_probe_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): + if not self._last_homed: + 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 load_config(config): + msp = DockableProbe(config) + config.get_printer().add_object("probe", probe.PrinterProbe(config, msp)) + return msp