3diy
/
klipper
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
klippy/extras/endstop_phase.py
View File

@ -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):

26
klippy/extras/homing.py
View File

@ -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: