probe: Initial support for Z-Probe hardware

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
Kevin O'Connor 2017-12-06 01:00:33 -05:00
parent 6c252d30f5
commit ce9db609ad
7 changed files with 91 additions and 6 deletions

View File

@ -4,6 +4,19 @@
# "example.cfg" file for description of common config parameters. # "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 # In a multi-extruder printer add an additional extruder section for
# each additional extruder. The additional extruder sections should be # each additional extruder. The additional extruder sections should be
# named "extruder1", "extruder2", "extruder3", and so on. See the # named "extruder1", "extruder2", "extruder3", and so on. See the

View File

@ -26,8 +26,12 @@ class CartKinematics:
self.steppers[1].set_max_jerk(max_halt_velocity, max_accel) self.steppers[1].set_max_jerk(max_halt_velocity, max_accel)
self.steppers[2].set_max_jerk( self.steppers[2].set_max_jerk(
min(max_halt_velocity, self.max_z_velocity), max_accel) 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) 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): def set_position(self, newpos):
for i in StepList: for i in StepList:
self.steppers[i].set_position(newpos[i]) self.steppers[i].set_position(newpos[i])

View File

@ -33,8 +33,13 @@ class CoreXYKinematics:
self.steppers[1].set_max_jerk(max_xy_halt_velocity, max_accel) self.steppers[1].set_max_jerk(max_xy_halt_velocity, max_accel)
self.steppers[2].set_max_jerk( self.steppers[2].set_max_jerk(
min(max_halt_velocity, self.max_z_velocity), self.max_z_accel) 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) 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): def set_position(self, newpos):
pos = (newpos[0] + newpos[1], newpos[0] - newpos[1], newpos[2]) pos = (newpos[0] + newpos[1], newpos[0] - newpos[1], newpos[2])
for i in StepList: for i in StepList:

View File

@ -70,7 +70,7 @@ class DeltaKinematics:
% (math.sqrt(self.max_xy2), math.sqrt(self.slow_xy2), % (math.sqrt(self.max_xy2), math.sqrt(self.slow_xy2),
math.sqrt(self.very_slow_xy2))) math.sqrt(self.very_slow_xy2)))
self.set_position([0., 0., 0.]) self.set_position([0., 0., 0.])
def get_steppers(self): def get_steppers(self, flags=""):
return list(self.steppers) return list(self.steppers)
def _cartesian_to_actuator(self, coord): def _cartesian_to_actuator(self, coord):
return [math.sqrt(self.arm2[i] - (self.towers[i][0] - coord[0])**2 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) ey_y = matrix_mul(ey, y)
ez_z = matrix_mul(ez, z) ez_z = matrix_mul(ez, z)
return matrix_add(carriage1, matrix_add(ex_x, matrix_add(ey_y, 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): def set_position(self, newpos):
pos = self._cartesian_to_actuator(newpos) pos = self._cartesian_to_actuator(newpos)
for i in StepList: for i in StepList:

54
klippy/extras/probe.py Normal file
View File

@ -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)

View File

@ -40,7 +40,7 @@ class Homing:
dist_ticks = adjusted_freq * mcu_stepper.get_step_dist() dist_ticks = adjusted_freq * mcu_stepper.get_step_dist()
ticks_per_step = math.ceil(dist_ticks / speed) ticks_per_step = math.ceil(dist_ticks / speed)
return dist_ticks / ticks_per_step 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 # Start endstop checking
print_time = self.toolhead.get_last_move_time() print_time = self.toolhead.get_last_move_time()
for mcu_endstop, name in endstops: for mcu_endstop, name in endstops:
@ -50,9 +50,10 @@ class Homing:
print_time, ENDSTOP_SAMPLE_TIME, ENDSTOP_SAMPLE_COUNT, print_time, ENDSTOP_SAMPLE_TIME, ENDSTOP_SAMPLE_COUNT,
min_step_dist / speed) min_step_dist / speed)
# Issue move # Issue move
movepos = self._fill_coord(movepos)
error = None error = None
try: try:
self.toolhead.move(self._fill_coord(movepos), speed) self.toolhead.move(movepos, speed)
except EndstopError as e: except EndstopError as e:
error = "Error during homing move: %s" % (str(e),) error = "Error during homing move: %s" % (str(e),)
# Wait for endstops to trigger # Wait for endstops to trigger
@ -64,6 +65,11 @@ class Homing:
except mcu_endstop.TimeoutError as e: except mcu_endstop.TimeoutError as e:
if error is None: if error is None:
error = "Failed to home %s: %s" % (name, str(e)) 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: if error is not None:
raise EndstopError(error) raise EndstopError(error)
def home(self, forcepos, movepos, endstops, speed, second_home=False): def home(self, forcepos, movepos, endstops, speed, second_home=False):

View File

@ -99,7 +99,7 @@ class MCU_stepper:
pos = params['pos'] pos = params['pos']
if self._invert_dir: if self._invert_dir:
pos = -pos pos = -pos
self._mcu_position_offset = pos - self._commanded_pos self._commanded_pos = pos - self._mcu_position_offset
def step(self, print_time, sdir): def step(self, print_time, sdir):
count = self._ffi_lib.stepcompress_push( count = self._ffi_lib.stepcompress_push(
self._stepqueue, print_time, sdir) self._stepqueue, print_time, sdir)