diff --git a/config/example-extras.cfg b/config/example-extras.cfg
index ce3af6402..70dc1c1fc 100644
--- a/config/example-extras.cfg
+++ b/config/example-extras.cfg
@@ -4,6 +4,19 @@
# "example.cfg" file for description of common config parameters.
+# Z height probe. One may define this section to enable Z height
+# probing hardware. When this section is enabled, PROBE and
+# QUERY_PROBE extended g-code commands become available.
+#[probe]
+#pin: ar15
+# Probe detection pin. This parameter must be provided.
+#speed: 5.0
+# Speed (in mm/s) of the Z axis when probing. The default is 5mm/s.
+#z_position: 0.0
+# The Z position to command the head to move to during a PROBE
+# command. The default is 0.
+
+
# In a multi-extruder printer add an additional extruder section for
# each additional extruder. The additional extruder sections should be
# named "extruder1", "extruder2", "extruder3", and so on. See the
diff --git a/klippy/cartesian.py b/klippy/cartesian.py
index 5e73297e2..ff4d8b44d 100644
--- a/klippy/cartesian.py
+++ b/klippy/cartesian.py
@@ -26,8 +26,12 @@ class CartKinematics:
self.steppers[1].set_max_jerk(max_halt_velocity, max_accel)
self.steppers[2].set_max_jerk(
min(max_halt_velocity, self.max_z_velocity), max_accel)
- def get_steppers(self):
+ def get_steppers(self, flags=""):
+ if flags == "Z":
+ return [self.steppers[2]]
return list(self.steppers)
+ def get_position(self):
+ return [s.mcu_stepper.get_commanded_position() for s in self.steppers]
def set_position(self, newpos):
for i in StepList:
self.steppers[i].set_position(newpos[i])
diff --git a/klippy/corexy.py b/klippy/corexy.py
index 8ace928ef..4b52f949f 100644
--- a/klippy/corexy.py
+++ b/klippy/corexy.py
@@ -33,8 +33,13 @@ class CoreXYKinematics:
self.steppers[1].set_max_jerk(max_xy_halt_velocity, max_accel)
self.steppers[2].set_max_jerk(
min(max_halt_velocity, self.max_z_velocity), self.max_z_accel)
- def get_steppers(self):
+ def get_steppers(self, flags=""):
+ if flags == "Z":
+ return [self.steppers[2]]
return list(self.steppers)
+ def get_position(self):
+ pos = [s.mcu_stepper.get_commanded_position() for s in self.steppers]
+ return [0.5 * (pos[0] + pos[1]), 0.5 * (pos[0] - pos[1]), pos[2]]
def set_position(self, newpos):
pos = (newpos[0] + newpos[1], newpos[0] - newpos[1], newpos[2])
for i in StepList:
diff --git a/klippy/delta.py b/klippy/delta.py
index 5014fe017..6d2490e59 100644
--- a/klippy/delta.py
+++ b/klippy/delta.py
@@ -70,7 +70,7 @@ class DeltaKinematics:
% (math.sqrt(self.max_xy2), math.sqrt(self.slow_xy2),
math.sqrt(self.very_slow_xy2)))
self.set_position([0., 0., 0.])
- def get_steppers(self):
+ def get_steppers(self, flags=""):
return list(self.steppers)
def _cartesian_to_actuator(self, coord):
return [math.sqrt(self.arm2[i] - (self.towers[i][0] - coord[0])**2
@@ -101,6 +101,9 @@ class DeltaKinematics:
ey_y = matrix_mul(ey, y)
ez_z = matrix_mul(ez, z)
return matrix_add(carriage1, matrix_add(ex_x, matrix_add(ey_y, ez_z)))
+ def get_position(self):
+ spos = [s.mcu_stepper.get_commanded_position() for s in self.steppers]
+ return self._actuator_to_cartesian(spos)
def set_position(self, newpos):
pos = self._cartesian_to_actuator(newpos)
for i in StepList:
diff --git a/klippy/extras/probe.py b/klippy/extras/probe.py
new file mode 100644
index 000000000..73bdfafaf
--- /dev/null
+++ b/klippy/extras/probe.py
@@ -0,0 +1,54 @@
+# 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 homing
+
+class PrinterProbe:
+ def __init__(self, config):
+ self.printer = config.get_printer()
+ self.speed = config.getfloat('speed', 5.0)
+ self.z_position = config.getfloat('z_position', 0.)
+ ppins = self.printer.lookup_object('pins')
+ pin_params = ppins.lookup_pin('endstop', config.get('pin'))
+ mcu = pin_params['chip']
+ mcu.add_config_object(self)
+ self.mcu_probe = mcu.setup_pin(pin_params)
+ 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):
+ toolhead = self.printer.lookup_object('toolhead')
+ z_steppers = toolhead.get_kinematics().get_steppers("Z")
+ for s in z_steppers:
+ for mcu_endstop, name in s.get_endstops():
+ for mcu_stepper in mcu_endstop.get_steppers():
+ self.mcu_probe.add_stepper(mcu_stepper)
+ 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.homing_move(
+ pos, [(self.mcu_probe, "probe")], self.speed, probe_pos=True)
+ except homing.EndstopError as e:
+ raise self.gcode.error(str(e))
+ 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],))
+
+def load_config(config):
+ if config.get_name() != 'probe':
+ raise config.error("Invalid probe config name")
+ return PrinterProbe(config)
diff --git a/klippy/homing.py b/klippy/homing.py
index 3a926083f..276c31d92 100644
--- a/klippy/homing.py
+++ b/klippy/homing.py
@@ -40,7 +40,7 @@ class Homing:
dist_ticks = adjusted_freq * mcu_stepper.get_step_dist()
ticks_per_step = math.ceil(dist_ticks / speed)
return dist_ticks / ticks_per_step
- def homing_move(self, movepos, endstops, speed):
+ def homing_move(self, movepos, endstops, speed, probe_pos=False):
# Start endstop checking
print_time = self.toolhead.get_last_move_time()
for mcu_endstop, name in endstops:
@@ -50,9 +50,10 @@ class Homing:
print_time, ENDSTOP_SAMPLE_TIME, ENDSTOP_SAMPLE_COUNT,
min_step_dist / speed)
# Issue move
+ movepos = self._fill_coord(movepos)
error = None
try:
- self.toolhead.move(self._fill_coord(movepos), speed)
+ self.toolhead.move(movepos, speed)
except EndstopError as e:
error = "Error during homing move: %s" % (str(e),)
# Wait for endstops to trigger
@@ -64,6 +65,11 @@ class Homing:
except mcu_endstop.TimeoutError as e:
if error is None:
error = "Failed to home %s: %s" % (name, str(e))
+ if probe_pos:
+ self.set_homed_position(
+ list(self.toolhead.get_kinematics().get_position()) + [None])
+ else:
+ self.toolhead.set_position(movepos)
if error is not None:
raise EndstopError(error)
def home(self, forcepos, movepos, endstops, speed, second_home=False):
diff --git a/klippy/mcu.py b/klippy/mcu.py
index c4f9d969f..40fcbad73 100644
--- a/klippy/mcu.py
+++ b/klippy/mcu.py
@@ -99,7 +99,7 @@ class MCU_stepper:
pos = params['pos']
if self._invert_dir:
pos = -pos
- self._mcu_position_offset = pos - self._commanded_pos
+ self._commanded_pos = pos - self._mcu_position_offset
def step(self, print_time, sdir):
count = self._ffi_lib.stepcompress_push(
self._stepqueue, print_time, sdir)