diff options
Diffstat (limited to 'klippy/extras/homing.py')
| -rw-r--r-- | klippy/extras/homing.py | 73 |
1 files changed, 54 insertions, 19 deletions
diff --git a/klippy/extras/homing.py b/klippy/extras/homing.py index 67a19513..220877e1 100644 --- a/klippy/extras/homing.py +++ b/klippy/extras/homing.py @@ -3,7 +3,7 @@ # Copyright (C) 2016-2021 Kevin O'Connor <kevin@koconnor.net> # # This file may be distributed under the terms of the GNU GPLv3 license. -import logging, math, collections +import logging, math HOMING_START_DELAY = 0.001 ENDSTOP_SAMPLE_TIME = .000015 @@ -21,6 +21,18 @@ def multi_complete(printer, completions): reactor.register_callback(lambda e: cp.complete(1) if c.wait() else 0) return cp +# Tracking of stepper positions during a homing/probing move +class StepperPosition: + def __init__(self, stepper, endstop_name): + self.stepper = stepper + self.endstop_name = endstop_name + self.stepper_name = stepper.get_name() + self.start_pos = stepper.get_mcu_position() + self.halt_pos = self.trig_pos = None + def note_home_end(self, trigger_time): + self.halt_pos = self.stepper.get_mcu_position() + self.trig_pos = self.stepper.get_past_mcu_position(trigger_time) + # Implementation of homing/probing moves class HomingMove: def __init__(self, printer, endstops, toolhead=None): @@ -29,7 +41,7 @@ class HomingMove: if toolhead is None: toolhead = printer.lookup_object('toolhead') self.toolhead = toolhead - self.end_mcu_pos = [] + self.stepper_positions = [] def get_mcu_endstops(self): return [es for es, name in self.endstops] def _calc_endstop_rate(self, mcu_endstop, movepos, speed): @@ -44,6 +56,14 @@ class HomingMove: if max_steps <= 0.: return .001 return move_t / max_steps + def calc_toolhead_pos(self, kin_spos, offsets): + kin_spos = dict(kin_spos) + kin = self.toolhead.get_kinematics() + for stepper in kin.get_steppers(): + sname = stepper.get_name() + kin_spos[sname] += offsets.get(sname, 0) * stepper.get_step_dist() + thpos = self.toolhead.get_position() + return list(kin.calc_position(kin_spos))[:3] + thpos[3:] def homing_move(self, movepos, speed, probe_pos=False, triggered=True, check_triggered=True): # Notify start of homing/probing move @@ -53,9 +73,9 @@ class HomingMove: kin = self.toolhead.get_kinematics() kin_spos = {s.get_name(): s.get_commanded_position() for s in kin.get_steppers()} - start_mcu_pos = [(s, name, s.get_mcu_position()) - for es, name in self.endstops - for s in es.get_steppers()] + self.stepper_positions = [ StepperPosition(s, name) + for es, name in self.endstops + for s in es.get_steppers() ] # Start endstop checking print_time = self.toolhead.get_last_move_time() endstop_triggers = [] @@ -74,24 +94,39 @@ class HomingMove: except self.printer.command_error as e: error = "Error during homing move: %s" % (str(e),) # Wait for endstops to trigger + trigger_times = {} move_end_print_time = self.toolhead.get_last_move_time() for mcu_endstop, name in self.endstops: trigger_time = mcu_endstop.home_wait(move_end_print_time) - if trigger_time < 0. and error is None: + if trigger_time > 0.: + trigger_times[name] = trigger_time + elif trigger_time < 0. and error is None: error = "Communication timeout during homing %s" % (name,) - elif not trigger_time and check_triggered and error is None: + elif check_triggered and error is None: error = "No trigger on %s after full movement" % (name,) # Determine stepper halt positions self.toolhead.flush_step_generation() - self.end_mcu_pos = [(s, name, spos, s.get_mcu_position()) - for s, name, spos in start_mcu_pos] + for sp in self.stepper_positions: + tt = trigger_times.get(sp.endstop_name, move_end_print_time) + sp.note_home_end(tt) if probe_pos: - for s, name, spos, epos in self.end_mcu_pos: - sname = s.get_name() - if sname in kin_spos: - kin_spos[sname] += (epos - spos) * s.get_step_dist() - movepos = list(kin.calc_position(kin_spos))[:3] + movepos[3:] - self.toolhead.set_position(movepos) + halt_steps = {sp.stepper_name: sp.halt_pos - sp.start_pos + for sp in self.stepper_positions} + trig_steps = {sp.stepper_name: sp.trig_pos - sp.start_pos + for sp in self.stepper_positions} + haltpos = trigpos = self.calc_toolhead_pos(kin_spos, trig_steps) + if trig_steps != halt_steps: + haltpos = self.calc_toolhead_pos(kin_spos, halt_steps) + else: + haltpos = trigpos = movepos + over_steps = {sp.stepper_name: sp.halt_pos - sp.trig_pos + for sp in self.stepper_positions} + if any(over_steps.values()): + self.toolhead.set_position(movepos) + halt_kin_spos = {s.get_name(): s.get_commanded_position() + for s in kin.get_steppers()} + haltpos = self.calc_toolhead_pos(halt_kin_spos, over_steps) + self.toolhead.set_position(haltpos) # Signal homing/probing move complete try: self.printer.send_event("homing:homing_move_end", self) @@ -100,13 +135,13 @@ class HomingMove: error = str(e) if error is not None: raise self.printer.command_error(error) - return movepos + return trigpos def check_no_movement(self): if self.printer.get_start_args().get('debuginput') is not None: return None - for s, name, spos, epos in self.end_mcu_pos: - if spos == epos: - return name + for sp in self.stepper_positions: + if sp.start_pos == sp.trig_pos: + return sp.endstop_name return None # State tracking of homing requests |