diff options
Diffstat (limited to 'klippy/extras/homing.py')
| -rw-r--r-- | klippy/extras/homing.py | 181 |
1 files changed, 118 insertions, 63 deletions
diff --git a/klippy/extras/homing.py b/klippy/extras/homing.py index 723648c1..0a2b1f32 100644 --- a/klippy/extras/homing.py +++ b/klippy/extras/homing.py @@ -6,9 +6,10 @@ import logging, math HOMING_START_DELAY = 0.001 -ENDSTOP_SAMPLE_TIME = .000015 +ENDSTOP_SAMPLE_TIME = 0.000015 ENDSTOP_SAMPLE_COUNT = 4 + # Return a completion that completes when all completions in a list complete def multi_complete(printer, completions): if len(completions) == 1: @@ -18,10 +19,10 @@ def multi_complete(printer, completions): cp = reactor.register_callback(lambda e: [c.wait() for c in completions]) # If any completion indicates an error, then exit main completion early for c in completions: - reactor.register_callback( - lambda e, c=c: cp.complete(1) if c.wait() else 0) + reactor.register_callback(lambda e, c=c: 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): @@ -31,15 +32,21 @@ class StepperPosition: self.start_pos = stepper.get_mcu_position() self.start_cmd_pos = stepper.mcu_to_commanded_position(self.start_pos) 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) + def verify_no_probe_skew(self, haltpos): new_start_pos = self.stepper.get_mcu_position(self.start_cmd_pos) if new_start_pos != self.start_pos: logging.warning( "Stepper '%s' position skew after probe: pos %d now %d", - self.stepper.get_name(), self.start_pos, new_start_pos) + self.stepper.get_name(), + self.start_pos, + new_start_pos, + ) + # Implementation of homing/probing moves class HomingMove: @@ -47,23 +54,34 @@ class HomingMove: self.printer = printer self.endstops = [es for es in endstops if es[0].get_steppers()] if toolhead is None: - toolhead = printer.lookup_object('toolhead') + toolhead = printer.lookup_object("toolhead") self.toolhead = toolhead 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): 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_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 + 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.0: + return 0.001 return move_t / max_steps + def calc_toolhead_pos(self, kin_spos, offsets): kin_spos = dict(kin_spos) kin = self.toolhead.get_kinematics() @@ -72,28 +90,38 @@ class HomingMove: kin_spos[sname] += offsets.get(sname, 0) * stepper.get_step_dist() thpos = self.toolhead.get_position() cpos = kin.calc_position(kin_spos) - return [cp if cp is not None else tp - for cp, tp in zip(cpos, thpos[:3])] + thpos[3:] - def homing_move(self, movepos, speed, probe_pos=False, - triggered=True, check_triggered=True): + return [ + cp if cp is not None else tp for cp, tp in zip(cpos, thpos[:3]) + ] + thpos[3:] + + def homing_move( + self, movepos, speed, probe_pos=False, triggered=True, check_triggered=True + ): # Notify start of homing/probing move self.printer.send_event("homing:homing_move_begin", self) # Note start location self.toolhead.flush_step_generation() kin = self.toolhead.get_kinematics() - kin_spos = {s.get_name(): s.get_commanded_position() - for s in kin.get_steppers()} - self.stepper_positions = [ StepperPosition(s, name) - for es, name in self.endstops - for s in es.get_steppers() ] + kin_spos = { + s.get_name(): s.get_commanded_position() for s in kin.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 = [] for mcu_endstop, name in self.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, - triggered=triggered) + wait = mcu_endstop.home_start( + print_time, + ENDSTOP_SAMPLE_TIME, + ENDSTOP_SAMPLE_COUNT, + rest_time, + triggered=triggered, + ) endstop_triggers.append(wait) all_endstop_trigger = multi_complete(self.printer, endstop_triggers) self.toolhead.dwell(HOMING_START_DELAY) @@ -113,7 +141,7 @@ class HomingMove: if error is None: error = "Error during homing %s: %s" % (name, str(e)) continue - if trigger_time > 0.: + if trigger_time > 0.0: trigger_times[name] = trigger_time elif check_triggered and error is None: error = "No trigger on %s after full movement" % (name,) @@ -123,10 +151,14 @@ class HomingMove: tt = trigger_times.get(sp.endstop_name, move_end_print_time) sp.note_home_end(tt) if probe_pos: - 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} + 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) @@ -135,12 +167,15 @@ class HomingMove: sp.verify_no_probe_skew(haltpos) else: haltpos = trigpos = movepos - over_steps = {sp.stepper_name: sp.halt_pos - sp.trig_pos - for sp in self.stepper_positions} + 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()} + 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 @@ -152,30 +187,37 @@ class HomingMove: if error is not None: raise self.printer.command_error(error) return trigpos + def check_no_movement(self): - if self.printer.get_start_args().get('debuginput') is not None: + if self.printer.get_start_args().get("debuginput") is not None: return None for sp in self.stepper_positions: if sp.start_pos == sp.trig_pos: return sp.endstop_name return None + # State tracking of homing requests class Homing: def __init__(self, printer): self.printer = printer - self.toolhead = printer.lookup_object('toolhead') + self.toolhead = printer.lookup_object("toolhead") self.changed_axes = [] self.trigger_mcu_pos = {} self.adjust_pos = {} + def set_axes(self, axes): self.changed_axes = axes + def get_axes(self): return self.changed_axes + def get_trigger_position(self, stepper_name): return self.trigger_mcu_pos[stepper_name] + def set_stepper_adjustment(self, stepper_name, adjustment): self.adjust_pos[stepper_name] = adjustment + def _fill_coord(self, coord): # Fill in any None entries in 'coord' with current toolhead position thcoord = list(self.toolhead.get_position()) @@ -183,8 +225,10 @@ class Homing: 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 home_rails(self, rails, forcepos, movepos): # Notify of upcoming homing operation self.printer.send_event("homing:home_rails_begin", self, rails) @@ -205,60 +249,68 @@ class Homing: startpos = self._fill_coord(forcepos) homepos = self._fill_coord(movepos) axes_d = [hp - sp for hp, sp in zip(homepos, startpos)] - move_d = math.sqrt(sum([d*d for d in axes_d[:3]])) - retract_r = min(1., hi.retract_dist / move_d) - retractpos = [hp - ad * retract_r - for hp, ad in zip(homepos, axes_d)] + move_d = math.sqrt(sum([d * d for d in axes_d[:3]])) + retract_r = min(1.0, hi.retract_dist / move_d) + retractpos = [hp - ad * retract_r for hp, ad in zip(homepos, axes_d)] self.toolhead.move(retractpos, hi.retract_speed) # Home again - startpos = [rp - ad * retract_r - for rp, ad in zip(retractpos, axes_d)] + startpos = [rp - ad * retract_r for rp, ad in zip(retractpos, axes_d)] self.toolhead.set_position(startpos) hmove = HomingMove(self.printer, endstops) hmove.homing_move(homepos, hi.second_homing_speed) if hmove.check_no_movement() is not None: raise self.printer.command_error( "Endstop %s still triggered after retract" - % (hmove.check_no_movement(),)) + % (hmove.check_no_movement(),) + ) # Signal home operation complete self.toolhead.flush_step_generation() - self.trigger_mcu_pos = {sp.stepper_name: sp.trig_pos - for sp in hmove.stepper_positions} + self.trigger_mcu_pos = { + sp.stepper_name: sp.trig_pos for sp in hmove.stepper_positions + } self.adjust_pos = {} self.printer.send_event("homing:home_rails_end", self, rails) if any(self.adjust_pos.values()): # Apply any homing offsets kin = self.toolhead.get_kinematics() homepos = self.toolhead.get_position() - kin_spos = {s.get_name(): (s.get_commanded_position() - + self.adjust_pos.get(s.get_name(), 0.)) - for s in kin.get_steppers()} + kin_spos = { + s.get_name(): ( + s.get_commanded_position() + self.adjust_pos.get(s.get_name(), 0.0) + ) + for s in kin.get_steppers() + } newpos = kin.calc_position(kin_spos) for axis in force_axes: if newpos[axis] is None: raise self.printer.command_error( - "Cannot determine position of toolhead on " - "axis %s after homing" % "xyz"[axis]) + "Cannot determine position of toolhead on " + "axis %s after homing" % "xyz"[axis] + ) homepos[axis] = newpos[axis] self.toolhead.set_position(homepos) + 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 manual_home(self, toolhead, endstops, pos, speed, - triggered, check_triggered): + gcode = self.printer.lookup_object("gcode") + gcode.register_command("G28", self.cmd_G28) + + def manual_home(self, toolhead, endstops, pos, speed, triggered, check_triggered): hmove = HomingMove(self.printer, endstops, toolhead) try: - hmove.homing_move(pos, speed, triggered=triggered, - check_triggered=check_triggered) + hmove.homing_move( + pos, speed, triggered=triggered, check_triggered=check_triggered + ) except self.printer.command_error: if self.printer.is_shutdown(): raise self.printer.command_error( - "Homing failed due to printer shutdown") + "Homing failed due to printer shutdown" + ) raise + def probing_move(self, mcu_probe, pos, speed): endstops = [(mcu_probe, "probe")] hmove = HomingMove(self.printer, endstops) @@ -267,31 +319,34 @@ class PrinterHoming: except self.printer.command_error: if self.printer.is_shutdown(): raise self.printer.command_error( - "Probing failed due to printer shutdown") + "Probing failed due to printer shutdown" + ) raise if hmove.check_no_movement() is not None: - raise self.printer.command_error( - "Probe triggered prior to movement") + raise self.printer.command_error("Probe triggered prior to movement") return epos + def cmd_G28(self, gcmd): # Move to origin axes = [] - for pos, axis in enumerate('XYZ'): + 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.set_axes(axes) - kin = self.printer.lookup_object('toolhead').get_kinematics() + kin = self.printer.lookup_object("toolhead").get_kinematics() try: kin.home(homing_state) except self.printer.command_error: if self.printer.is_shutdown(): raise self.printer.command_error( - "Homing failed due to printer shutdown") - self.printer.lookup_object('stepper_enable').motor_off() + "Homing failed due to printer shutdown" + ) + self.printer.lookup_object("stepper_enable").motor_off() raise + def load_config(config): return PrinterHoming(config) |