From 65ed55a43be0b995fde2d620ea838352efa7afb8 Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Wed, 9 Jun 2021 23:34:47 -0400 Subject: [PATCH] endstop_phase: Calculate phase based on trigger position Update the endstop_phase code to use the phase of the stepper motor at the endstop trigger time. Signed-off-by: Kevin O'Connor --- klippy/extras/endstop_phase.py | 36 +++++++++++++++++----------------- klippy/extras/homing.py | 26 ++++++++++++++---------- 2 files changed, 34 insertions(+), 28 deletions(-) diff --git a/klippy/extras/endstop_phase.py b/klippy/extras/endstop_phase.py index fa57a0fc..521a3c9f 100644 --- a/klippy/extras/endstop_phase.py +++ b/klippy/extras/endstop_phase.py @@ -32,8 +32,7 @@ class PhaseCalc: 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() + 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() @@ -42,10 +41,10 @@ class PhaseCalc: raise self.printer.command_error("Stepper %s phase unknown" % (self.name,)) mcu_phase_offset = 0 - phase = (mcu_pos + mcu_phase_offset) % self.phases + phase = (trig_mcu_pos + mcu_phase_offset) % self.phases self.phase_history[phase] += 1 self.last_phase = phase - self.last_mcu_position = mcu_pos + self.last_mcu_position = trig_mcu_pos return phase # Adjusted endstop trigger positions @@ -94,18 +93,19 @@ class EndstopPhase: " 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, pos): + def align_endstop(self, rail): if not self.endstop_align_zero or self.endstop_phase is None: - return pos + 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 - return int(pos / full_step + .5) * full_step + phase_offset - def get_homed_offset(self, stepper): - phase = self.phase_calc.calc_phase(stepper) + 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 @@ -119,15 +119,13 @@ class EndstopPhase: self.name, phase, self.endstop_phase)) return delta * self.step_dist def handle_home_rails_end(self, homing_state, rails): - kin_spos = homing_state.get_stepper_trigger_positions() - orig_pos = kin_spos.get(self.name) - if orig_pos is None: - return for rail in rails: stepper = rail.get_steppers()[0] if stepper.get_name() == self.name: - offset = self.get_homed_offset(stepper) - kin_spos[self.name] = self.align_endstop(orig_pos) + offset + 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 @@ -142,7 +140,7 @@ class EndstopPhases: self.gcode.register_command("ENDSTOP_PHASE_CALIBRATE", self.cmd_ENDSTOP_PHASE_CALIBRATE, desc=self.cmd_ENDSTOP_PHASE_CALIBRATE_help) - def update_stepper(self, stepper, is_primary): + 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: @@ -162,12 +160,14 @@ class EndstopPhases: if is_primary: phase_calc.is_primary = True if phase_calc.stats_only: - phase_calc.calc_phase(stepper) + 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(): - self.update_stepper(stepper, is_primary) + 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): diff --git a/klippy/extras/homing.py b/klippy/extras/homing.py index 220877e1..951f63ae 100644 --- a/klippy/extras/homing.py +++ b/klippy/extras/homing.py @@ -150,13 +150,16 @@ class Homing: self.printer = printer self.toolhead = printer.lookup_object('toolhead') self.changed_axes = [] - self.kin_spos = {} + 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_stepper_trigger_positions(self): - return self.kin_spos + 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()) @@ -200,16 +203,19 @@ class Homing: % (hmove.check_no_movement(),)) # Signal home operation complete 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.kin_spos = dict(kin_spos) + 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 kin_spos != self.kin_spos: + if any(self.adjust_pos.values()): # Apply any homing offsets - adjustpos = kin.calc_position(self.kin_spos) + kin = self.toolhead.get_kinematics() + 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: - movepos[axis] = adjustpos[axis] + movepos[axis] = newpos[axis] self.toolhead.set_position(movepos) class PrinterHoming: