mirror of
https://github.com/openglow-org/forgefirm.git
synced 2026-09-27 16:51:12 -07:00
Homing spike: accelerometer bump-detect proven on hardware
The head lis2hh12 (i2c-3 0x1e, direct-I2C at ~530 Hz - st_accel sysfs one-shots are ~6 Hz and the kernel has no IIO triggers) sees rail contact as a 20-40x jolt over the creep baseline within ~4 ms. bump_seek.py: 3/3 detected hits, zero false positives over ~180 mm of creep, jog-cancel stop and back-off. BRINGUP carries the full record and the driver integration design.
This commit is contained in:
ScottW514
+32
-18
@@ -429,24 +429,38 @@ at live snapshots).
|
||||
there.
|
||||
- **Interlock readback semantics cross-check: OPEN** (see
|
||||
factory-laser-safety-readbacks notes).
|
||||
3. **Homing (design decided 2026-08-02)**: the factory machine has NO
|
||||
X/Y home switches (only an unpopulated IO header — hardware project
|
||||
for another day). Current-spike stall sensing is a dead end (the PIC
|
||||
current attrs are setpoints, not measurements, and chopper-driven
|
||||
steppers don't draw more current when stalled). Plan, simple first:
|
||||
1. **Primary: accelerometer bump-detect** — the head lis2hh12 (in
|
||||
the DT, `head/accel_irq` readback) senses the contact jolt while
|
||||
creeping toward the corner; stop, back off, zero. Needs IIO
|
||||
bring-up on the dev image.
|
||||
2. **Fallback: soft-bump** — PIC current dropped to a weak value,
|
||||
slow constant-velocity stream past full travel, harmless step
|
||||
skipping against the hard stop, back off, zero counters, restore
|
||||
run current. Zero new sensing; ~±1 full step (0.15 mm)
|
||||
repeatability; brief grind during the skip. Y "weak" value needs
|
||||
empirical tuning (factory run is already only 22).
|
||||
Camera homing (the factory's actual method) is a future option once
|
||||
the camera service exists. Z homes against the hall sensor (top),
|
||||
hall-supervised only.
|
||||
3. **Homing — accelerometer bump-detect PROVEN 2026-08-03** (the
|
||||
factory machine has NO X/Y home switches; current-spike stall
|
||||
sensing is a dead end — the PIC current attrs are setpoints, not
|
||||
measurements). Spike results (tools `scripts/bench/accel_fast.py`,
|
||||
`bump_seek.py`; machine driven via grblHAL TCP jogs + 0x85 cancel):
|
||||
- **Sensors**: three lis2hh12 bind via mainline st_accel. The HEAD
|
||||
accel is **i2c-3 addr 0x1e** (proven by jog discrimination; Z
|
||||
reads −1 g). 0x1d on the same bus is a static board part (+1 g);
|
||||
i2c-0 0x1e is the lid. **st_accel sysfs one-shot reads are ~6 Hz**
|
||||
(the driver power-cycles per read) and this kernel has no IIO
|
||||
triggers — the working path is **direct I2C via /dev/i2c-3**
|
||||
(unbind st-accel first): CTRL1=0x6F (800 Hz ODR), burst-read
|
||||
OUT_X..Z → **~530 Hz** from Python, faster from C.
|
||||
- **Contact signature is unmistakable**: creep (F120) moving
|
||||
baseline ≈0.5–2 k counts (summed 3-axis |dev| from an EMA
|
||||
gravity tracker); rail contact jumps to **29–42 k within two
|
||||
samples (~4 ms)** — 20–40× over baseline. Detector: per-cycle
|
||||
learned threshold max(mean+8σ, floor), 2-sample confirm.
|
||||
- **Results: 3/3 hits, zero false positives over ~180 mm** of
|
||||
accumulated creep. Detection latency ≈4–6 ms ≈ 0.01 mm at
|
||||
2 mm/s. Post-cancel push-through is dominated by the **200 ms
|
||||
stream queue (~0.4 mm at F120)**, visible as counter drift
|
||||
between repeat hits (skipped steps against the rail).
|
||||
- **Implementation design**: detection lives in the driver as a
|
||||
virtual limit switch feeding grblHAL's homing cycle (direct-I2C
|
||||
read thread during homing only); homing runs with a shallow
|
||||
queue (small GFSINK_DEPTH) to cut push-through; **zero at the
|
||||
pressed position** so counter drift from skipped steps cancels;
|
||||
dual-phase seek/latch like standard Grbl. Fallback soft-bump
|
||||
(weak-current grind) remains available but likely unnecessary.
|
||||
Camera homing (the factory's actual method) is a future option.
|
||||
Z homes against the hall sensor (top), hall-supervised only.
|
||||
4. **6.5 safety mapping**: door/estop evdev → feed-hold/halt in the
|
||||
backend; underrun → grblHAL alarm; interlock-trip recovery check.
|
||||
5. **6.6 camera service: DONE 2026-08-03, bench- and operator-verified**
|
||||
|
||||
@@ -20,6 +20,8 @@ target board (dev image, python3 present) unless noted.
|
||||
| `temp_calibrate.py` | Coolant temperature spot-check helper (`watch` / `point <measured_C>` / `fit`) — pairs a measured temperature with averaged raw ADC readings and fits a per-machine line to sanity-check the factory curve against a thermometer. |
|
||||
| `build-glowforge.sh` | Cross-compiles **grblHAL-glowforge** (the canonical driver repo, `../../../grblHAL-glowforge`) in the forge-yocto WSL distro. Run: `wsl -d forge-yocto -- bash <path>/build-glowforge.sh` (from PowerShell; Git Bash mangles /mnt/c paths). This is the production controller build. |
|
||||
| `build-forgectrl.sh` | Cross-compiles **forgectrl** (the canonical control-daemon repo, `../../../forgectrl`) the same way, borrowing the toolchain from the forgectrl recipe workdir (regenerate with `bitbake forgectrl` after a clean). |
|
||||
| `accel_fast.py` | Direct-I2C sampler for the two head-bus LIS2HH12s (runs on the board; unbinds/rebinds st-accel around the capture, 800 Hz ODR, ~270 Hz per device polled): optional mid-capture jogs via local grblHAL TCP. CSV to /tmp/accel.csv. The head accel is i2c-3 0x1e. |
|
||||
| `bump_seek.py` | Accelerometer bump-seek homing prototype (runs on the board): creeps toward a rail in bounded jog segments via grblHAL TCP, learns the moving-noise baseline per segment, detects the contact jolt (~530 Hz sampling, 2-sample confirm), jog-cancels (0x85) and backs off. CSV to /tmp/bump.csv. |
|
||||
| `build-feeder.sh` | Cross-compiles `feeder.c` the same way. |
|
||||
| `puls_profile.py` | Decodes factory `.puls` streams (raw or GF1-headered) into velocity/accel profiles: peak speeds, ramp-slope fits, per-move segments, Z cadence. Runs anywhere (stdlib only). Source of the factory-true grblHAL defaults: 700/590 mm/s² accel, 200 mm/s max rate, 28160 Hz travel tick. |
|
||||
| `bench_m2.py` | Motion-quality bench, runs against the board over TCP:23: bounded round-trip jogs (sanity, max-rate, diagonal) + feed-hold/resume mid-move, reporting peak feed, state transitions, and position drift. |
|
||||
|
||||
@@ -0,0 +1,90 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Fast direct-I2C sampler for the two LIS2HH12s on the head bus (i2c-3).
|
||||
|
||||
Unbinds st_accel from both for the capture (rebinds after), programs
|
||||
800 Hz ODR, and polls OUT_X..OUT_Z as fast as the bus allows.
|
||||
|
||||
Usage: accel_fast.py <seconds> [jog-gcode [jog2-gcode]]
|
||||
Jogs are sent to the local grblHAL at t=2 s (and t=2+4 s for jog2).
|
||||
CSV to /tmp/accel.csv: t,addr,x,y,z
|
||||
"""
|
||||
import fcntl
|
||||
import os
|
||||
import socket
|
||||
import struct
|
||||
import sys
|
||||
import time
|
||||
|
||||
I2C_SLAVE = 0x0703
|
||||
BUS = '/dev/i2c-3'
|
||||
ADDRS = [0x1e, 0x1d]
|
||||
WHO_AM_I = 0x0F
|
||||
CTRL1 = 0x20
|
||||
OUT_X_L = 0x28
|
||||
|
||||
DRIVER = '/sys/bus/i2c/drivers/st-accel-i2c'
|
||||
|
||||
|
||||
def bind_ctl(op, dev):
|
||||
try:
|
||||
with open(DRIVER + '/' + op, 'w') as f:
|
||||
f.write(dev)
|
||||
except OSError:
|
||||
pass
|
||||
|
||||
|
||||
def main():
|
||||
dur = float(sys.argv[1]) if len(sys.argv) > 1 else 5.0
|
||||
jogs = sys.argv[2:4]
|
||||
|
||||
for a in ADDRS:
|
||||
bind_ctl('unbind', '3-%04x' % a)
|
||||
|
||||
fd = os.open(BUS, os.O_RDWR)
|
||||
for a in ADDRS:
|
||||
fcntl.ioctl(fd, I2C_SLAVE, a)
|
||||
os.write(fd, bytes([WHO_AM_I]))
|
||||
who = os.read(fd, 1)[0]
|
||||
os.write(fd, bytes([CTRL1, 0x6F])) # 800 Hz, BDU, XYZ on
|
||||
print('addr 0x%02x WHO_AM_I=0x%02x' % (a, who))
|
||||
time.sleep(0.05)
|
||||
|
||||
sock = None
|
||||
if jogs:
|
||||
sock = socket.create_connection(('127.0.0.1', 23), timeout=3)
|
||||
sock.settimeout(0.1)
|
||||
|
||||
out = open('/tmp/accel.csv', 'w')
|
||||
t0 = time.monotonic()
|
||||
sent = 0
|
||||
n = 0
|
||||
while True:
|
||||
t = time.monotonic() - t0
|
||||
if t > dur:
|
||||
break
|
||||
if sock and sent < len(jogs) and t > 2.0 + 4.0 * sent:
|
||||
sock.sendall((jogs[sent] + '\n').encode())
|
||||
sent += 1
|
||||
for a in ADDRS:
|
||||
fcntl.ioctl(fd, I2C_SLAVE, a)
|
||||
os.write(fd, bytes([OUT_X_L]))
|
||||
d = os.read(fd, 6)
|
||||
x, y, z = struct.unpack('<hhh', d)
|
||||
out.write('%.5f,%02x,%d,%d,%d\n' % (t, a, x, y, z))
|
||||
n += 1
|
||||
out.close()
|
||||
if sock:
|
||||
try:
|
||||
sock.recv(4096)
|
||||
except OSError:
|
||||
pass
|
||||
sock.close()
|
||||
os.close(fd)
|
||||
|
||||
for a in ADDRS:
|
||||
bind_ctl('bind', '3-%04x' % a)
|
||||
print('samples:', n, 'rate: %.0f Hz per device' % (n / 2 / dur))
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,202 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Accelerometer bump-seek prototype: creep toward a rail in bounded jog
|
||||
segments, detect the contact jolt on the head LIS2HH12, jog-cancel at once,
|
||||
back off. This is the homing-cycle detection loop, run standalone.
|
||||
|
||||
Usage: bump_seek.py [dir(+|-)] [feed] [seg_mm] [max_mm]
|
||||
Logs every sample to /tmp/bump.csv: t,x,y,z,dev,state
|
||||
"""
|
||||
import fcntl
|
||||
import os
|
||||
import socket
|
||||
import struct
|
||||
import sys
|
||||
import time
|
||||
|
||||
I2C_SLAVE = 0x0703
|
||||
BUS = '/dev/i2c-3'
|
||||
HEAD = 0x1e
|
||||
CTRL1 = 0x20
|
||||
OUT_X_L = 0x28
|
||||
DRIVER = '/sys/bus/i2c/drivers/st-accel-i2c'
|
||||
|
||||
BASELINE_S = 1.2 # of each segment: learn moving-noise floor
|
||||
EMA_A = 0.05 # gravity/slope tracker
|
||||
K_SIGMA = 8.0 # detection threshold multiplier
|
||||
MIN_THRESH = 800.0 # absolute floor (counts, ~0.5 g summed dev)
|
||||
CONFIRM = 2 # consecutive samples over threshold
|
||||
|
||||
|
||||
def unbind():
|
||||
try:
|
||||
with open(DRIVER + '/unbind', 'w') as f:
|
||||
f.write('3-%04x' % HEAD)
|
||||
except OSError:
|
||||
pass
|
||||
|
||||
|
||||
def rebind():
|
||||
try:
|
||||
with open(DRIVER + '/bind', 'w') as f:
|
||||
f.write('3-%04x' % HEAD)
|
||||
except OSError:
|
||||
pass
|
||||
|
||||
|
||||
class Accel:
|
||||
def __init__(self):
|
||||
self.fd = os.open(BUS, os.O_RDWR)
|
||||
fcntl.ioctl(self.fd, I2C_SLAVE, HEAD)
|
||||
os.write(self.fd, bytes([CTRL1, 0x6F]))
|
||||
time.sleep(0.05)
|
||||
self.ema = None
|
||||
|
||||
def read(self):
|
||||
os.write(self.fd, bytes([OUT_X_L]))
|
||||
x, y, z = struct.unpack('<hhh', os.read(self.fd, 6))
|
||||
if self.ema is None:
|
||||
self.ema = [float(x), float(y), float(z)]
|
||||
dev = 0.0
|
||||
for i, v in enumerate((x, y, z)):
|
||||
dev += abs(v - self.ema[i])
|
||||
self.ema[i] += EMA_A * (v - self.ema[i])
|
||||
return x, y, z, dev
|
||||
|
||||
|
||||
class Grbl:
|
||||
def __init__(self):
|
||||
self.s = socket.create_connection(('127.0.0.1', 23), timeout=3)
|
||||
self.s.settimeout(0.05)
|
||||
self.drain()
|
||||
|
||||
def drain(self):
|
||||
buf = b''
|
||||
try:
|
||||
while True:
|
||||
d = self.s.recv(4096)
|
||||
if not d:
|
||||
break
|
||||
buf += d
|
||||
except OSError:
|
||||
pass
|
||||
return buf.decode(errors='replace')
|
||||
|
||||
def send(self, line):
|
||||
self.s.sendall((line + '\n').encode())
|
||||
|
||||
def cancel(self):
|
||||
self.s.sendall(b'\x85')
|
||||
|
||||
def status(self):
|
||||
self.drain()
|
||||
self.s.sendall(b'?')
|
||||
time.sleep(0.05)
|
||||
return self.drain()
|
||||
|
||||
def wait_idle(self, timeout=30):
|
||||
t0 = time.monotonic()
|
||||
while time.monotonic() - t0 < timeout:
|
||||
st = self.status()
|
||||
if '<Idle' in st:
|
||||
return st
|
||||
time.sleep(0.2)
|
||||
return None
|
||||
|
||||
|
||||
def mpos(st):
|
||||
try:
|
||||
p = st.split('MPos:')[1].split('|')[0]
|
||||
return [float(v) for v in p.split(',')]
|
||||
except (IndexError, ValueError):
|
||||
return None
|
||||
|
||||
|
||||
def main():
|
||||
dirn = sys.argv[1] if len(sys.argv) > 1 else '-'
|
||||
feed = int(sys.argv[2]) if len(sys.argv) > 2 else 120
|
||||
seg = float(sys.argv[3]) if len(sys.argv) > 3 else 15.0
|
||||
maxmm = float(sys.argv[4]) if len(sys.argv) > 4 else 200.0
|
||||
|
||||
unbind()
|
||||
acc = Accel()
|
||||
g = Grbl()
|
||||
log = open('/tmp/bump.csv', 'w')
|
||||
|
||||
st = g.wait_idle(5)
|
||||
if st is None:
|
||||
print('controller not idle, aborting')
|
||||
return 1
|
||||
start = mpos(st)
|
||||
print('start MPos:', start)
|
||||
|
||||
t0 = time.monotonic()
|
||||
traveled = 0.0
|
||||
hit = False
|
||||
try:
|
||||
while traveled < maxmm and not hit:
|
||||
g.send('$J=G91X%s%.3fF%d' % (dirn, seg, feed))
|
||||
seg_t0 = time.monotonic()
|
||||
exp_dur = seg / feed * 60.0
|
||||
last_poll = 0.0
|
||||
over = 0
|
||||
base_devs = []
|
||||
thresh = None
|
||||
# segment loop: sample until idle (segment done) or hit
|
||||
while True:
|
||||
t = time.monotonic() - t0
|
||||
x, y, z, dev = acc.read()
|
||||
state = 'base'
|
||||
seg_t = time.monotonic() - seg_t0
|
||||
if seg_t < 0.3:
|
||||
state = 'ramp' # ignore accel/decel transients
|
||||
elif seg_t < 0.3 + BASELINE_S:
|
||||
base_devs.append(dev)
|
||||
state = 'learn'
|
||||
else:
|
||||
if thresh is None:
|
||||
m = sum(base_devs) / len(base_devs)
|
||||
sd = (sum((v - m) ** 2 for v in base_devs)
|
||||
/ len(base_devs)) ** 0.5
|
||||
thresh = max(m + K_SIGMA * sd, MIN_THRESH)
|
||||
print('segment thresh: %.0f (base mean %.0f sd %.0f)'
|
||||
% (thresh, m, sd))
|
||||
if dev > thresh:
|
||||
over += 1
|
||||
state = 'OVER'
|
||||
if over >= CONFIRM:
|
||||
g.cancel()
|
||||
hit = True
|
||||
state = 'HIT'
|
||||
else:
|
||||
over = 0
|
||||
log.write('%.5f,%d,%d,%d,%.0f,%s\n'
|
||||
% (t, x, y, z, dev, state))
|
||||
if hit:
|
||||
break
|
||||
# poll for segment completion only near its expected end,
|
||||
# so status round-trips never pause active detection
|
||||
if seg_t > exp_dur - 0.3 and seg_t - last_poll > 0.3:
|
||||
last_poll = seg_t
|
||||
stq = g.status()
|
||||
if '<Idle' in stq:
|
||||
break
|
||||
traveled += seg
|
||||
print('segment done, traveled<=%.0f mm, hit=%s' % (traveled, hit))
|
||||
finally:
|
||||
g.cancel()
|
||||
time.sleep(0.3)
|
||||
st = g.wait_idle(10)
|
||||
pos = mpos(st) if st else None
|
||||
print('stopped at MPos:', pos)
|
||||
if hit and pos:
|
||||
print('backing off 3 mm')
|
||||
g.send('$J=G91X%sF600' % ('3' if dirn == '-' else '-3'))
|
||||
g.wait_idle(10)
|
||||
log.close()
|
||||
rebind()
|
||||
print('HIT' if hit else 'NO CONTACT within %.0f mm' % maxmm)
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
sys.exit(main())
|
||||
Reference in New Issue
Block a user