klipper update

klippy/extras/homing.py

@@ -0,0 +1,277 @@
+# Helper code for implementing homing operations
+#
+# 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
+
+HOMING_START_DELAY = 0.001
+ENDSTOP_SAMPLE_TIME = .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:
+        return completions[0]
+    # Build completion that waits for all completions
+    reactor = printer.get_reactor()
+    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)
+    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):
+        self.printer = printer
+        self.endstops = endstops
+        if toolhead is None:
+            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_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 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
+        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() ]
+        # 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)
+            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 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.:
+                trigger_times[name] = trigger_time
+            elif trigger_time < 0. and error is None:
+                error = "Communication timeout during homing %s" % (name,)
+            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()
+        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:
+            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)
+        except self.printer.command_error as e:
+            if error is None:
+                error = str(e)
+        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:
+            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.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())
+        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 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]
+        startpos = self._fill_coord(forcepos)
+        homepos = self._fill_coord(movepos)
+        self.toolhead.set_position(startpos, 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()
+        hmove = HomingMove(self.printer, endstops)
+        hmove.homing_move(homepos, hi.speed)
+        # Perform second home
+        if hi.retract_dist:
+            # Retract
+            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)]
+            self.toolhead.move(retractpos, hi.retract_speed)
+            # Home again
+            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(),))
+        # 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.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()}
+            newpos = kin.calc_position(kin_spos)
+            for axis in homing_axes:
+                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):
+        hmove = HomingMove(self.printer, endstops, toolhead)
+        try:
+            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")
+            raise
+    def probing_move(self, mcu_probe, pos, speed):
+        endstops = [(mcu_probe, "probe")]
+        hmove = HomingMove(self.printer, endstops)
+        try:
+            epos = hmove.homing_move(pos, speed, probe_pos=True)
+        except self.printer.command_error:
+            if self.printer.is_shutdown():
+                raise self.printer.command_error(
+                    "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")
+        return epos
+    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.set_axes(axes)
+        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()
+            raise
+
+def load_config(config):
+    return PrinterHoming(config)