From ce9db609ad6f0f99c171b643464fe1e6963ec6c6 Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Wed, 6 Dec 2017 01:00:33 -0500 Subject: [PATCH] probe: Initial support for Z-Probe hardware Signed-off-by: Kevin O'Connor --- config/example-extras.cfg | 13 ++++++++++ klippy/cartesian.py | 6 ++++- klippy/corexy.py | 7 ++++- klippy/delta.py | 5 +++- klippy/extras/probe.py | 54 +++++++++++++++++++++++++++++++++++++++ klippy/homing.py | 10 ++++++-- klippy/mcu.py | 2 +- 7 files changed, 91 insertions(+), 6 deletions(-) create mode 100644 klippy/extras/probe.py diff --git a/config/example-extras.cfg b/config/example-extras.cfg index ce3af640..70dc1c1f 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 5e73297e..ff4d8b44 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 8ace928e..4b52f949 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 5014fe01..6d2490e5 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 00000000..73bdfafa --- /dev/null +++ b/klippy/extras/probe.py @@ -0,0 +1,54 @@ +# Z-Probe support +# +# Copyright (C) 2017-2018 Kevin O'Connor +# +# 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 3a926083..276c31d9 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 c4f9d969..40fcbad7 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)