From 654546e3382b87610a51d01d17c917fb8d8bbaeb Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Tue, 26 Jul 2016 22:06:14 -0400 Subject: [PATCH] stepper: Support stepper phase adjustments when homing Add support for enhancing the precision of endstop switches by also inspecting the phase of the stepper motor when the endstop triggers. Signed-off-by: Kevin O'Connor --- config/example.cfg | 22 +++++++++++++++++++++- config/makergear-m2-2012.cfg | 6 ++++++ klippy/cartesian.py | 2 ++ klippy/mcu.py | 8 ++++++++ klippy/stepper.py | 35 +++++++++++++++++++++++++++++++++++ klippy/toolhead.py | 10 +++++++++- 6 files changed, 81 insertions(+), 2 deletions(-) diff --git a/config/example.cfg b/config/example.cfg index b458e8b4..5614b5fc 100644 --- a/config/example.cfg +++ b/config/example.cfg @@ -11,7 +11,7 @@ # available pin and board names.) # Pin names may be preceded by an '!' to indicate that a reverse # polarity should be used (eg, trigger on low instead of high). Input -# pins may be prceded by an '^' to indicate that a hardware pull-up +# pins may be preceded by a '^' to indicate that a hardware pull-up # resistor should be enabled for the pin. @@ -38,6 +38,26 @@ homing_retract_dist: 5.0 # Distance to backoff (in mm) before homing a second time during homing homing_positive_dir: False # If true, homes in a positive direction (away from zero) +homing_stepper_phases: 0 +# One may optionally set this to the number of phases of the stepper +# motor driver (which is the number of micro-steps multiplied by +# four). When set, the phase of the stepper driver will be used +# during homing to improve the accuracy of the endstop switch. +homing_endstop_accuracy: 0.200 +# Sets the expected accuracy (in mm) of the endstop. This represents +# the maximum error distance the endstop may trigger (eg, if an +# endstop may occasionally trigger 100um early or up to 100um late +# then set this to 0.200 for 200um). This setting is used with +# homing_stepper_phases and is only useful if that parameter is also +# configured. +#homing_endstop_phase: 0 +# This specifies the phase of the stepper motor driver to expect +# when hitting the endstop. This setting is only meaningful if +# homing_stepper_phases is also set. Only set this value if one is +# sure the stepper motor driver is reset every time the mcu is +# reset. If this is not set, but homing_stepper_phases is set, then +# the stepper phase will be detected on the first home and that +# phase will be used on all subsequent homes. position_min: -0.25 # Minimum valid distance (in mm) the user may command the stepper to # move to (not currently enforced) diff --git a/config/makergear-m2-2012.cfg b/config/makergear-m2-2012.cfg index 1721cac7..a3a9173e 100644 --- a/config/makergear-m2-2012.cfg +++ b/config/makergear-m2-2012.cfg @@ -12,6 +12,8 @@ max_velocity: 500 max_accel: 3000 endstop_pin: ^PB6 homing_speed: 50.0 +homing_stepper_phases: 32 +homing_endstop_accuracy: .200 position_min: -0.25 position_endstop: 0.0 position_max: 200 @@ -25,6 +27,8 @@ max_velocity: 500 max_accel: 3000 endstop_pin: ^PB5 homing_speed: 50.0 +homing_stepper_phases: 32 +homing_endstop_accuracy: .200 position_min: -0.25 position_endstop: 0.0 position_max: 250 @@ -39,6 +43,8 @@ max_accel: 30 endstop_pin: ^PB4 homing_speed: 4.0 homing_retract_dist: 2.0 +homing_stepper_phases: 32 +homing_endstop_accuracy: .050 position_min: 0.1 position_endstop: 0.7 position_max: 200 diff --git a/klippy/cartesian.py b/klippy/cartesian.py index f061f673..b904d73f 100644 --- a/klippy/cartesian.py +++ b/klippy/cartesian.py @@ -33,6 +33,8 @@ class CartKinematics: accel_factor = min([self.steppers[i].max_accel / abs(axes_d[i]) for i in StepList if axes_d[i]]) return velocity_factor * move_d, accel_factor * move_d + def get_homed_position(self): + return [s.get_homed_position() for s in self.steppers] def home(self, toolhead, axes): # Each axis is homed independently and in order homing_state = homing.Homing(toolhead, axes) diff --git a/klippy/mcu.py b/klippy/mcu.py index 728ec91d..864ebbfb 100644 --- a/klippy/mcu.py +++ b/klippy/mcu.py @@ -40,6 +40,8 @@ class MCU_stepper: max_error, self._step_cmd.msgid, self._oid) def get_oid(self): return self._oid + def get_invert_dir(self): + return self._invert_dir def note_stepper_stop(self): self._sdir = -1 self._last_move_clock = -2**29 @@ -88,6 +90,7 @@ class MCU_endstop: , self._oid) self._query_cmd = mcu.lookup_command("end_stop_query oid=%c") self._homing = False + self._last_position = 0 self._next_query_clock = 0 mcu_freq = self._mcu.get_mcu_freq() self._retry_query_ticks = mcu_freq * self.RETRY_QUERY @@ -104,6 +107,7 @@ class MCU_endstop: self._stepper.note_stepper_stop() def _handle_end_stop_state(self, params): logging.debug("end_stop_state %s" % (params,)) + self._last_position = params['pos'] self._homing = params['homing'] != 0 def is_homing(self): if not self._homing: @@ -116,6 +120,10 @@ class MCU_endstop: msg = self._query_cmd.encode(self._oid) self._mcu.send(msg, cq=self._cmd_queue) return self._homing + def get_last_position(self): + if self._stepper.get_invert_dir(): + return -self._last_position + return self._last_position def get_print_clock(self, print_time): return self._mcu.get_print_clock(print_time) diff --git a/klippy/stepper.py b/klippy/stepper.py index 7c8438fc..c15d194c 100644 --- a/klippy/stepper.py +++ b/klippy/stepper.py @@ -21,6 +21,23 @@ class PrinterStepper: self.homing_positive_dir = config.getboolean( 'homing_positive_dir', False) self.homing_retract_dist = config.getfloat('homing_retract_dist', 5.) + self.homing_stepper_phases = config.getint('homing_stepper_phases') + self.homing_endstop_phase = config.getint('homing_endstop_phase') + endstop_accuracy = config.getfloat('homing_endstop_accuracy') + self.homing_endstop_accuracy = None + if self.homing_stepper_phases: + if endstop_accuracy is None: + self.homing_endstop_accuracy = self.homing_stepper_phases//2 - 1 + elif self.homing_endstop_phase is not None: + self.homing_endstop_accuracy = int(math.ceil( + endstop_accuracy * self.inv_step_dist / 2.)) + else: + self.homing_endstop_accuracy = int(math.ceil( + endstop_accuracy * self.inv_step_dist)) + if self.homing_endstop_accuracy >= self.homing_stepper_phases/2: + logging.info("Endstop for %s is not accurate enough for stepper" + " phase adjustment" % (self.config.section,)) + self.homing_stepper_phases = None self.position_min = config.getfloat('position_min', 0.) self.position_endstop = config.getfloat('position_endstop') self.position_max = config.getfloat('position_max') @@ -68,3 +85,21 @@ class PrinterStepper: move_clock = int(self.mcu_endstop.get_print_clock(move_time)) self.mcu_endstop.home(move_clock, int(self.clock_ticks / hz)) return self.mcu_endstop + def get_homed_position(self): + if not self.homing_stepper_phases: + return self.position_endstop + pos = self.mcu_endstop.get_last_position() + pos %= self.homing_stepper_phases + if self.homing_endstop_phase is None: + logging.info("Setting %s endstop phase to %d" % ( + self.config.section, pos)) + self.homing_endstop_phase = pos + return self.position_endstop + delta = (pos - self.homing_endstop_phase) % self.homing_stepper_phases + if delta >= self.homing_stepper_phases - self.homing_endstop_accuracy: + delta -= self.homing_stepper_phases + elif delta > self.homing_endstop_accuracy: + logging.error("Endstop %s incorrect phase (got %d vs %d)" % ( + self.config.section, pos, self.homing_endstop_phase)) + return self.position_endstop + return self.position_endstop + delta * self.step_dist diff --git a/klippy/toolhead.py b/klippy/toolhead.py index 35d296e1..8c389d19 100644 --- a/klippy/toolhead.py +++ b/klippy/toolhead.py @@ -254,7 +254,15 @@ class ToolHead: move = Move(self, newpos, move_d, axes_d, speed, self.max_xy_accel) self.move_queue.add_move(move) def home(self, axes): - return self.kin.home(self, axes) + homing = self.kin.home(self, axes) + def axes_update(axes): + pos = self.get_position() + homepos = self.kin.get_homed_position() + for axis in axes: + pos[axis] = homepos[axis] + self.set_position(pos) + homing.plan_axes_update(axes_update) + return homing def dwell(self, delay): self.get_last_move_time() self.update_move_time(delay)