232 lines
11 KiB
Python
232 lines
11 KiB
Python
# Endstop accuracy improvement via stepper phase tracking
|
|
#
|
|
# Copyright (C) 2016-2021 Kevin O'Connor <kevin@koconnor.net>
|
|
#
|
|
# This file may be distributed under the terms of the GNU GPLv3 license.
|
|
import math, logging
|
|
import stepper
|
|
|
|
TRINAMIC_DRIVERS = ["tmc2130", "tmc2208", "tmc2209", "tmc2660", "tmc5160"]
|
|
|
|
# Calculate the trigger phase of a stepper motor
|
|
class PhaseCalc:
|
|
def __init__(self, printer, name, phases=None):
|
|
self.printer = printer
|
|
self.name = name
|
|
self.phases = phases
|
|
self.tmc_module = None
|
|
# Statistics tracking for ENDSTOP_PHASE_CALIBRATE
|
|
self.phase_history = self.last_phase = self.last_mcu_position = None
|
|
self.is_primary = self.stats_only = False
|
|
def lookup_tmc(self):
|
|
for driver in TRINAMIC_DRIVERS:
|
|
driver_name = "%s %s" % (driver, self.name)
|
|
module = self.printer.lookup_object(driver_name, None)
|
|
if module is not None:
|
|
self.tmc_module = module
|
|
if self.phases is None:
|
|
phase_offset, self.phases = module.get_phase_offset()
|
|
break
|
|
if self.phases is not None:
|
|
self.phase_history = [0] * self.phases
|
|
def convert_phase(self, driver_phase, driver_phases):
|
|
phases = self.phases
|
|
return (int(float(driver_phase) / driver_phases * phases + .5) % phases)
|
|
def calc_phase(self, stepper, trig_mcu_pos):
|
|
mcu_phase_offset = 0
|
|
if self.tmc_module is not None:
|
|
mcu_phase_offset, phases = self.tmc_module.get_phase_offset()
|
|
if mcu_phase_offset is None:
|
|
if self.printer.get_start_args().get('debugoutput') is None:
|
|
raise self.printer.command_error("Stepper %s phase unknown"
|
|
% (self.name,))
|
|
mcu_phase_offset = 0
|
|
phase = (trig_mcu_pos + mcu_phase_offset) % self.phases
|
|
self.phase_history[phase] += 1
|
|
self.last_phase = phase
|
|
self.last_mcu_position = trig_mcu_pos
|
|
return phase
|
|
|
|
# Adjusted endstop trigger positions
|
|
class EndstopPhase:
|
|
def __init__(self, config):
|
|
self.printer = config.get_printer()
|
|
self.name = config.get_name().split()[1]
|
|
# Obtain step_distance and microsteps from stepper config section
|
|
sconfig = config.getsection(self.name)
|
|
rotation_dist, steps_per_rotation = stepper.parse_step_distance(sconfig)
|
|
self.step_dist = rotation_dist / steps_per_rotation
|
|
self.phases = sconfig.getint("microsteps", note_valid=False) * 4
|
|
self.phase_calc = PhaseCalc(self.printer, self.name, self.phases)
|
|
# Register event handlers
|
|
self.printer.register_event_handler("klippy:connect",
|
|
self.phase_calc.lookup_tmc)
|
|
self.printer.register_event_handler("homing:home_rails_end",
|
|
self.handle_home_rails_end)
|
|
self.printer.load_object(config, "endstop_phase")
|
|
# Read config
|
|
self.endstop_phase = None
|
|
trigger_phase = config.get('trigger_phase', None)
|
|
if trigger_phase is not None:
|
|
p, ps = config.getintlist('trigger_phase', sep='/', count=2)
|
|
if p >= ps:
|
|
raise config.error("Invalid trigger_phase '%s'"
|
|
% (trigger_phase,))
|
|
self.endstop_phase = self.phase_calc.convert_phase(p, ps)
|
|
self.endstop_align_zero = config.getboolean('endstop_align_zero', False)
|
|
self.endstop_accuracy = config.getfloat('endstop_accuracy', None,
|
|
above=0.)
|
|
# Determine endstop accuracy
|
|
if self.endstop_accuracy is None:
|
|
self.endstop_phase_accuracy = self.phases//2 - 1
|
|
elif self.endstop_phase is not None:
|
|
self.endstop_phase_accuracy = int(
|
|
math.ceil(self.endstop_accuracy * .5 / self.step_dist))
|
|
else:
|
|
self.endstop_phase_accuracy = int(
|
|
math.ceil(self.endstop_accuracy / self.step_dist))
|
|
if self.endstop_phase_accuracy >= self.phases // 2:
|
|
raise config.error("Endstop for %s is not accurate enough for"
|
|
" stepper phase adjustment" % (self.name,))
|
|
if self.printer.get_start_args().get('debugoutput') is not None:
|
|
self.endstop_phase_accuracy = self.phases
|
|
def align_endstop(self, rail):
|
|
if not self.endstop_align_zero or self.endstop_phase is None:
|
|
return 0.
|
|
# Adjust the endstop position so 0.0 is always at a full step
|
|
microsteps = self.phases // 4
|
|
half_microsteps = microsteps // 2
|
|
phase_offset = (((self.endstop_phase + half_microsteps) % microsteps)
|
|
- half_microsteps) * self.step_dist
|
|
full_step = microsteps * self.step_dist
|
|
pe = rail.get_homing_info().position_endstop
|
|
return int(pe / full_step + .5) * full_step - pe + phase_offset
|
|
def get_homed_offset(self, stepper, trig_mcu_pos):
|
|
phase = self.phase_calc.calc_phase(stepper, trig_mcu_pos)
|
|
if self.endstop_phase is None:
|
|
logging.info("Setting %s endstop phase to %d", self.name, phase)
|
|
self.endstop_phase = phase
|
|
return 0.
|
|
delta = (phase - self.endstop_phase) % self.phases
|
|
if delta >= self.phases - self.endstop_phase_accuracy:
|
|
delta -= self.phases
|
|
elif delta > self.endstop_phase_accuracy:
|
|
raise self.printer.command_error(
|
|
"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, homing_state, rails):
|
|
for rail in rails:
|
|
stepper = rail.get_steppers()[0]
|
|
if stepper.get_name() == self.name:
|
|
trig_mcu_pos = homing_state.get_trigger_position(self.name)
|
|
align = self.align_endstop(rail)
|
|
offset = self.get_homed_offset(stepper, trig_mcu_pos)
|
|
homing_state.set_stepper_adjustment(self.name, align + offset)
|
|
return
|
|
|
|
# Support for ENDSTOP_PHASE_CALIBRATE command
|
|
class EndstopPhases:
|
|
def __init__(self, config):
|
|
self.printer = config.get_printer()
|
|
self.tracking = {}
|
|
# Register handlers
|
|
self.printer.register_event_handler("homing:home_rails_end",
|
|
self.handle_home_rails_end)
|
|
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)
|
|
def update_stepper(self, stepper, trig_mcu_pos, is_primary):
|
|
stepper_name = stepper.get_name()
|
|
phase_calc = self.tracking.get(stepper_name)
|
|
if phase_calc is None:
|
|
# Check if stepper has an endstop_phase config section defined
|
|
mod_name = "endstop_phase %s" % (stepper_name,)
|
|
m = self.printer.lookup_object(mod_name, None)
|
|
if m is not None:
|
|
phase_calc = m.phase_calc
|
|
else:
|
|
# Create new PhaseCalc tracker
|
|
phase_calc = PhaseCalc(self.printer, stepper_name)
|
|
phase_calc.stats_only = True
|
|
phase_calc.lookup_tmc()
|
|
self.tracking[stepper_name] = phase_calc
|
|
if phase_calc.phase_history is None:
|
|
return
|
|
if is_primary:
|
|
phase_calc.is_primary = True
|
|
if phase_calc.stats_only:
|
|
phase_calc.calc_phase(stepper, trig_mcu_pos)
|
|
def handle_home_rails_end(self, homing_state, rails):
|
|
for rail in rails:
|
|
is_primary = True
|
|
for stepper in rail.get_steppers():
|
|
sname = stepper.get_name()
|
|
trig_mcu_pos = homing_state.get_trigger_position(sname)
|
|
self.update_stepper(stepper, trig_mcu_pos, is_primary)
|
|
is_primary = False
|
|
cmd_ENDSTOP_PHASE_CALIBRATE_help = "Calibrate stepper phase"
|
|
def cmd_ENDSTOP_PHASE_CALIBRATE(self, gcmd):
|
|
stepper_name = gcmd.get('STEPPER', None)
|
|
if stepper_name is None:
|
|
self.report_stats()
|
|
return
|
|
phase_calc = self.tracking.get(stepper_name)
|
|
if phase_calc is None or phase_calc.phase_history is None:
|
|
raise gcmd.error("Stats not available for stepper %s"
|
|
% (stepper_name,))
|
|
endstop_phase, phases = self.generate_stats(stepper_name, phase_calc)
|
|
if not phase_calc.is_primary:
|
|
return
|
|
configfile = self.printer.lookup_object('configfile')
|
|
section = 'endstop_phase %s' % (stepper_name,)
|
|
configfile.remove_section(section)
|
|
configfile.set(section, "trigger_phase",
|
|
"%s/%s" % (endstop_phase, phases))
|
|
gcmd.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, phase_calc):
|
|
phase_history = phase_calc.phase_history
|
|
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: trigger_phase=%d/%d (range %d to %d)"
|
|
% (stepper_name, best_phase, phases, lo, hi))
|
|
return best_phase, phases
|
|
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 in sorted(self.tracking.keys()):
|
|
phase_calc = self.tracking[stepper_name]
|
|
if phase_calc is None or not phase_calc.is_primary:
|
|
continue
|
|
self.generate_stats(stepper_name, phase_calc)
|
|
def get_status(self, eventtime):
|
|
lh = { name: {'phase': pc.last_phase, 'phases': pc.phases,
|
|
'mcu_position': pc.last_mcu_position}
|
|
for name, pc in self.tracking.items()
|
|
if pc.phase_history is not None }
|
|
return { 'last_home': lh }
|
|
|
|
def load_config_prefix(config):
|
|
return EndstopPhase(config)
|
|
|
|
def load_config(config):
|
|
return EndstopPhases(config)
|