# Nozzle touch with bd_pressure, and a comparison with Beacon contact. # # The bd_pressure endstop output is not wired, so it cannot stop a homing # move. BDP_TOUCH instead lowers the nozzle in small steps and reads the # force (chk_delta, register 64-65 of the load-check firmware) after each # step, until the force reaches STOP or Z reaches the floor. From the # curve it reports two touch heights: # threshold z: where the force first reaches THRESHOLD (interpolated # between the two steps around it), like an endstop would # fit z: where a straight line through the readings at or above # THRESHOLD reaches the no-contact level (the mean of the # readings before the descent): the zero-force touch point # Z is the toolhead Z (the frame of the last Z homing). # # TOUCH_COMPARE alternates Beacon's contact probe (PROBE_METHOD=contact, # one sample) and BDP_TOUCH at the current X/Y, N times, and reports the # mean and spread of each and of the per-pair difference. Run it through # the TOUCH_COMPARE macro (macros/sensors/touch_compare.cfg), which heats # the nozzle and homes first. # # Safety: the force only goes to STOP (default 80; a light finger push on # the nozzle is about 41, a purge 64-246), the descent ends at FLOOR below # the start, and the nozzle always goes back up to the start height, also # after an error. It refuses to run while the bdp_guard is armed (in stop # mode the touch would shut Klipper down) or while the sensor is in PA mode. # # Install: copy to ~/klipper/klippy/extras/bdp_touch.py (install.sh links # it) and add # # [bdp_touch] # sensor: bd_pa # name after "bdpressure" in [bdpressure bd_pa] # # Commands: # BDP_TOUCH [THRESHOLD=15] [STOP=80] [STEP=0.005] [FLOOR=0.4] [SETTLE=0.05] # [READS=3] # touch at the current X/Y, starting at the current Z # _TOUCH_COMPARE [N=10] [START=0.1] [THRESHOLD=..] [STOP=..] [STEP=..] # the comparison loop; START is the BDP_TOUCH start height above the # Beacon contact result of the same pair # # Every touch writes its whole curve to klippy.log ("bdp_touch: curve"), # so it can be analysed later. # # Status (printer.bdp_touch): threshold_z, fit_z, peak, noise, slope # (force per 0.01 mm), points, of the last BDP_TOUCH import logging import math REG_MODE = 48 # status_clk: 7 = PA mode, 2 = probe (endstop) mode REG_RESET_PROBE = 52 # write 1 = take the current reading as zero MODE_PA = 7 REG_EXT = 54 # extension marker 0xBD + version REG_DELTA = 64 # chk_delta, int16 little-endian EXT_MAGIC = 0xBD STEP_SPEED = 2. # mm/s for the small Z steps LIFT_SPEED = 5. def _stats(values): n = len(values) mean = sum(values) / n std = math.sqrt(sum((v - mean) ** 2 for v in values) / n) return mean, std, max(values) - min(values) class BDPTouch: def __init__(self, config): self.printer = config.get_printer() self.reactor = self.printer.get_reactor() self.sensor_name = 'bdpressure ' + config.get('sensor', 'bd_pa') self.sensor = None self.toolhead = None self.last = {'threshold_z': None, 'fit_z': None, 'peak': 0, 'noise': 0., 'slope': None, 'points': 0} self.printer.register_event_handler('klippy:connect', self._handle_connect) gcode = self.printer.lookup_object('gcode') self.gcode = gcode gcode.register_command('BDP_TOUCH', self.cmd_BDP_TOUCH, desc="Touch the bed with the nozzle and " "measure it with bd_pressure") gcode.register_command('_TOUCH_COMPARE', self.cmd_TOUCH_COMPARE, desc="Compare Beacon contact and bd_pressure " "touches (use the TOUCH_COMPARE macro)") def _handle_connect(self): self.sensor = self.printer.lookup_object(self.sensor_name, None) self.toolhead = self.printer.lookup_object('toolhead') # Looked up at every use: bdp_guard puts a locked wrapper on the sensor @property def i2c(self): return getattr(self.sensor, 'i2c', None) def _check_ready(self, gcmd): if self.i2c is None: raise gcmd.error("BDP_TOUCH: no [%s]" % self.sensor_name) status = self.toolhead.get_status(self.reactor.monotonic()) if 'z' not in status['homed_axes']: raise gcmd.error("BDP_TOUCH: home Z first") guard = self.printer.lookup_object('bdp_guard', None) if guard is not None and guard.timer is not None: raise gcmd.error("BDP_TOUCH: the bdp_guard is armed " "(BDP_GUARD_DISARM first)") if getattr(self.sensor, 'last_state', 0): raise gcmd.error("BDP_TOUCH: a bd_pressure calibration runs") ext = bytearray(self.i2c.i2c_read([REG_EXT], 2)['response']) if len(ext) != 2 or ext[0] != EXT_MAGIC: raise gcmd.error("BDP_TOUCH: bd_pressure load-check firmware " "not found") mode = bytearray(self.i2c.i2c_read([REG_MODE], 1)['response']) if len(mode) == 1 and mode[0] == MODE_PA: raise gcmd.error("BDP_TOUCH: bd_pressure is in PA mode " "(home Z once to switch it back)") def _read_delta(self, reads): total = 0 for i in range(reads): data = bytearray(self.i2c.i2c_read([REG_DELTA], 2)['response']) if len(data) != 2: raise self.printer.command_error("BDP_TOUCH: short read") d = data[0] | (data[1] << 8) total += d - 0x10000 if d & 0x8000 else d return total / float(reads) def _move_z(self, z, speed): self.toolhead.manual_move([None, None, z], speed) self.toolhead.wait_moves() def touch(self, gcmd, threshold, stop, step, floor, settle, reads): """Lower the nozzle from the current Z; returns the result dict.""" self._check_ready(gcmd) self.toolhead.wait_moves() start = self.toolhead.get_position()[2] # New zero with the nozzle free, then the no-contact level and noise self.i2c.i2c_write([REG_RESET_PROBE, 1]) self.reactor.pause(self.reactor.monotonic() + 0.3) base = [] for i in range(10): base.append(self._read_delta(1)) self.reactor.pause(self.reactor.monotonic() + 0.01) base_mean, noise, _ = _stats(base) curve = [] try: z = start while True: z = round(z - step, 6) if z < start - floor: raise gcmd.error( "BDP_TOUCH: no touch within %.3f mm (peak %.0f)" % (floor, max([abs(d) for zz, d in curve] or [0]))) self._move_z(z, STEP_SPEED) self.reactor.pause(self.reactor.monotonic() + settle) d = self._read_delta(reads) curve.append((z, d)) if abs(d - base_mean) >= stop: break finally: self._move_z(start, LIFT_SPEED) logging.info("bdp_touch: curve start=%.4f base=%.2f noise=%.2f %s", start, base_mean, noise, " ".join("%.4f:%.1f" % p for p in curve)) # The sign of the force depends on the gauge: take it from the end sign = 1. if curve[-1][1] - base_mean >= 0 else -1. force = [(zz, sign * (d - base_mean)) for zz, d in curve] thr_z = None prev = (start, 0.) for zz, f in force: if f >= threshold: pz, pf = prev thr_z = pz + (zz - pz) * (threshold - pf) / (f - pf) break prev = (zz, f) fit_z = slope = None pts = [(zz, f) for zz, f in force if f >= threshold] if len(pts) >= 3: n = len(pts) mz = sum(p[0] for p in pts) / n mf = sum(p[1] for p in pts) / n szz = sum((p[0] - mz) ** 2 for p in pts) if szz > 0: a = sum((p[0] - mz) * (p[1] - mf) for p in pts) / szz if a < 0: fit_z = mz - mf / a # where the line reaches force 0 slope = -a * 0.01 self.last = {'threshold_z': thr_z, 'fit_z': fit_z, 'peak': max(f for zz, f in force), 'noise': noise, 'slope': slope, 'points': len(pts)} return self.last def _touch_params(self, gcmd): return dict( threshold=gcmd.get_float('THRESHOLD', 15., above=0.), stop=gcmd.get_float('STOP', 80., above=0., maxval=150.), step=gcmd.get_float('STEP', 0.005, minval=0.001, maxval=0.02), floor=gcmd.get_float('FLOOR', 0.4, above=0., maxval=1.), settle=gcmd.get_float('SETTLE', 0.05, minval=0., maxval=1.), reads=gcmd.get_int('READS', 3, minval=1, maxval=10)) def _fmt(self, v): return "-" if v is None else "%.4f" % v def cmd_BDP_TOUCH(self, gcmd): p = self._touch_params(gcmd) if p['threshold'] >= p['stop']: raise gcmd.error("BDP_TOUCH: THRESHOLD must be below STOP") r = self.touch(gcmd, **p) gcmd.respond_info( "BDP_TOUCH: threshold z %s, fit z %s, peak %.0f, noise %.1f, " "%s per 0.01 mm, %d points" % (self._fmt(r['threshold_z']), self._fmt(r['fit_z']), r['peak'], r['noise'], "-" if r['slope'] is None else "%.0f" % r['slope'], r['points'])) def cmd_TOUCH_COMPARE(self, gcmd): beacon = self.printer.lookup_object('beacon', None) if beacon is None: raise gcmd.error("_TOUCH_COMPARE: no [beacon]") n = gcmd.get_int('N', 10, minval=2, maxval=50) start_above = gcmd.get_float('START', 0.1, minval=0.05, maxval=1.) p = self._touch_params(gcmd) if p['threshold'] >= p['stop']: raise gcmd.error("_TOUCH_COMPARE: THRESHOLD must be below STOP") self._check_ready(gcmd) rows = [] for i in range(n): self._move_z(2., LIFT_SPEED) probe_cmd = self.gcode.create_gcode_command( "PROBE", "PROBE", {'PROBE_METHOD': 'contact', 'SAMPLES': '1'}) bz = beacon.run_probe(probe_cmd)[2] # Beacon lifts at least 1 mm after its touch; come down from # above to the BDP start height, so both approach downwards self._move_z(bz + start_above, LIFT_SPEED) r = self.touch(gcmd, **p) rows.append((bz, r['threshold_z'], r['fit_z'], r['peak'])) gcmd.respond_info( "touch %d/%d: beacon %.4f, bdp threshold %s, bdp fit %s, " "peak %.0f" % (i + 1, n, bz, self._fmt(r['threshold_z']), self._fmt(r['fit_z']), r['peak'])) self._move_z(5., LIFT_SPEED) lines = ["TOUCH_COMPARE: %d pairs (mean / std / range, mm)" % n] cols = [("beacon contact", [r[0] for r in rows]), ("bdp threshold", [r[1] for r in rows]), ("bdp fit", [r[2] for r in rows])] for name, vals in cols: vals = [v for v in vals if v is not None] if len(vals) >= 2: lines.append(" %s: %.4f / %.4f / %.4f (%d)" % ((name,) + _stats(vals) + (len(vals),))) for name, k in (("threshold - beacon", 1), ("fit - beacon", 2)): diffs = [r[k] - r[0] for r in rows if r[k] is not None] if len(diffs) >= 2: lines.append(" %s: %.4f / %.4f / %.4f (%d)" % ((name,) + _stats(diffs) + (len(diffs),))) msg = "\n".join(lines) logging.info("bdp_touch: %s", msg) gcmd.respond_info(msg) def get_status(self, eventtime): return dict(self.last) def load_config(config): return BDPTouch(config)