From 3d38211b8ab0b4736cdcd7e4b3f3a05bb598db3e Mon Sep 17 00:00:00 2001
From: Florian Heilmann <Florian.Heilmann@gmx.net>
Date: Tue, 6 Aug 2019 19:03:42 +0000
Subject: [PATCH] extra: Add Safe Z home extra

Signed-off-by: Florian Heilmann <Florian.Heilmann@gmx.net>
---
 config/example-extras.cfg    | 19 +++++++++
 klippy/extras/safe_z_home.py | 82 ++++++++++++++++++++++++++++++++++++
 2 files changed, 101 insertions(+)
 create mode 100644 klippy/extras/safe_z_home.py

diff --git a/config/example-extras.cfg b/config/example-extras.cfg
index 50d1ecfb..f3f1d1f8 100644
--- a/config/example-extras.cfg
+++ b/config/example-extras.cfg
@@ -1316,6 +1316,25 @@
 #   axis. The default is to not force a position for an axis.
 
 
+# Safe Z homing. One may use this mechanism to home the Z axis at a
+# specific XY coordinate. This is useful if the toolhead, for example
+# has to move to the center of the bed before Z can be homed.
+#[safe_z_home]
+#home_xy_position:
+#   A X,Y coordinate (e.g. 100,100) where the Z homing should be
+#   performed. This parameter must be provided.
+#speed: 50.0
+#   Speed at which the toolhead is moved to the safe Z home coordinate.
+#   The default is 50 mm/s
+#z_hop: 0.0
+#   Lift the Z axis prior to homing. This is applied to any homing command,
+#   even if it doesn't home the Z axis. If the Z axis is already homed and
+#   the zhop would exceed the printer limits, the zhop is ignored.
+#   The default is 0.0mm.
+#z_hop_speed: 20.0
+#   Speed at which the Z axis is lifted prior to homing. The default is 20mm/s.
+
+
 # Support manually moving stepper motors for diagnostic purposes.
 # Note, using this feature may place the printer in an invalid state -
 # see docs/G-Codes.md for important details.
diff --git a/klippy/extras/safe_z_home.py b/klippy/extras/safe_z_home.py
new file mode 100644
index 00000000..dc3fbf5d
--- /dev/null
+++ b/klippy/extras/safe_z_home.py
@@ -0,0 +1,82 @@
+# Perform Z Homing at specific XY coordinates.
+#
+# Copyright (C) 2019 Florian Heilmann <Florian.Heilmann@gmx.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+
+class SafeZHoming:
+    def __init__(self, config):
+        self.printer = config.get_printer()
+        try:
+            x_pos, y_pos = config.get("home_xy_position",
+                                            default=",").split(',')
+            self.home_x_pos, self.home_y_pos = float(x_pos), float(y_pos)
+        except:
+            raise config.error("Unable to parse home_xy_position in %s" % (
+                config.get_name()))
+
+        self.z_hop = config.getfloat("z_hop", default=0.0)
+        self.z_hop_speed = config.getfloat('z_hop_speed', 15., above=0.)
+        self.max_z = config.getsection('stepper_z').getfloat('position_max')
+        self.speed = config.getfloat('speed', 50.0, above=0.)
+        self.gcode = self.printer.lookup_object('gcode')
+        self.gcode.register_command("G28", None)
+        self.gcode.register_command("G28", self.cmd_G28)
+
+        if config.has_section("homing_override"):
+            raise config.error("homing_override and safe_z_homing cannot"
+                                    +" be used simultaneously")
+
+    def cmd_G28(self, params):
+
+        toolhead = self.printer.lookup_object('toolhead')
+        kinematics = toolhead.get_kinematics()
+
+        # Perform Z Hop if necessary
+        if self.z_hop != 0.0:
+            # Check if the zhop would exceed the printer limits
+            pos = toolhead.get_position()
+            kin_status = kinematics.get_status()
+            if ('Z' in kin_status['homed_axes'] and
+                pos[2] + self.z_hop > self.max_z):
+                self.gcode.respond_info(
+                    "No zhop performed, target Z out of bounds: " +
+                    str(pos[2] + self.z_hop)
+                )
+            else:
+                # Perform the Z-Hop
+                toolhead.set_position(pos, homing_axes=[2])
+                pos[2] = pos[2] + self.z_hop
+                toolhead.move(pos, self.z_hop_speed)
+                self.gcode.reset_last_position()
+
+        # Determine which axes we need to home
+        if not any([axis in params.keys() for axis in ['X', 'Y', 'Z']]):
+            need_x, need_y, need_z = [True] * 3
+        else:
+            need_x, need_y, need_z = tuple(axis in params
+                                           for axis in ['X', 'Y', 'Z'])
+
+        # Home XY axes if necessary
+        new_params = {}
+        if need_x:
+            new_params['X'] = '0'
+        if need_y:
+            new_params['Y'] = '0'
+        if new_params:
+            self.gcode.cmd_G28(new_params)
+        # Home Z axis if necessary
+        if need_z:
+            # Move to safe XY homing position
+            pos = toolhead.get_position()
+            if self.home_x_pos:
+                pos[0] = self.home_x_pos
+            if self.home_y_pos:
+                pos[1] = self.home_y_pos
+            toolhead.move(pos, self.speed)
+            self.gcode.reset_last_position()
+            # Home Z
+            self.gcode.cmd_G28({'Z': '0'})
+
+def load_config(config):
+    return SafeZHoming(config)