endstop_phase: Unify phase conversion code

Introduce a new PhaseCalc class and use that for both homing and
ENDSTOP_PHASE_CALIBRATE.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
Kevin O'Connor 2021-06-04 22:24:37 -04:00
parent 4596a244e3
commit be4fb7f128
1 changed files with 88 additions and 81 deletions

View File

@ -8,23 +8,65 @@ import stepper
TRINAMIC_DRIVERS = ["tmc2130", "tmc2208", "tmc2209", "tmc2660", "tmc5160"]
def convert_phase(driver_phase, driver_phases, phases):
return (int(float(driver_phase) / driver_phases * phases + .5) % phases)
# 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:
self.phases = module.get_microsteps() * 4
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):
mcu_pos = stepper.get_mcu_position()
if self.tmc_module is None:
phase = mcu_pos % self.phases
else:
try:
driver_phase, driver_phases = self.tmc_module.get_phase()
except Exception as e:
msg = "Unable to get stepper %s phase: %s" % (self.name, str(e))
logging.exception(msg)
raise self.printer.command_error(msg)
if stepper.is_dir_inverted():
driver_phase = (driver_phases - 1) - driver_phase
phase = self.convert_phase(driver_phase, driver_phases)
self.phase_history[phase] += 1
self.last_phase = phase
self.last_mcu_position = 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]
# Register event handlers
self.printer.register_event_handler("klippy:connect",
self.handle_connect)
self.printer.register_event_handler("homing:home_rails_end",
self.handle_home_rails_end)
self.printer.load_object(config, "endstop_phase")
# Obtain step_distance and microsteps from stepper config section
sconfig = config.getsection(self.name)
self.step_dist = stepper.parse_step_distance(sconfig)
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)
@ -37,7 +79,7 @@ class EndstopPhase:
if p >= ps:
raise config.error("Invalid trigger_phase '%s'"
% (trigger_phase,))
self.endstop_phase = convert_phase(p, ps, self.phases)
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.)
@ -55,16 +97,6 @@ class EndstopPhase:
" stepper phase adjustment" % (self.name,))
if self.printer.get_start_args().get('debugoutput') is not None:
self.endstop_phase_accuracy = self.phases
self.phase_history = [0] * self.phases
self.get_phase = None
def handle_connect(self):
# Check for trinamic driver with get_phase() method
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.get_phase = module.get_phase
break
def align_endstop(self, pos):
if not self.endstop_align_zero or self.endstop_phase is None:
return pos
@ -76,19 +108,7 @@ class EndstopPhase:
full_step = microsteps * self.step_dist
return int(pos / full_step + .5) * full_step + phase_offset
def get_homed_offset(self, stepper):
if self.get_phase is not None:
try:
driver_phase, driver_phases = self.get_phase()
except Exception as e:
msg = "Unable to get stepper %s phase: %s" % (self.name, str(e))
logging.exception(msg)
raise self.printer.command_error(msg)
if stepper.is_dir_inverted():
driver_phase = (driver_phases - 1) - driver_phase
phase = convert_phase(driver_phase, driver_phases, self.phases)
else:
phase = stepper.get_mcu_position() % self.phases
self.phase_history[phase] += 1
phase = self.phase_calc.calc_phase(stepper)
if self.endstop_phase is None:
logging.info("Setting %s endstop phase to %d", self.name, phase)
self.endstop_phase = phase
@ -113,11 +133,11 @@ class EndstopPhase:
kin_spos[self.name] = self.align_endstop(orig_pos) + offset
return
# Support for ENDSTOP_PHASE_CALIBRATE command
class EndstopPhases:
def __init__(self, config):
self.printer = config.get_printer()
self.tracking = {}
self.last_home_info = {}
# Register handlers
self.printer.register_event_handler("homing:home_rails_end",
self.handle_home_rails_end)
@ -125,62 +145,45 @@ class EndstopPhases:
self.gcode.register_command("ENDSTOP_PHASE_CALIBRATE",
self.cmd_ENDSTOP_PHASE_CALIBRATE,
desc=self.cmd_ENDSTOP_PHASE_CALIBRATE_help)
def lookup_stepper(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 {"get_phase": None, "is_rail": False,
"phase_history": m.phase_history}
for driver in TRINAMIC_DRIVERS:
mod_name = "%s %s" % (driver, stepper_name)
def update_stepper(self, stepper, 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:
return {"get_phase": m.get_phase, "is_rail": False,
"phase_history": [0] * (m.get_microsteps() * 4)}
return None
def update_stepper(self, stepper, is_rail):
stepper_name = stepper.get_name()
if stepper_name not in self.tracking:
info = self.lookup_stepper(stepper, stepper_name)
self.tracking[stepper_name] = info
info = self.tracking[stepper_name]
if info is 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_rail:
info["is_rail"] = True
get_phase = info["get_phase"]
if get_phase is None:
return
try:
driver_phase, driver_phases = get_phase()
except:
logging.exception("Error in EndstopPhases get_phase")
return
phase_history = info["phase_history"]
phase = convert_phase(driver_phase, driver_phases, len(phase_history))
phase_history[phase] += 1
self.last_home_info[stepper.get_name()] = {
'phase': phase, 'phases': len(phase_history),
'mcu_position': stepper.get_mcu_position()
}
if is_primary:
phase_calc.is_primary = True
if phase_calc.stats_only:
phase_calc.calc_phase(stepper)
def handle_home_rails_end(self, homing_state, rails):
for rail in rails:
is_rail = True
is_primary = True
for stepper in rail.get_steppers():
self.update_stepper(stepper, is_rail)
is_rail = False
self.update_stepper(stepper, 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
info = self.tracking.get(stepper_name)
if info is None:
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, info)
if not info["is_rail"]:
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,)
@ -190,8 +193,8 @@ class EndstopPhases:
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, info):
phase_history = info["phase_history"]
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)
@ -216,12 +219,16 @@ class EndstopPhases:
"No steppers found. (Be sure to home at least once.)")
return
for stepper_name in sorted(self.tracking.keys()):
info = self.tracking[stepper_name]
if info is None or not info["is_rail"]:
phase_calc = self.tracking[stepper_name]
if phase_calc is None or not phase_calc.is_primary:
continue
self.generate_stats(stepper_name, info)
self.generate_stats(stepper_name, phase_calc)
def get_status(self, eventtime):
return { 'last_home': dict(self.last_home_info) }
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)