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 <kevin@koconnor.net>
This commit is contained in:
Kevin O'Connor 2021-06-09 23:34:47 -04:00
parent 3814a13251
commit 65ed55a43b
2 changed files with 34 additions and 28 deletions

View File

@ -32,8 +32,7 @@ class PhaseCalc:
def convert_phase(self, driver_phase, driver_phases): def convert_phase(self, driver_phase, driver_phases):
phases = self.phases phases = self.phases
return (int(float(driver_phase) / driver_phases * phases + .5) % phases) return (int(float(driver_phase) / driver_phases * phases + .5) % phases)
def calc_phase(self, stepper): def calc_phase(self, stepper, trig_mcu_pos):
mcu_pos = stepper.get_mcu_position()
mcu_phase_offset = 0 mcu_phase_offset = 0
if self.tmc_module is not None: if self.tmc_module is not None:
mcu_phase_offset, phases = self.tmc_module.get_phase_offset() 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" raise self.printer.command_error("Stepper %s phase unknown"
% (self.name,)) % (self.name,))
mcu_phase_offset = 0 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.phase_history[phase] += 1
self.last_phase = phase self.last_phase = phase
self.last_mcu_position = mcu_pos self.last_mcu_position = trig_mcu_pos
return phase return phase
# Adjusted endstop trigger positions # Adjusted endstop trigger positions
@ -94,18 +93,19 @@ class EndstopPhase:
" stepper phase adjustment" % (self.name,)) " stepper phase adjustment" % (self.name,))
if self.printer.get_start_args().get('debugoutput') is not None: if self.printer.get_start_args().get('debugoutput') is not None:
self.endstop_phase_accuracy = self.phases 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: 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 # Adjust the endstop position so 0.0 is always at a full step
microsteps = self.phases // 4 microsteps = self.phases // 4
half_microsteps = microsteps // 2 half_microsteps = microsteps // 2
phase_offset = (((self.endstop_phase + half_microsteps) % microsteps) phase_offset = (((self.endstop_phase + half_microsteps) % microsteps)
- half_microsteps) * self.step_dist - half_microsteps) * self.step_dist
full_step = microsteps * self.step_dist full_step = microsteps * self.step_dist
return int(pos / full_step + .5) * full_step + phase_offset pe = rail.get_homing_info().position_endstop
def get_homed_offset(self, stepper): return int(pe / full_step + .5) * full_step - pe + phase_offset
phase = self.phase_calc.calc_phase(stepper) 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: if self.endstop_phase is None:
logging.info("Setting %s endstop phase to %d", self.name, phase) logging.info("Setting %s endstop phase to %d", self.name, phase)
self.endstop_phase = phase self.endstop_phase = phase
@ -119,15 +119,13 @@ class EndstopPhase:
self.name, phase, self.endstop_phase)) self.name, phase, self.endstop_phase))
return delta * self.step_dist return delta * self.step_dist
def handle_home_rails_end(self, homing_state, rails): 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: for rail in rails:
stepper = rail.get_steppers()[0] stepper = rail.get_steppers()[0]
if stepper.get_name() == self.name: if stepper.get_name() == self.name:
offset = self.get_homed_offset(stepper) trig_mcu_pos = homing_state.get_trigger_position(self.name)
kin_spos[self.name] = self.align_endstop(orig_pos) + offset align = self.align_endstop(rail)
offset = self.get_homed_offset(stepper, trig_mcu_pos)
homing_state.set_stepper_adjustment(self.name, align + offset)
return return
# Support for ENDSTOP_PHASE_CALIBRATE command # Support for ENDSTOP_PHASE_CALIBRATE command
@ -142,7 +140,7 @@ class EndstopPhases:
self.gcode.register_command("ENDSTOP_PHASE_CALIBRATE", self.gcode.register_command("ENDSTOP_PHASE_CALIBRATE",
self.cmd_ENDSTOP_PHASE_CALIBRATE, self.cmd_ENDSTOP_PHASE_CALIBRATE,
desc=self.cmd_ENDSTOP_PHASE_CALIBRATE_help) 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() stepper_name = stepper.get_name()
phase_calc = self.tracking.get(stepper_name) phase_calc = self.tracking.get(stepper_name)
if phase_calc is None: if phase_calc is None:
@ -162,12 +160,14 @@ class EndstopPhases:
if is_primary: if is_primary:
phase_calc.is_primary = True phase_calc.is_primary = True
if phase_calc.stats_only: 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): def handle_home_rails_end(self, homing_state, rails):
for rail in rails: for rail in rails:
is_primary = True is_primary = True
for stepper in rail.get_steppers(): 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 is_primary = False
cmd_ENDSTOP_PHASE_CALIBRATE_help = "Calibrate stepper phase" cmd_ENDSTOP_PHASE_CALIBRATE_help = "Calibrate stepper phase"
def cmd_ENDSTOP_PHASE_CALIBRATE(self, gcmd): def cmd_ENDSTOP_PHASE_CALIBRATE(self, gcmd):

View File

@ -150,13 +150,16 @@ class Homing:
self.printer = printer self.printer = printer
self.toolhead = printer.lookup_object('toolhead') self.toolhead = printer.lookup_object('toolhead')
self.changed_axes = [] self.changed_axes = []
self.kin_spos = {} self.trigger_mcu_pos = {}
self.adjust_pos = {}
def set_axes(self, axes): def set_axes(self, axes):
self.changed_axes = axes self.changed_axes = axes
def get_axes(self): def get_axes(self):
return self.changed_axes return self.changed_axes
def get_stepper_trigger_positions(self): def get_trigger_position(self, stepper_name):
return self.kin_spos 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): def _fill_coord(self, coord):
# Fill in any None entries in 'coord' with current toolhead position # Fill in any None entries in 'coord' with current toolhead position
thcoord = list(self.toolhead.get_position()) thcoord = list(self.toolhead.get_position())
@ -200,16 +203,19 @@ class Homing:
% (hmove.check_no_movement(),)) % (hmove.check_no_movement(),))
# Signal home operation complete # Signal home operation complete
self.toolhead.flush_step_generation() self.toolhead.flush_step_generation()
kin = self.toolhead.get_kinematics() self.trigger_mcu_pos = {sp.stepper_name: sp.trig_pos
kin_spos = {s.get_name(): s.get_commanded_position() for sp in hmove.stepper_positions}
for s in kin.get_steppers()} self.adjust_pos = {}
self.kin_spos = dict(kin_spos)
self.printer.send_event("homing:home_rails_end", self, rails) 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 # 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: for axis in homing_axes:
movepos[axis] = adjustpos[axis] movepos[axis] = newpos[axis]
self.toolhead.set_position(movepos) self.toolhead.set_position(movepos)
class PrinterHoming: class PrinterHoming: