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:
+32
-18
@@ -429,24 +429,38 @@ at live snapshots).
|
|||||||
there.
|
there.
|
||||||
- **Interlock readback semantics cross-check: OPEN** (see
|
- **Interlock readback semantics cross-check: OPEN** (see
|
||||||
factory-laser-safety-readbacks notes).
|
factory-laser-safety-readbacks notes).
|
||||||
3. **Homing (design decided 2026-08-02)**: the factory machine has NO
|
3. **Homing — accelerometer bump-detect PROVEN 2026-08-03** (the
|
||||||
X/Y home switches (only an unpopulated IO header — hardware project
|
factory machine has NO X/Y home switches; current-spike stall
|
||||||
for another day). Current-spike stall sensing is a dead end (the PIC
|
sensing is a dead end — the PIC current attrs are setpoints, not
|
||||||
current attrs are setpoints, not measurements, and chopper-driven
|
measurements). Spike results (tools `scripts/bench/accel_fast.py`,
|
||||||
steppers don't draw more current when stalled). Plan, simple first:
|
`bump_seek.py`; machine driven via grblHAL TCP jogs + 0x85 cancel):
|
||||||
1. **Primary: accelerometer bump-detect** — the head lis2hh12 (in
|
- **Sensors**: three lis2hh12 bind via mainline st_accel. The HEAD
|
||||||
the DT, `head/accel_irq` readback) senses the contact jolt while
|
accel is **i2c-3 addr 0x1e** (proven by jog discrimination; Z
|
||||||
creeping toward the corner; stop, back off, zero. Needs IIO
|
reads −1 g). 0x1d on the same bus is a static board part (+1 g);
|
||||||
bring-up on the dev image.
|
i2c-0 0x1e is the lid. **st_accel sysfs one-shot reads are ~6 Hz**
|
||||||
2. **Fallback: soft-bump** — PIC current dropped to a weak value,
|
(the driver power-cycles per read) and this kernel has no IIO
|
||||||
slow constant-velocity stream past full travel, harmless step
|
triggers — the working path is **direct I2C via /dev/i2c-3**
|
||||||
skipping against the hard stop, back off, zero counters, restore
|
(unbind st-accel first): CTRL1=0x6F (800 Hz ODR), burst-read
|
||||||
run current. Zero new sensing; ~±1 full step (0.15 mm)
|
OUT_X..Z → **~530 Hz** from Python, faster from C.
|
||||||
repeatability; brief grind during the skip. Y "weak" value needs
|
- **Contact signature is unmistakable**: creep (F120) moving
|
||||||
empirical tuning (factory run is already only 22).
|
baseline ≈0.5–2 k counts (summed 3-axis |dev| from an EMA
|
||||||
Camera homing (the factory's actual method) is a future option once
|
gravity tracker); rail contact jumps to **29–42 k within two
|
||||||
the camera service exists. Z homes against the hall sensor (top),
|
samples (~4 ms)** — 20–40× over baseline. Detector: per-cycle
|
||||||
hall-supervised only.
|
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
|
4. **6.5 safety mapping**: door/estop evdev → feed-hold/halt in the
|
||||||
backend; underrun → grblHAL alarm; interlock-trip recovery check.
|
backend; underrun → grblHAL alarm; interlock-trip recovery check.
|
||||||
5. **6.6 camera service: DONE 2026-08-03, bench- and operator-verified**
|
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. |
|
| `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-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). |
|
| `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. |
|
| `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. |
|
| `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. |
|
| `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