From 8260a3a0c38e25168e3dfc414ed62fd23e380b21 Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Fri, 8 Jan 2021 12:37:57 -0500 Subject: homing: Make homing.py an "extras" module Move klippy/homing.py to klippy/extras/homing.py and convert the code to an "extras" modules. Signed-off-by: Kevin O'Connor --- klippy/extras/endstop_phase.py | 6 +- klippy/extras/gcode_move.py | 21 ++--- klippy/extras/homing.py | 182 +++++++++++++++++++++++++++++++++++++++ klippy/extras/homing_override.py | 2 +- klippy/extras/probe.py | 10 +-- klippy/extras/safe_z_home.py | 2 +- klippy/homing.py | 159 ---------------------------------- klippy/toolhead.py | 4 +- 8 files changed, 201 insertions(+), 185 deletions(-) create mode 100644 klippy/extras/homing.py delete mode 100644 klippy/homing.py (limited to 'klippy') diff --git a/klippy/extras/endstop_phase.py b/klippy/extras/endstop_phase.py index 66e9fc38..8e70be74 100644 --- a/klippy/extras/endstop_phase.py +++ b/klippy/extras/endstop_phase.py @@ -1,6 +1,6 @@ # Endstop accuracy improvement via stepper phase tracking # -# Copyright (C) 2016-2018 Kevin O'Connor +# Copyright (C) 2016-2021 Kevin O'Connor # # This file may be distributed under the terms of the GNU GPLv3 license. import math, logging @@ -101,7 +101,7 @@ class EndstopPhase: "Endstop %s incorrect phase (got %d vs %d)" % ( self.name, phase, self.endstop_phase)) return delta * self.step_dist - def handle_home_rails_end(self, rails): + def handle_home_rails_end(self, homing_state, rails): for rail in rails: stepper = rail.get_steppers()[0] if stepper.get_name() != self.name: @@ -149,7 +149,7 @@ class EndstopPhases: return phase = convert_phase(driver_phase, driver_phases, len(phase_history)) phase_history[phase] += 1 - def handle_home_rails_end(self, rails): + def handle_home_rails_end(self, homing_state, rails): for rail in rails: stepper = rail.get_steppers()[0] stepper_name = stepper.get_name() diff --git a/klippy/extras/gcode_move.py b/klippy/extras/gcode_move.py index 4bdfdf8b..1b9a22b7 100644 --- a/klippy/extras/gcode_move.py +++ b/klippy/extras/gcode_move.py @@ -4,7 +4,6 @@ # # This file may be distributed under the terms of the GNU GPLv3 license. import logging -import homing class GCodeMove: def __init__(self, config): @@ -19,11 +18,13 @@ class GCodeMove: self.reset_last_position) printer.register_event_handler("extruder:activate_extruder", self._handle_activate_extruder) + printer.register_event_handler("homing:home_rails_end", + self._handle_home_rails_end) self.is_printer_ready = False # Register g-code commands gcode = printer.lookup_object('gcode') handlers = [ - 'G1', 'G28', 'G20', 'G21', + 'G1', 'G20', 'G21', 'M82', 'M83', 'G90', 'G91', 'G92', 'M220', 'M221', 'SET_GCODE_OFFSET', 'SAVE_GCODE_STATE', 'RESTORE_GCODE_STATE', ] @@ -68,6 +69,10 @@ class GCodeMove: self.reset_last_position() self.extrude_factor = 1. self.base_position[3] = self.last_position[3] + def _handle_home_rails_end(self, homing_state, rails): + self.reset_last_position() + for axis in homing_state.get_axes(): + self.base_position[axis] = self.homing_position[axis] def set_move_transform(self, transform, force=False): if self.move_transform is not None and not force: raise self.printer.config_error( @@ -134,18 +139,6 @@ class GCodeMove: raise gcmd.error("Unable to parse move '%s'" % (gcmd.get_commandline(),)) self.move_with_transform(self.last_position, self.speed) - def cmd_G28(self, gcmd): - # Move to origin - axes = [] - for pos, axis in enumerate('XYZ'): - if gcmd.get(axis, None) is not None: - axes.append(pos) - if not axes: - axes = [0, 1, 2] - homing_state = homing.Homing(self.printer) - homing_state.home_axes(axes) - for axis in homing_state.get_axes(): - self.base_position[axis] = self.homing_position[axis] # G-Code coordinate manipulation def cmd_G20(self, gcmd): # Set units to inches diff --git a/klippy/extras/homing.py b/klippy/extras/homing.py new file mode 100644 index 00000000..283e0df7 --- /dev/null +++ b/klippy/extras/homing.py @@ -0,0 +1,182 @@ +# Helper code for implementing homing operations +# +# Copyright (C) 2016-2021 Kevin O'Connor +# +# This file may be distributed under the terms of the GNU GPLv3 license. +import logging, math, collections + +HOMING_START_DELAY = 0.001 +ENDSTOP_SAMPLE_TIME = .000015 +ENDSTOP_SAMPLE_COUNT = 4 + +# State tracking during toolhead homing/probing operations +class Homing: + def __init__(self, printer): + self.printer = printer + self.toolhead = printer.lookup_object('toolhead') + self.changed_axes = [] + self.verify_retract = True + if self.printer.get_start_args().get("debuginput"): + self.verify_retract = False + def set_axes(self, axes): + self.changed_axes = axes + def get_axes(self): + return self.changed_axes + def _fill_coord(self, coord): + # Fill in any None entries in 'coord' with current toolhead position + thcoord = list(self.toolhead.get_position()) + for i in range(len(coord)): + if coord[i] is not None: + thcoord[i] = coord[i] + return thcoord + def set_homed_position(self, pos): + self.toolhead.set_position(self._fill_coord(pos)) + def _calc_endstop_rate(self, mcu_endstop, movepos, speed): + startpos = self.toolhead.get_position() + axes_d = [mp - sp for mp, sp in zip(movepos, startpos)] + move_d = math.sqrt(sum([d*d for d in axes_d[:3]])) + move_t = move_d / speed + max_steps = max([(abs(s.calc_position_from_coord(startpos) + - s.calc_position_from_coord(movepos)) + / s.get_step_dist()) + for s in mcu_endstop.get_steppers()]) + if max_steps <= 0.: + return .001 + return move_t / max_steps + def homing_move(self, movepos, endstops, speed, + probe_pos=False, verify_movement=False): + # Notify start of homing/probing move + self.printer.send_event("homing:homing_move_begin", + [es for es, name in endstops]) + # Note start location + self.toolhead.flush_step_generation() + kin = self.toolhead.get_kinematics() + for s in kin.get_steppers(): + s.set_tag_position(s.get_commanded_position()) + start_mcu_pos = [(s, name, s.get_mcu_position()) + for es, name in endstops for s in es.get_steppers()] + # Start endstop checking + print_time = self.toolhead.get_last_move_time() + endstop_triggers = [] + for mcu_endstop, name in endstops: + rest_time = self._calc_endstop_rate(mcu_endstop, movepos, speed) + wait = mcu_endstop.home_start(print_time, ENDSTOP_SAMPLE_TIME, + ENDSTOP_SAMPLE_COUNT, rest_time) + endstop_triggers.append(wait) + all_endstop_trigger = multi_complete(self.printer, endstop_triggers) + self.toolhead.dwell(HOMING_START_DELAY) + # Issue move + error = None + try: + self.toolhead.drip_move(movepos, speed, all_endstop_trigger) + except CommandError as e: + error = "Error during homing move: %s" % (str(e),) + # Wait for endstops to trigger + move_end_print_time = self.toolhead.get_last_move_time() + for mcu_endstop, name in endstops: + did_trigger = mcu_endstop.home_wait(move_end_print_time) + if not did_trigger and error is None: + error = "Failed to home %s: Timeout during homing" % (name,) + # Determine stepper halt positions + self.toolhead.flush_step_generation() + end_mcu_pos = [(s, name, spos, s.get_mcu_position()) + for s, name, spos in start_mcu_pos] + if probe_pos: + for s, name, spos, epos in end_mcu_pos: + md = (epos - spos) * s.get_step_dist() + s.set_tag_position(s.get_tag_position() + md) + self.set_homed_position(kin.calc_tag_position()) + else: + self.toolhead.set_position(movepos) + # Signal homing/probing move complete + try: + self.printer.send_event("homing:homing_move_end", + [es for es, name in endstops]) + except CommandError as e: + if error is None: + error = str(e) + if error is not None: + raise CommandError(error) + # Check if some movement occurred + if verify_movement: + for s, name, spos, epos in end_mcu_pos: + if spos == epos: + if probe_pos: + raise CommandError("Probe triggered prior to movement") + raise CommandError( + "Endstop %s still triggered after retract" % (name,)) + def home_rails(self, rails, forcepos, movepos): + # Notify of upcoming homing operation + self.printer.send_event("homing:home_rails_begin", self, rails) + # Alter kinematics class to think printer is at forcepos + homing_axes = [axis for axis in range(3) if forcepos[axis] is not None] + forcepos = self._fill_coord(forcepos) + movepos = self._fill_coord(movepos) + self.toolhead.set_position(forcepos, homing_axes=homing_axes) + # Perform first home + endstops = [es for rail in rails for es in rail.get_endstops()] + hi = rails[0].get_homing_info() + self.homing_move(movepos, endstops, hi.speed) + # Perform second home + if hi.retract_dist: + # Retract + axes_d = [mp - fp for mp, fp in zip(movepos, forcepos)] + move_d = math.sqrt(sum([d*d for d in axes_d[:3]])) + retract_r = min(1., hi.retract_dist / move_d) + retractpos = [mp - ad * retract_r + for mp, ad in zip(movepos, axes_d)] + self.toolhead.move(retractpos, hi.retract_speed) + # Home again + forcepos = [rp - ad * retract_r + for rp, ad in zip(retractpos, axes_d)] + self.toolhead.set_position(forcepos) + self.homing_move(movepos, endstops, hi.second_homing_speed, + verify_movement=self.verify_retract) + # Signal home operation complete + self.toolhead.flush_step_generation() + kin = self.toolhead.get_kinematics() + for s in kin.get_steppers(): + s.set_tag_position(s.get_commanded_position()) + ret = self.printer.send_event("homing:home_rails_end", self, rails) + if any(ret): + # Apply any homing offsets + adjustpos = kin.calc_tag_position() + for axis in homing_axes: + movepos[axis] = adjustpos[axis] + self.toolhead.set_position(movepos) + def home_axes(self, axes): + self.changed_axes = axes + try: + self.toolhead.get_kinematics().home(self) + except CommandError: + self.printer.lookup_object('stepper_enable').motor_off() + raise + +# Return a completion that completes when all completions in a list complete +def multi_complete(printer, completions): + if len(completions) == 1: + return completions[0] + cb = (lambda e: all([c.wait() for c in completions])) + return printer.get_reactor().register_callback(cb) + +class PrinterHoming: + def __init__(self, config): + self.printer = config.get_printer() + # Register g-code commands + gcode = self.printer.lookup_object('gcode') + gcode.register_command('G28', self.cmd_G28) + def new_homing_state(self): + return Homing(self.printer) + def cmd_G28(self, gcmd): + # Move to origin + axes = [] + for pos, axis in enumerate('XYZ'): + if gcmd.get(axis, None) is not None: + axes.append(pos) + if not axes: + axes = [0, 1, 2] + homing_state = Homing(self.printer) + homing_state.home_axes(axes) + +def load_config(config): + return PrinterHoming(config) diff --git a/klippy/extras/homing_override.py b/klippy/extras/homing_override.py index 149bd022..a1cf94b5 100644 --- a/klippy/extras/homing_override.py +++ b/klippy/extras/homing_override.py @@ -13,7 +13,7 @@ class HomingOverride: gcode_macro = self.printer.load_object(config, 'gcode_macro') self.template = gcode_macro.load_template(config, 'gcode') self.in_script = False - self.printer.load_object(config, 'gcode_move') + self.printer.load_object(config, 'homing') self.gcode = self.printer.lookup_object('gcode') self.prev_G28 = self.gcode.register_command("G28", None) self.gcode.register_command("G28", self.cmd_G28) diff --git a/klippy/extras/probe.py b/klippy/extras/probe.py index e1115e0e..fe6cb151 100644 --- a/klippy/extras/probe.py +++ b/klippy/extras/probe.py @@ -1,10 +1,10 @@ # Z-Probe support # -# Copyright (C) 2017-2020 Kevin O'Connor +# Copyright (C) 2017-2021 Kevin O'Connor # # This file may be distributed under the terms of the GNU GPLv3 license. import logging -import pins, homing +import pins from . import manual_probe HINT_TIMEOUT = """ @@ -75,11 +75,11 @@ class PrinterProbe: def _handle_homing_move_end(self, endstops): if self.mcu_probe in endstops: self.mcu_probe.probe_finish() - def _handle_home_rails_begin(self, rails): + def _handle_home_rails_begin(self, homing_state, rails): endstops = [es for rail in rails for es, name in rail.get_endstops()] if self.mcu_probe in endstops: self.multi_probe_begin() - def _handle_home_rails_end(self, rails): + def _handle_home_rails_end(self, homing_state, rails): endstops = [es for rail in rails for es, name in rail.get_endstops()] if self.mcu_probe in endstops: self.multi_probe_end() @@ -112,7 +112,7 @@ class PrinterProbe: curtime = self.printer.get_reactor().monotonic() if 'z' not in toolhead.get_status(curtime)['homed_axes']: raise self.printer.command_error("Must home before probe") - homing_state = homing.Homing(self.printer) + homing_state = self.printer.lookup_object('homing').new_homing_state() pos = toolhead.get_position() pos[2] = self.z_position endstops = [(self.mcu_probe, "probe")] diff --git a/klippy/extras/safe_z_home.py b/klippy/extras/safe_z_home.py index 93536274..6dd2f292 100644 --- a/klippy/extras/safe_z_home.py +++ b/klippy/extras/safe_z_home.py @@ -20,7 +20,7 @@ class SafeZHoming: self.max_z = zconfig.getfloat('position_max', note_valid=False) self.speed = config.getfloat('speed', 50.0, above=0.) self.move_to_previous = config.getboolean('move_to_previous', False) - self.printer.load_object(config, 'gcode_move') + self.printer.load_object(config, 'homing') self.gcode = self.printer.lookup_object('gcode') self.prev_G28 = self.gcode.register_command("G28", None) self.gcode.register_command("G28", self.cmd_G28) diff --git a/klippy/homing.py b/klippy/homing.py deleted file mode 100644 index 49f5a8dc..00000000 --- a/klippy/homing.py +++ /dev/null @@ -1,159 +0,0 @@ -# Code for state tracking during homing operations -# -# Copyright (C) 2016-2021 Kevin O'Connor -# -# This file may be distributed under the terms of the GNU GPLv3 license. -import logging, math, collections - -HOMING_START_DELAY = 0.001 -ENDSTOP_SAMPLE_TIME = .000015 -ENDSTOP_SAMPLE_COUNT = 4 - -class Homing: - def __init__(self, printer): - self.printer = printer - self.toolhead = printer.lookup_object('toolhead') - self.changed_axes = [] - self.verify_retract = True - if self.printer.get_start_args().get("debuginput"): - self.verify_retract = False - def set_axes(self, axes): - self.changed_axes = axes - def get_axes(self): - return self.changed_axes - def _fill_coord(self, coord): - # Fill in any None entries in 'coord' with current toolhead position - thcoord = list(self.toolhead.get_position()) - for i in range(len(coord)): - if coord[i] is not None: - thcoord[i] = coord[i] - return thcoord - def set_homed_position(self, pos): - self.toolhead.set_position(self._fill_coord(pos)) - def _calc_endstop_rate(self, mcu_endstop, movepos, speed): - startpos = self.toolhead.get_position() - axes_d = [mp - sp for mp, sp in zip(movepos, startpos)] - move_d = math.sqrt(sum([d*d for d in axes_d[:3]])) - move_t = move_d / speed - max_steps = max([(abs(s.calc_position_from_coord(startpos) - - s.calc_position_from_coord(movepos)) - / s.get_step_dist()) - for s in mcu_endstop.get_steppers()]) - if max_steps <= 0.: - return .001 - return move_t / max_steps - def homing_move(self, movepos, endstops, speed, - probe_pos=False, verify_movement=False): - # Notify start of homing/probing move - self.printer.send_event("homing:homing_move_begin", - [es for es, name in endstops]) - # Note start location - self.toolhead.flush_step_generation() - kin = self.toolhead.get_kinematics() - for s in kin.get_steppers(): - s.set_tag_position(s.get_commanded_position()) - start_mcu_pos = [(s, name, s.get_mcu_position()) - for es, name in endstops for s in es.get_steppers()] - # Start endstop checking - print_time = self.toolhead.get_last_move_time() - endstop_triggers = [] - for mcu_endstop, name in endstops: - rest_time = self._calc_endstop_rate(mcu_endstop, movepos, speed) - wait = mcu_endstop.home_start(print_time, ENDSTOP_SAMPLE_TIME, - ENDSTOP_SAMPLE_COUNT, rest_time) - endstop_triggers.append(wait) - all_endstop_trigger = multi_complete(self.printer, endstop_triggers) - self.toolhead.dwell(HOMING_START_DELAY) - # Issue move - error = None - try: - self.toolhead.drip_move(movepos, speed, all_endstop_trigger) - except CommandError as e: - error = "Error during homing move: %s" % (str(e),) - # Wait for endstops to trigger - move_end_print_time = self.toolhead.get_last_move_time() - for mcu_endstop, name in endstops: - did_trigger = mcu_endstop.home_wait(move_end_print_time) - if not did_trigger and error is None: - error = "Failed to home %s: Timeout during homing" % (name,) - # Determine stepper halt positions - self.toolhead.flush_step_generation() - end_mcu_pos = [(s, name, spos, s.get_mcu_position()) - for s, name, spos in start_mcu_pos] - if probe_pos: - for s, name, spos, epos in end_mcu_pos: - md = (epos - spos) * s.get_step_dist() - s.set_tag_position(s.get_tag_position() + md) - self.set_homed_position(kin.calc_tag_position()) - else: - self.toolhead.set_position(movepos) - # Signal homing/probing move complete - try: - self.printer.send_event("homing:homing_move_end", - [es for es, name in endstops]) - except CommandError as e: - if error is None: - error = str(e) - if error is not None: - raise CommandError(error) - # Check if some movement occurred - if verify_movement: - for s, name, spos, epos in end_mcu_pos: - if spos == epos: - if probe_pos: - raise CommandError("Probe triggered prior to movement") - raise CommandError( - "Endstop %s still triggered after retract" % (name,)) - def home_rails(self, rails, forcepos, movepos): - # Notify of upcoming homing operation - self.printer.send_event("homing:home_rails_begin", rails) - # Alter kinematics class to think printer is at forcepos - homing_axes = [axis for axis in range(3) if forcepos[axis] is not None] - forcepos = self._fill_coord(forcepos) - movepos = self._fill_coord(movepos) - self.toolhead.set_position(forcepos, homing_axes=homing_axes) - # Perform first home - endstops = [es for rail in rails for es in rail.get_endstops()] - hi = rails[0].get_homing_info() - self.homing_move(movepos, endstops, hi.speed) - # Perform second home - if hi.retract_dist: - # Retract - axes_d = [mp - fp for mp, fp in zip(movepos, forcepos)] - move_d = math.sqrt(sum([d*d for d in axes_d[:3]])) - retract_r = min(1., hi.retract_dist / move_d) - retractpos = [mp - ad * retract_r - for mp, ad in zip(movepos, axes_d)] - self.toolhead.move(retractpos, hi.retract_speed) - # Home again - forcepos = [rp - ad * retract_r - for rp, ad in zip(retractpos, axes_d)] - self.toolhead.set_position(forcepos) - self.homing_move(movepos, endstops, hi.second_homing_speed, - verify_movement=self.verify_retract) - # Signal home operation complete - self.toolhead.flush_step_generation() - kin = self.toolhead.get_kinematics() - for s in kin.get_steppers(): - s.set_tag_position(s.get_commanded_position()) - ret = self.printer.send_event("homing:home_rails_end", rails) - if any(ret): - # Apply any homing offsets - adjustpos = kin.calc_tag_position() - for axis in homing_axes: - movepos[axis] = adjustpos[axis] - self.toolhead.set_position(movepos) - def home_axes(self, axes): - self.changed_axes = axes - try: - self.toolhead.get_kinematics().home(self) - except CommandError: - self.printer.lookup_object('stepper_enable').motor_off() - raise - -# Return a completion that completes when all completions in a list complete -def multi_complete(printer, completions): - if len(completions) == 1: - return completions[0] - cb = (lambda e: all([c.wait() for c in completions])) - return printer.get_reactor().register_callback(cb) diff --git a/klippy/toolhead.py b/klippy/toolhead.py index 5c229dc7..c6ba130f 100644 --- a/klippy/toolhead.py +++ b/klippy/toolhead.py @@ -273,8 +273,8 @@ class ToolHead: desc=self.cmd_SET_VELOCITY_LIMIT_help) gcode.register_command('M204', self.cmd_M204) # Load some default modules - modules = ["gcode_move", "idle_timeout", "statistics", "manual_probe", - "tuning_tower"] + modules = ["gcode_move", "homing", "idle_timeout", "statistics", + "manual_probe", "tuning_tower"] for module_name in modules: self.printer.load_object(config, module_name) # Print time tracking -- cgit v1.2.3-70-g09d2