mirror of
https://github.com/openglow-org/forgefirm.git
synced 2026-09-27 16:51:12 -07:00
Milestone 2: factory-true motion tuning, bench-verified
Extract the factory motion profile from the _RESOURCES pulse streams (new puls_profile.py): 700/590 mm/s2 X/Y accel on v2.6.0 firmware, 202 mm/s travel peak at the 28160 Hz travel tick, PIC currents X 135/33 and Y 22/5 run/hold, decay mode 1. Add the bench_m2.py round-trip jog + feed-hold suite (all green 2026-08-02: sustained 200 mm/s, exact returns, clean hold/resume, zero underruns) and update BRINGUP.md: board moved to a fixed lease at 172.16.1.97, analog config now applied by the sink itself, $RST=$ note for stale stored settings.
This commit is contained in:
+50
-23
@@ -1,6 +1,7 @@
|
|||||||
# ForgeFIRM bring-up status & cold-start runbook
|
# ForgeFIRM bring-up status & cold-start runbook
|
||||||
|
|
||||||
Last updated: **2026-07-26** — the day the machine first moved under grblHAL.
|
Last updated: **2026-08-02** — milestone 2 (factory-true motion tuning)
|
||||||
|
bench-verified.
|
||||||
Read together with `AUDIT_ACTION_PLAN.md` in the project root (sibling of
|
Read together with `AUDIT_ACTION_PLAN.md` in the project root (sibling of
|
||||||
this repo; per-finding status of the 2026-07-03 audit) and
|
this repo; per-finding status of the 2026-07-03 audit) and
|
||||||
`kernel-module-glowforge/UAPI.md` (the pulse-stream feeder contract).
|
`kernel-module-glowforge/UAPI.md` (the pulse-stream feeder contract).
|
||||||
@@ -28,9 +29,27 @@ deadman/safety loop; camera error paths).
|
|||||||
counters agree step-for-step. Motion-only: the laser latch is forced
|
counters agree step-for-step. Motion-only: the laser latch is forced
|
||||||
locked, byte bit 4 is never emitted.
|
locked, byte bit 4 is never emitted.
|
||||||
|
|
||||||
|
**Milestone 2 (motion quality): bench-verified 2026-08-02.** The factory
|
||||||
|
motion constants were extracted from the `_RESOURCES` pulse files
|
||||||
|
(`scripts/bench/puls_profile.py`) and applied end-to-end:
|
||||||
|
- grblHAL defaults now factory-true: 12000 mm/min max rate (X/Y),
|
||||||
|
700/590 mm/s² accel (X/Y). Machine tick default 28160 Hz (the factory's
|
||||||
|
own travel-move tick; 10 kHz caps an axis at 187.5 mm/s).
|
||||||
|
- The sink now applies the whole analog machine config itself at init
|
||||||
|
(modes, decay, motor_lock, PIC currents) and switches PIC currents
|
||||||
|
run↔hold around motion like the factory did (135/22 running, 33/5 idle,
|
||||||
|
drop deferred until the kernel queue has drained).
|
||||||
|
- Bench (`scripts/bench/bench_m2.py`, all green): sustained 200 mm/s on a
|
||||||
|
120 mm jog, exact round-trip positioning, feed-hold parks and resumes
|
||||||
|
cleanly, current switching observed live, zero underruns at 28160 Hz.
|
||||||
|
- NOTE: stored $-settings beat freshly baked defaults — after changing
|
||||||
|
`GLOWFORGE_DEFAULTS` values, run `$RST=$` once on the board (the sim
|
||||||
|
persists settings in its eeprom file in /data).
|
||||||
|
|
||||||
## The bench
|
## The bench
|
||||||
|
|
||||||
- **Board**: SSH `root@172.16.1.130`, empty password
|
- **Board**: SSH `root@172.16.1.97` (fixed DHCP lease since 2026-08-02;
|
||||||
|
was .130), empty password
|
||||||
(`ssh -o PreferredAuthentications=none` logs straight in). Dev image
|
(`ssh -o PreferredAuthentications=none` logs straight in). Dev image
|
||||||
(`forgefirm-image-dev`) on SD; BusyBox userland + python3 + gdb/strace.
|
(`forgefirm-image-dev`) on SD; BusyBox userland + python3 + gdb/strace.
|
||||||
Serial console on ttymxc0 available at the bench.
|
Serial console on ttymxc0 available at the bench.
|
||||||
@@ -61,24 +80,21 @@ glowforge pulse-stream sink).
|
|||||||
|
|
||||||
1. Build: cross-compile the backend in the forge-yocto WSL distro (from
|
1. Build: cross-compile the backend in the forge-yocto WSL distro (from
|
||||||
PowerShell). Produces `build-arm/grblHAL_glowforge` in the WSL tree
|
PowerShell). Produces `build-arm/grblHAL_glowforge` in the WSL tree
|
||||||
(`-O1 -g`, `GLOWFORGE_DEFAULTS=ON` → machine scaling baked in:
|
(`-O1 -g`, `GLOWFORGE_DEFAULTS=ON` → machine constants baked in:
|
||||||
53.333 µsteps/mm XY @ ×8, 2.832 half-steps/mm Z, 0.417" Z travel).
|
53.333 µsteps/mm XY @ ×8, 2.832 half-steps/mm Z, 0.417" Z travel,
|
||||||
|
12000 mm/min max, 700/590 mm/s² accel — factory-derived, see
|
||||||
|
`puls_profile.py`).
|
||||||
2. Deploy to `/usr/bin/grblHAL_glowforge` on the board.
|
2. Deploy to `/usr/bin/grblHAL_glowforge` on the board.
|
||||||
3. **Analog machine config — required after every module reload/boot; the
|
3. Start: `cd /data && GFSINK=/dev/glowforge grblHAL_glowforge -p 23 -n -t 1.0`
|
||||||
kernel does NOT do this** (the cloud stack normally did):
|
|
||||||
```sh
|
|
||||||
echo 150 > /sys/glowforge/pic/x_step_current # run current; 33 = weak hold.
|
|
||||||
echo 150 > /sys/glowforge/pic/y_step_current # Factory-true values TBD
|
|
||||||
echo 8 > /sys/glowforge/cnc/x_mode # ×8 microstepping to match
|
|
||||||
echo 8 > /sys/glowforge/cnc/y_mode # the baked steps/mm
|
|
||||||
echo 8 > /sys/glowforge/cnc/motor_lock # Z locked; X/Y free
|
|
||||||
echo 1 > /sys/glowforge/cnc/laser_latch # laser locked out
|
|
||||||
```
|
|
||||||
4. Start: `cd /data && GFSINK=/dev/glowforge grblHAL_glowforge -p 23 -n -t 1.0`
|
|
||||||
(`-t 1.0` is REQUIRED: the real-time throttle is what bounds the
|
(`-t 1.0` is REQUIRED: the real-time throttle is what bounds the
|
||||||
queue). Env knobs: `GFSINK_RATE` (machine tick, default 10000 Hz),
|
queue). Env knobs: `GFSINK_RATE` (machine tick, default 28160 Hz =
|
||||||
`GFSINK_DEPTH_MS` (queue depth = feed-hold latency, default 200).
|
factory travel tick), `GFSINK_DEPTH_MS` (queue depth = feed-hold
|
||||||
5. Connect LightBurn/UGS to `172.16.1.130:23`, or jog raw:
|
latency, default 200). The sink applies the full analog machine config
|
||||||
|
itself at init (×8 modes, decay 1, motor_lock 8, laser latched, PIC
|
||||||
|
hold currents) and swaps PIC run/hold currents around motion — the old
|
||||||
|
manual sysfs block is no longer needed. If the baked $-defaults
|
||||||
|
changed since the last run, `$RST=$` once (stored settings win).
|
||||||
|
4. Connect LightBurn/UGS to `172.16.1.97:23`, or jog raw:
|
||||||
`$J=G91X40F1200`.
|
`$J=G91X40F1200`.
|
||||||
|
|
||||||
## Hardware facts bank (measured)
|
## Hardware facts bank (measured)
|
||||||
@@ -92,6 +108,17 @@ glowforge pulse-stream sink).
|
|||||||
half-steps ≈ 10.6 mm ≈ 0.417"; 0.3534 mm/half-step. Never blind-drive Z
|
half-steps ≈ 10.6 mm ≈ 0.417"; 0.3534 mm/half-step. Never blind-drive Z
|
||||||
— hall-supervised only.
|
— hall-supervised only.
|
||||||
- XY: 0.15 mm per full step; DIR bit set = −X / +Y (Y1/Y2 complementary).
|
- XY: 0.15 mm per full step; DIR bit set = −X / +Y (Y1/Y2 complementary).
|
||||||
|
- Factory motion profile (measured from `_RESOURCES` pulse streams with
|
||||||
|
`puls_profile.py`): accel ≈ 700 mm/s² X / 590 mm/s² Y on v2.6.0
|
||||||
|
firmware (2018 firmware used ≈1000); header HAxr=132/HAyr=112/HAar=133
|
||||||
|
⇒ ≈5.3 mm/s² per HA unit. Travel moves peak 202 mm/s vector
|
||||||
|
(≈ 8 in/s) at STfr=28160 Hz; prints/hunts run STfr=10000. Cut feed in
|
||||||
|
the sample print: 145 mm/s. Z cadence ≈ 61–115 ms per half-step
|
||||||
|
(≈ 5.7 mm/s max).
|
||||||
|
- Factory analog config (constant across all captured jobs, 2018→2026):
|
||||||
|
PIC currents X 135 run / 33 hold, Y 22 run / 5 hold (axis DAC scales
|
||||||
|
differ by design); x/y_decay=1; ×8 microstepping; run currents applied
|
||||||
|
only while motion plays, hold otherwise.
|
||||||
- Laser PWM: 39.98 kHz register-verified (divider 13 × 127 counts).
|
- Laser PWM: 39.98 kHz register-verified (divider 13 × 127 counts).
|
||||||
- Switches: truthy = closed/OK; SW_INTERLOCK reads False on units without
|
- Switches: truthy = closed/OK; SW_INTERLOCK reads False on units without
|
||||||
the rear plug — must NOT gate motion (beam is hardware-gated).
|
the rear plug — must NOT gate motion (beam is hardware-gated).
|
||||||
@@ -100,11 +127,11 @@ glowforge pulse-stream sink).
|
|||||||
|
|
||||||
## Next work (in rough order)
|
## Next work (in rough order)
|
||||||
|
|
||||||
1. **Backend milestone 2 — motion quality**: motion is loud/jerky at the
|
1. **Backend milestone 2 — motion quality: DONE 2026-08-02** (see status
|
||||||
10 kHz tick. Raise `GFSINK_RATE` to 20–50 kHz (finer step-timing
|
section above). Remaining human check: listen/watch a jog session for
|
||||||
quantization), find factory-true run currents (puls-file headers carry
|
noise and smoothness vs the pre-tuning state (all electrical/protocol
|
||||||
them — see gfutilities settings map XSrc/YSrc) and sane accel/max-rate;
|
checks are green; loudness was likely the 150/150 currents + unset
|
||||||
verify smooth diagonal moves and feed-hold mid-move.
|
decay mode, both now factory-true).
|
||||||
2. **Laser mapping** (gated on the scope session): spindle → power bytes
|
2. **Laser mapping** (gated on the scope session): spindle → power bytes
|
||||||
(bit 7) + bit 4 laser-enable, M3/M4/$32 semantics, PWM-reset rule per
|
(bit 7) + bit 4 laser-enable, M3/M4/$32 semantics, PWM-reset rule per
|
||||||
the contract. **No live fire before the standing scope gates**: LASER_PWM
|
the contract. **No live fire before the standing scope gates**: LASER_PWM
|
||||||
|
|||||||
@@ -9,6 +9,8 @@ target board (dev image, python3 present) unless noted.
|
|||||||
| `bench_phase2.py` | End-of-data protocol bench (audit M2–M5): underrun detection/ack, parked no-replay guard, resume(0), continuous-feed stability, 20× run/underrun cycles. Motion-safe (motors locked, laser latched). 16/16 PASS on 2026-07-26. |
|
| `bench_phase2.py` | End-of-data protocol bench (audit M2–M5): underrun detection/ack, parked no-replay guard, resume(0), continuous-feed stability, 20× run/underrun cycles. Motion-safe (motors locked, laser latched). 16/16 PASS on 2026-07-26. |
|
||||||
| `check_pwm.py` | Laser PWM register check (audit M8): reads PWM2 PWMCR/PWMPR via /dev/mem, expects divider 13 × ~127 counts ≈ 40 kHz. The scope on LASER_PWM remains the final pre-live-fire gate. |
|
| `check_pwm.py` | Laser PWM register check (audit M8): reads PWM2 PWMCR/PWMPR via /dev/mem, expects divider 13 × ~127 counts ≈ 40 kHz. The scope on LASER_PWM remains the final pre-live-fire gate. |
|
||||||
| `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 (milestone 2): 700/590 mm/s² accel, 200 mm/s max rate, 28160 Hz travel tick. |
|
||||||
|
| `bench_m2.py` | Milestone-2 motion 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. All-green 2026-08-02 at factory-true settings. |
|
||||||
|
|
||||||
The build scripts borrow the Yocto cross toolchain + sysroot from the ulfius
|
The build scripts borrow the Yocto cross toolchain + sysroot from the ulfius
|
||||||
2.7.15 work directory in the WSL build tree; if that path ages out after a
|
2.7.15 work directory in the WSL build tree; if that path ages out after a
|
||||||
|
|||||||
@@ -0,0 +1,133 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
"""Milestone-2 motion-quality bench: factory-true rates/accels over TCP.
|
||||||
|
|
||||||
|
Runs a bounded, return-to-start jog sequence against grblHAL on the board
|
||||||
|
(default 172.16.1.97:23) and reports peak feed reached, state transitions,
|
||||||
|
and final position drift. Every move is relative and round-trip, so the
|
||||||
|
head ends where it started; the laser stays latched (motion-only backend).
|
||||||
|
|
||||||
|
Sequence: sanity jogs (X, Y, 40 mm out/back at 2400 mm/min), max-rate X
|
||||||
|
out/back (60 mm at F12000 - peaks ~200 mm/s mid-move), diagonal out/back,
|
||||||
|
then a G1 move with a feed-hold/resume in the middle.
|
||||||
|
"""
|
||||||
|
import socket
|
||||||
|
import sys
|
||||||
|
import time
|
||||||
|
|
||||||
|
HOST = sys.argv[1] if len(sys.argv) > 1 else '172.16.1.97'
|
||||||
|
PORT = 23
|
||||||
|
|
||||||
|
|
||||||
|
class Grbl:
|
||||||
|
def __init__(self, host, port):
|
||||||
|
self.s = socket.create_connection((host, port), timeout=5)
|
||||||
|
self.s.settimeout(0.2)
|
||||||
|
self.buf = b''
|
||||||
|
time.sleep(0.5)
|
||||||
|
self.drain()
|
||||||
|
|
||||||
|
def drain(self):
|
||||||
|
try:
|
||||||
|
while True:
|
||||||
|
d = self.s.recv(4096)
|
||||||
|
if not d:
|
||||||
|
break
|
||||||
|
self.buf += d
|
||||||
|
except socket.timeout:
|
||||||
|
pass
|
||||||
|
out, self.buf = self.buf, b''
|
||||||
|
return out.decode('ascii', 'replace')
|
||||||
|
|
||||||
|
def cmd(self, line, timeout=5.0):
|
||||||
|
"""Send a line, wait for ok/error."""
|
||||||
|
self.s.sendall(line.encode() + b'\n')
|
||||||
|
deadline = time.time() + timeout
|
||||||
|
text = ''
|
||||||
|
while time.time() < deadline:
|
||||||
|
text += self.drain()
|
||||||
|
if 'ok' in text or 'error' in text:
|
||||||
|
return text.strip()
|
||||||
|
time.sleep(0.02)
|
||||||
|
return '(timeout) ' + text.strip()
|
||||||
|
|
||||||
|
def status(self):
|
||||||
|
"""Realtime ? query -> raw <...> report or ''."""
|
||||||
|
self.s.sendall(b'?')
|
||||||
|
t = time.time() + 1.0
|
||||||
|
text = ''
|
||||||
|
while time.time() < t:
|
||||||
|
text += self.drain()
|
||||||
|
if '>' in text:
|
||||||
|
break
|
||||||
|
time.sleep(0.02)
|
||||||
|
if '<' in text and '>' in text:
|
||||||
|
return text[text.rfind('<'):text.rfind('>') + 1]
|
||||||
|
return ''
|
||||||
|
|
||||||
|
def wait_idle(self, timeout=30.0, poll=0.05):
|
||||||
|
"""Poll until Idle; return (peak_feed_mm_min, states_seen)."""
|
||||||
|
peak = 0.0
|
||||||
|
states = []
|
||||||
|
deadline = time.time() + timeout
|
||||||
|
while time.time() < deadline:
|
||||||
|
st = self.status()
|
||||||
|
if st:
|
||||||
|
state = st[1:].split('|')[0]
|
||||||
|
if not states or states[-1] != state:
|
||||||
|
states.append(state)
|
||||||
|
for f in st.split('|'):
|
||||||
|
if f.startswith('FS:'):
|
||||||
|
peak = max(peak, float(f[3:].split(',')[0]))
|
||||||
|
if state.startswith('Idle'):
|
||||||
|
return peak, states
|
||||||
|
time.sleep(poll)
|
||||||
|
return peak, states + ['TIMEOUT']
|
||||||
|
|
||||||
|
def rt(self, ch):
|
||||||
|
self.s.sendall(ch)
|
||||||
|
|
||||||
|
|
||||||
|
def main():
|
||||||
|
g = Grbl(HOST, PORT)
|
||||||
|
st = g.status()
|
||||||
|
print('connect: %s' % st)
|
||||||
|
if 'Alarm' in st:
|
||||||
|
print('unlock: %s' % g.cmd('$X'))
|
||||||
|
|
||||||
|
for name, out, back, feed in (
|
||||||
|
('X sanity 40mm', '$J=G91X40F2400', '$J=G91X-40F2400', 2400),
|
||||||
|
('Y sanity 40mm', '$J=G91Y40F2400', '$J=G91Y-40F2400', 2400),
|
||||||
|
('X max-rate 60mm', '$J=G91X60F12000', '$J=G91X-60F12000', 12000),
|
||||||
|
('diag 40mm', '$J=G91X40Y40F8000', '$J=G91X-40Y-40F8000', 8000),
|
||||||
|
):
|
||||||
|
for jog in (out, back):
|
||||||
|
r = g.cmd(jog)
|
||||||
|
if 'error' in r:
|
||||||
|
print('%s: JOG REFUSED: %s' % (name, r))
|
||||||
|
return 1
|
||||||
|
peak, states = g.wait_idle()
|
||||||
|
print('%s %s: peak %.0f mm/min, states %s'
|
||||||
|
% (name, 'out' if jog == out else 'back', peak, states))
|
||||||
|
|
||||||
|
# Feed-hold mid-move: G1 at 600 mm/min takes 3 s for 30 mm.
|
||||||
|
print('hold test: %s' % g.cmd('G91'))
|
||||||
|
g.cmd('G1X30F600', timeout=0.5) # ok arrives while moving
|
||||||
|
time.sleep(1.0)
|
||||||
|
g.rt(b'!')
|
||||||
|
time.sleep(0.8)
|
||||||
|
st = g.status()
|
||||||
|
print('after !: %s' % st)
|
||||||
|
held = 'Hold' in st
|
||||||
|
g.rt(b'~')
|
||||||
|
peak, states = g.wait_idle()
|
||||||
|
print('after ~: states %s' % states)
|
||||||
|
g.cmd('G1X-30F2400', timeout=0.5) # return to start
|
||||||
|
g.wait_idle()
|
||||||
|
print('hold worked: %s' % held)
|
||||||
|
|
||||||
|
print('final: %s' % g.status())
|
||||||
|
return 0
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
sys.exit(main())
|
||||||
@@ -0,0 +1,224 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
"""Extract motion profiles from Glowforge pulse files.
|
||||||
|
|
||||||
|
Decodes a factory .puls byte stream (one byte per machine tick) and reports
|
||||||
|
the velocity/acceleration profile the factory planner actually produced:
|
||||||
|
peak axis/vector speeds, acceleration ramp slopes, and per-move segments.
|
||||||
|
Used to derive factory-true grblHAL settings ($110/$111 max rate, $120/$121
|
||||||
|
accel) for the ForgeFIRM step backend (milestone 2, motion quality).
|
||||||
|
|
||||||
|
Accepts either a raw header-stripped stream (_RESOURCES/MOTION/*.puls) or a
|
||||||
|
full download with the GF1 header (magic at [1:4], total header length at
|
||||||
|
[4:8], then 8-byte key/value records). Header settings (STfr, XSmm) override
|
||||||
|
the --rate/--mode defaults when present.
|
||||||
|
|
||||||
|
Byte layout (kernel-module-glowforge/UAPI.md, hardware-verified):
|
||||||
|
bit7 set -> laser power byte (low 7 bits = power, no steps this tick)
|
||||||
|
bit0 X_STEP, bit1 X_DIR (set = -X)
|
||||||
|
bit2 Y_STEP, bit3 Y_DIR (set = +Y)
|
||||||
|
bit5 Z_STEP, bit6 Z_DIR (set = +Z, lens up)
|
||||||
|
bit4 LASER_EN
|
||||||
|
"""
|
||||||
|
import argparse
|
||||||
|
import json
|
||||||
|
import math
|
||||||
|
import sys
|
||||||
|
|
||||||
|
MM_PER_FULL_STEP = 0.15 # X/Y, hardware-verified
|
||||||
|
MM_PER_HALF_STEP_Z = 0.3534 # Z, hardware-verified
|
||||||
|
|
||||||
|
|
||||||
|
def parse_header(data: bytes):
|
||||||
|
"""Return (settings dict, pulse-data offset). Empty dict if headerless."""
|
||||||
|
if len(data) > 8 and data[1:4] == b'GF1':
|
||||||
|
total = int.from_bytes(data[4:8], 'little')
|
||||||
|
hdr = {}
|
||||||
|
pos = 8
|
||||||
|
while pos + 8 <= total:
|
||||||
|
key = data[pos:pos + 4].decode('ascii', 'replace')
|
||||||
|
hdr[key] = int.from_bytes(data[pos + 4:pos + 8], 'little')
|
||||||
|
pos += 8
|
||||||
|
return hdr, total
|
||||||
|
return {}, 0
|
||||||
|
|
||||||
|
|
||||||
|
def decode(data: bytes, offset: int):
|
||||||
|
"""Per-tick signed step deltas and laser state."""
|
||||||
|
n = len(data) - offset
|
||||||
|
dx = [0] * n
|
||||||
|
dy = [0] * n
|
||||||
|
dz = [0] * n
|
||||||
|
laser = [0] * n
|
||||||
|
power = [] # (tick, power value)
|
||||||
|
for i in range(n):
|
||||||
|
b = data[offset + i]
|
||||||
|
if b & 0x80:
|
||||||
|
power.append((i, b & 0x7F))
|
||||||
|
continue
|
||||||
|
if b & 0x01:
|
||||||
|
dx[i] = -1 if b & 0x02 else 1
|
||||||
|
if b & 0x04:
|
||||||
|
dy[i] = 1 if b & 0x08 else -1
|
||||||
|
if b & 0x20:
|
||||||
|
dz[i] = 1 if b & 0x40 else -1
|
||||||
|
laser[i] = (b >> 4) & 1
|
||||||
|
return dx, dy, dz, laser, power
|
||||||
|
|
||||||
|
|
||||||
|
def velocity(deltas, rate: float, mm_per_step: float, win: int):
|
||||||
|
"""Signed mm/s via centered window difference of the cumulative count."""
|
||||||
|
n = len(deltas)
|
||||||
|
cum = [0] * (n + 1)
|
||||||
|
for i, d in enumerate(deltas):
|
||||||
|
cum[i + 1] = cum[i] + d
|
||||||
|
v = [0.0] * n
|
||||||
|
scale = mm_per_step * rate / (2 * win)
|
||||||
|
for i in range(n):
|
||||||
|
lo = max(0, i - win)
|
||||||
|
hi = min(n, i + win)
|
||||||
|
v[i] = (cum[hi] - cum[lo]) * mm_per_step * rate / (hi - lo)
|
||||||
|
return v, cum[n]
|
||||||
|
|
||||||
|
|
||||||
|
def ramps(speed, rate: float, win: int, vmax: float):
|
||||||
|
"""Find sustained accel/decel ramps; return list of fitted slopes (mm/s^2).
|
||||||
|
|
||||||
|
A ramp = a maximal run where speed changes monotonically (within window
|
||||||
|
noise) by at least 25% of vmax. Slope from least-squares fit over the run.
|
||||||
|
"""
|
||||||
|
if vmax <= 0:
|
||||||
|
return []
|
||||||
|
n = len(speed)
|
||||||
|
out = []
|
||||||
|
i = 0
|
||||||
|
step = max(1, win // 4) # sample coarsely; windows overlap anyway
|
||||||
|
idx = list(range(0, n, step))
|
||||||
|
k = 1
|
||||||
|
while k < len(idx):
|
||||||
|
j0 = k - 1
|
||||||
|
rising = speed[idx[k]] > speed[idx[k - 1]]
|
||||||
|
while k < len(idx) and (speed[idx[k]] > speed[idx[k - 1]]) == rising \
|
||||||
|
and abs(speed[idx[k]] - speed[idx[k - 1]]) > 1e-9:
|
||||||
|
k += 1
|
||||||
|
seg = idx[j0:k]
|
||||||
|
dv = speed[seg[-1]] - speed[seg[0]]
|
||||||
|
if len(seg) >= 3 and abs(dv) >= 0.25 * vmax:
|
||||||
|
ts = [s / rate for s in seg]
|
||||||
|
vs = [speed[s] for s in seg]
|
||||||
|
tm = sum(ts) / len(ts)
|
||||||
|
vm = sum(vs) / len(vs)
|
||||||
|
num = sum((t - tm) * (v - vm) for t, v in zip(ts, vs))
|
||||||
|
den = sum((t - tm) ** 2 for t in ts)
|
||||||
|
if den > 0:
|
||||||
|
out.append(num / den)
|
||||||
|
if k < len(idx) and idx[k] == seg[-1]:
|
||||||
|
continue
|
||||||
|
k += 1
|
||||||
|
return out
|
||||||
|
|
||||||
|
|
||||||
|
def segments(speed, rate: float, thresh: float = 0.5):
|
||||||
|
"""Split into moves separated by >=20 ms of near-zero speed."""
|
||||||
|
n = len(speed)
|
||||||
|
gap = int(0.02 * rate)
|
||||||
|
moves = []
|
||||||
|
i = 0
|
||||||
|
while i < n:
|
||||||
|
while i < n and speed[i] <= thresh:
|
||||||
|
i += 1
|
||||||
|
if i >= n:
|
||||||
|
break
|
||||||
|
start = i
|
||||||
|
quiet = 0
|
||||||
|
while i < n and quiet < gap:
|
||||||
|
quiet = quiet + 1 if speed[i] <= thresh else 0
|
||||||
|
i += 1
|
||||||
|
end = i - quiet
|
||||||
|
moves.append((start, end, max(speed[start:end])))
|
||||||
|
return moves
|
||||||
|
|
||||||
|
|
||||||
|
def analyze(path: str, rate: float, mode: int, win_ms: float, dump_csv: str):
|
||||||
|
data = open(path, 'rb').read()
|
||||||
|
hdr, offset = parse_header(data)
|
||||||
|
if hdr:
|
||||||
|
rate = hdr.get('STfr', rate)
|
||||||
|
mode = hdr.get('XSmm', mode)
|
||||||
|
mm_us = MM_PER_FULL_STEP / mode
|
||||||
|
win = max(8, int(win_ms / 1000 * rate))
|
||||||
|
|
||||||
|
dx, dy, dz, laser, power = decode(data, offset)
|
||||||
|
n = len(dx)
|
||||||
|
vx, netx = velocity(dx, rate, mm_us, win)
|
||||||
|
vy, nety = velocity(dy, rate, mm_us, win)
|
||||||
|
speed = [math.hypot(a, b) for a, b in zip(vx, vy)]
|
||||||
|
vmax = max(speed) if speed else 0.0
|
||||||
|
|
||||||
|
rx = ramps([abs(v) for v in vx], rate, win, max(abs(v) for v in vx) if vx else 0)
|
||||||
|
ry = ramps([abs(v) for v in vy], rate, win, max(abs(v) for v in vy) if vy else 0)
|
||||||
|
rv = ramps(speed, rate, win, vmax)
|
||||||
|
|
||||||
|
zticks = [i for i, d in enumerate(dz) if d]
|
||||||
|
zint = [(b - a) / rate * 1000 for a, b in zip(zticks, zticks[1:])]
|
||||||
|
|
||||||
|
moves = segments(speed, rate)
|
||||||
|
|
||||||
|
rep = {
|
||||||
|
'file': path,
|
||||||
|
'ticks': n,
|
||||||
|
'rate_hz': rate,
|
||||||
|
'duration_s': round(n / rate, 3),
|
||||||
|
'microstep_mode': mode,
|
||||||
|
'header': {k: hdr[k] for k in ('STfr', 'XSmm', 'HAxr', 'HAyr', 'HAar',
|
||||||
|
'XSrc', 'YSrc') if k in hdr} or None,
|
||||||
|
'net_travel_mm': {'x': round(netx * mm_us, 3), 'y': round(nety * mm_us, 3)},
|
||||||
|
'steps': {'x': sum(1 for d in dx if d), 'y': sum(1 for d in dy if d),
|
||||||
|
'z': len(zticks)},
|
||||||
|
'peak_speed_mm_s': {'x': round(max((abs(v) for v in vx), default=0), 2),
|
||||||
|
'y': round(max((abs(v) for v in vy), default=0), 2),
|
||||||
|
'vector': round(vmax, 2)},
|
||||||
|
'accel_mm_s2': {
|
||||||
|
'x_ramps': [round(a) for a in rx],
|
||||||
|
'y_ramps': [round(a) for a in ry],
|
||||||
|
'vector_ramps': [round(a) for a in rv],
|
||||||
|
},
|
||||||
|
'z_step_interval_ms': {
|
||||||
|
'min': round(min(zint), 2) if zint else None,
|
||||||
|
'median': round(sorted(zint)[len(zint) // 2], 2) if zint else None,
|
||||||
|
},
|
||||||
|
'laser_on_ticks': sum(laser),
|
||||||
|
'power_bytes': power[:8],
|
||||||
|
'moves': [{'t0_s': round(a / rate, 3), 't1_s': round(b / rate, 3),
|
||||||
|
'peak_mm_s': round(p, 2)} for a, b, p in moves[:20]],
|
||||||
|
'move_count': len(moves),
|
||||||
|
}
|
||||||
|
|
||||||
|
if dump_csv:
|
||||||
|
stride = max(1, win // 4)
|
||||||
|
with open(dump_csv, 'w') as f:
|
||||||
|
f.write('t_s,vx_mm_s,vy_mm_s,speed_mm_s\n')
|
||||||
|
for i in range(0, n, stride):
|
||||||
|
f.write('%.4f,%.2f,%.2f,%.2f\n' % (i / rate, vx[i], vy[i], speed[i]))
|
||||||
|
return rep
|
||||||
|
|
||||||
|
|
||||||
|
def main():
|
||||||
|
ap = argparse.ArgumentParser(description=__doc__.splitlines()[0])
|
||||||
|
ap.add_argument('files', nargs='+')
|
||||||
|
ap.add_argument('--rate', type=float, default=10000,
|
||||||
|
help='machine tick Hz for headerless files (default 10000)')
|
||||||
|
ap.add_argument('--mode', type=int, default=8,
|
||||||
|
help='X/Y microstep mode for headerless files (default 8)')
|
||||||
|
ap.add_argument('--win-ms', type=float, default=8.0,
|
||||||
|
help='velocity window half-width in ms (default 8)')
|
||||||
|
ap.add_argument('--csv', help='dump velocity profile CSV (single file only)')
|
||||||
|
args = ap.parse_args()
|
||||||
|
for path in args.files:
|
||||||
|
rep = analyze(path, args.rate, args.mode, args.win_ms,
|
||||||
|
args.csv if len(args.files) == 1 else None)
|
||||||
|
json.dump(rep, sys.stdout, indent=2)
|
||||||
|
print()
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
main()
|
||||||
Reference in New Issue
Block a user