From 1e71909d020840863497977720289763d58fb046 Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Wed, 10 Oct 2018 22:15:00 -0400 Subject: [PATCH] endstop_phase: Add support for an ENDSTOP_PHASE_CALIBRATE command Signed-off-by: Kevin O'Connor --- config/example-extras.cfg | 3 +- docs/G-Codes.md | 11 ++++ klippy/extras/endstop_phase.py | 96 ++++++++++++++++++++++++++++++++++ 3 files changed, 109 insertions(+), 1 deletion(-) diff --git a/config/example-extras.cfg b/config/example-extras.cfg index aed162ee..978a1a32 100644 --- a/config/example-extras.cfg +++ b/config/example-extras.cfg @@ -326,7 +326,8 @@ # config section with an "endstop_phase" prefix followed by the name # of the corresponding stepper config section (for example, # "[endstop_phase stepper_z]"). This feature can improve the accuracy -# of endstop switches. +# of endstop switches. Add a bare "[endstop_phase]" declaration to +# enable the ENDSTOP_PHASE_CALIBRATE command. #[endstop_phase stepper_z] #phases: # This specifies the number of phases of the given stepper motor diff --git a/docs/G-Codes.md b/docs/G-Codes.md index ca83d2fa..f5135d27 100644 --- a/docs/G-Codes.md +++ b/docs/G-Codes.md @@ -221,6 +221,17 @@ is enabled: - `DUMP_TMC STEPPER=`: This command will read the TMC2130 driver registers and report their values. +## Endstop adjustments by stepper phase + +The following commands are available when an "endstop_phase" config +section is enabled: +- `ENDSTOP_PHASE_CALIBRATE [STEPPER=]`: If no STEPPER + parameter is provided then this command will reports statistics on + endstop stepper phases during past homing operations. When a STEPPER + parameter is provided it arranges for the given endstop phase + setting to be written to the config file (in conjunction with the + SAVE_CONFIG command). + ## Force movement The following commands are available when the "force_move" config diff --git a/klippy/extras/endstop_phase.py b/klippy/extras/endstop_phase.py index 552bcceb..81d9b147 100644 --- a/klippy/extras/endstop_phase.py +++ b/klippy/extras/endstop_phase.py @@ -43,9 +43,11 @@ class EndstopPhase: " stepper phase adjustment" % (self.name,)) if self.printer.get_start_args().get('debugoutput') is not None: self.endstop_accuracy = self.phases + self.phase_history = [0] * self.phases # Register event handler self.printer.register_event_handler( "homing:homed_rails", self.handle_homed_rails) + self.printer.try_load_module(config, "endstop_phase") def align_endstop(self, pos): if not self.endstop_align_zero or self.endstop_phase is None: return pos @@ -66,6 +68,7 @@ class EndstopPhase: raise homing.EndstopError(msg) else: phase = stepper.get_mcu_position() % self.phases + self.phase_history[phase] += 1 if self.endstop_phase is None: logging.info("Setting %s endstop phase to %d", self.name, phase) self.endstop_phase = phase @@ -91,5 +94,98 @@ class EndstopPhase: rail.set_commanded_position(pos) return True +class EndstopPhases: + def __init__(self, config): + self.printer = config.get_printer() + self.gcode = self.printer.lookup_object('gcode') + self.gcode.register_command("ENDSTOP_PHASE_CALIBRATE", + self.cmd_ENDSTOP_PHASE_CALIBRATE, + desc=self.cmd_ENDSTOP_PHASE_CALIBRATE_help) + self.tracking = {} + # Register event handler + self.printer.register_event_handler( + "homing:homed_rails", self.handle_homed_rails) + def lookup_rail(self, stepper, stepper_name): + mod_name = "endstop_phase %s" % (stepper_name,) + m = self.printer.lookup_object(mod_name, None) + if m is not None: + return (None, m.phase_history) + for driver in TRINAMIC_DRIVERS: + mod_name = "%s %s" % (driver, stepper_name) + m = self.printer.lookup_object(mod_name, None) + if m is not None: + return (m.get_phase, [0] * (m.get_microsteps() * 4)) + return None + def update_rail(self, info, stepper): + if info is None: + return + get_phase, phase_history = info + if get_phase is None: + return + try: + phase = get_phase() + except: + logging.exception("Error in EndstopPhases get_phase") + return + phase_history[phase] += 1 + def handle_homed_rails(self, homing_state, rails): + for rail in rails: + stepper = rail.get_steppers()[0] + stepper_name = stepper.get_name() + if stepper_name not in self.tracking: + info = self.lookup_rail(stepper, stepper_name) + self.tracking[stepper_name] = info + self.update_rail(self.tracking[stepper_name], stepper) + cmd_ENDSTOP_PHASE_CALIBRATE_help = "Calibrate stepper phase" + def cmd_ENDSTOP_PHASE_CALIBRATE(self, params): + stepper_name = self.gcode.get_str('STEPPER', params, None) + if stepper_name is None: + self.report_stats() + return + info = self.tracking.get(stepper_name) + if info is None: + raise self.gcode.error("Stats not available for stepper %s" % ( + stepper_name,)) + endstop_phase = self.generate_stats(stepper_name, info) + configfile = self.printer.lookup_object('configfile') + section = 'endstop_phase %s' % stepper_name + configfile.remove_section(section) + configfile.set(section, "endstop_phase", endstop_phase) + self.gcode.respond_info( + "The SAVE_CONFIG command will update the printer config\n" + "file with these parameters and restart the printer.") + def generate_stats(self, stepper_name, info): + get_phase, phase_history = info + wph = phase_history + phase_history + count = sum(phase_history) + phases = len(phase_history) + half_phases = phases // 2 + res = [] + for i in range(phases): + phase = i + half_phases + cost = sum([wph[j] * abs(j-phase) for j in range(i, i+phases)]) + res.append((cost, phase)) + res.sort() + best = res[0][1] + found = [j for j in range(best - half_phases, best + half_phases) + if wph[j]] + best_phase = best % phases + lo, hi = found[0] % phases, found[-1] % phases + self.gcode.respond_info("%s: endstop_phase=%d (range %d to %d)" % ( + stepper_name, best_phase, lo, hi)) + return best_phase + def report_stats(self): + if not self.tracking: + self.gcode.respond_info( + "No steppers found. (Be sure to home at least once.)") + return + for stepper_name, info in sorted(self.tracking.items()): + if info is None: + continue + self.generate_stats(stepper_name, info) + def load_config_prefix(config): return EndstopPhase(config) + +def load_config(config): + return EndstopPhases(config)