muehe/onefinity-firmware
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Z-A coupling interlock: prevent collision between Z and A tools

Browse Source 
The auxiliary A axis carries a tool that hangs below the Z spindle.
Beyond a small Z descent the two physically collide unless A drops
with Z. Enforce in machine coords:

    A_machine - Z_machine <= K
    K = (A_home_mm - z_home_mm) + couple_z_clearance_mm

With our setup K = (134 - 0) + 22 = 156. At rest A=134 Z=0, A-Z=134
which is fine. Z can descend 22mm before the rule starts forcing A
down with it.

Two complementary layers:

(1) AuxPreprocessor injection (auto-fix uploaded files)
    Tracks modal Z, A and distance mode (G90/G91) while scanning the
    file. When a line would put A above Z by more than the clearance
    we emit a 'G0 A<safe>' BEFORE the line so A is already at the
    safe position when Z descends. Endpoint check is sufficient
    because Z moves monotonically along a single line.

    Errors are raised (not silently auto-fixed) when:
      - the line lifts A above the safe band while Z stays put
        (would require auto-injecting a Z-up which could swing
        through a fixture)
      - the line endpoint targets an A above the safe band

    G91 disables injection with a one-shot warning; the runtime
    check still applies.

(2) Runtime check (ExternalAxis.check_coupling)
    Single source of truth for live motion. Hooked into:
      * Planner.__encode for every line block (covers MDI and
        running programs - gplan emits machine-coord targets)
      * ExternalAxis.execute_to_mm/enqueue_target_mm/enqueue_line
        for direct A motion (covers UI jog/move and planner-A
        dispatch)
    Raises ExternalAxisError on violation; gplan and the API both
    surface the message. Skipped when coupling is disabled or the
    axis isn't homed (mirrors the soft-limit gate).

    Continuous Z jog from the AVR is not gated - it's an active
    operator action without a pre-known endpoint. Operator-driven
    over-travel during continuous jog will be caught by the next
    MDI/file-load attempt.

Configuration in aux.json:
    couple_z_enabled        bool   default true (per agreed setup)
    couple_z_clearance_mm   float  default 22.0
    z_home_mm               float  default 0.0

Surfaced in the new Z-A Coupling section of the A Axis settings
page with a description of the rule. Existing aux.json files get
the new keys via the merged-defaults path on read.

Tested locally with synthetic gcode covering Z descent, combined
moves, A lift while Z deep, G92 reset, G91 mode, and combined
Z+A target violations.
This commit is contained in:
Henrik Muehe
2026-05-03 14:28:57 +02:00
parent 77bda775dd
commit 4d71585a00
6 changed files with 441 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
src/py/bbctrl/AuxAxis.py
View File

@@ -70,6 +70,22 @@ DEFAULTS = {
'step_accel_sps2': 12000,
'step_start_sps': 200,
'limit_low': True,
# ------------------------------------------------------------------
# Z-A coupling interlock
# ------------------------------------------------------------------
# The auxiliary A axis carries a tool that physically hangs below
# the Z-axis spindle nose. Beyond a certain Z descent the two
# collide unless A drops with Z. The constraint, in machine coords,
# is:
# A_machine - Z_machine <= K
# where K = (A_home_mm - z_home_mm) + couple_z_clearance_mm.
# When enabled this is enforced everywhere motion can be
# initiated (planner, MDI, jog, file load) and the AuxPreprocessor
# injects pre-position A moves before Z descends past the safe
# band.
'couple_z_enabled': True,
'couple_z_clearance_mm': 22.0, # Z drop allowed before A must follow
'z_home_mm': 0.0, # Z's machine position when homed
}
@@ -361,7 +377,7 @@ class AuxAxis(object):
return # no limits
if target_mm < lo - 1e-6 or target_mm > hi + 1e-6:
raise AuxAxisError(
'W=%.3f out of soft limits [%.3f, %.3f]' % (target_mm, lo, hi))
'A=%.3f out of soft limits [%.3f, %.3f]' % (target_mm, lo, hi))
def _mm_to_steps(self, mm):
spm = float(self._cfg['steps_per_mm'])

231
src/py/bbctrl/AuxPreprocessor.py
View File

@@ -59,15 +59,55 @@ _ATC_M_RE = re.compile(
# A axis through gplan.)
_W_TOKEN_RE = re.compile(r'(?<![A-Za-z_0-9])[Ww]\s*[-+]?\d*\.?\d+')
# Match a single axis word (letter + optional whitespace + signed decimal)
# for Z, A, X, Y. Used to extract modal targets while preserving the
# original line for emission. We deliberately ignore I/J/K/R (arc params)
# because they're not endpoints.
_AXIS_TOKEN_RES = {
'z': re.compile(r'(?<![A-Za-z_0-9])[Zz]\s*([-+]?\d*\.?\d+)'),
'a': re.compile(r'(?<![A-Za-z_0-9])[Aa]\s*([-+]?\d*\.?\d+)'),
'x': re.compile(r'(?<![A-Za-z_0-9])[Xx]\s*([-+]?\d*\.?\d+)'),
'y': re.compile(r'(?<![A-Za-z_0-9])[Yy]\s*([-+]?\d*\.?\d+)'),
}
_G_CODE_RE = re.compile(r'(?<![A-Za-z_0-9])[Gg]\s*0*(\d+(?:\.\d+)?)')
class AuxPreprocessorError(Exception):
pass
class AuxPreprocessor(object):
def __init__(self, log=None):
def __init__(self, log=None, coupling=None):
"""`coupling`, when supplied, enables Z-A coupling injection.
Expected shape:
{
'enabled': bool,
'clearance_mm': float, # max (A_wc - Z_wc)
'a_initial_wc': float, # A's work-coord position at
# file start (typically 0 if
# operator zeroed at home)
'z_initial_wc': float, # Z's work-coord position at
# file start (typically 0)
}
Pass None to disable injection (preprocessor still rewrites
ATC M-codes)."""
self.log = log
self._w_warned = False
self._coupling = coupling if (coupling and
coupling.get('enabled')) else None
# Modal state used while scanning the file.
if self._coupling is not None:
self._a_wc = float(coupling.get('a_initial_wc', 0.0))
self._z_wc = float(coupling.get('z_initial_wc', 0.0))
self._K = float(coupling.get('clearance_mm', 0.0))
else:
self._a_wc = 0.0
self._z_wc = 0.0
self._K = 0.0
self._g91_warned = False
# Distance mode: True for absolute (G90), False for incremental
# (G91). Per RS274 the modal default at start is G90.
self._g90 = True
def _info(self, msg):
if self.log: self.log.info(msg)
@@ -78,9 +118,12 @@ class AuxPreprocessor(object):
# ------------------------------------------------------------------ scan
@staticmethod
def file_uses_aux(path):
def file_uses_aux(path, coupling=None):
"""Quick check: does this file contain anything the preprocessor
would rewrite (currently: just ATC M-codes)?"""
would rewrite? Returns True for ATC M-codes always, and for
any Z/A move if coupling is enabled (we have to scan to know
whether injection is needed, so any motion file qualifies)."""
couple_active = bool(coupling and coupling.get('enabled'))
try:
with open(path, 'r', encoding='utf-8', errors='replace') as f:
for line in f:
@@ -88,6 +131,10 @@ class AuxPreprocessor(object):
code = code.split(';', 1)[0]
if _ATC_M_RE.search(code):
return True
if couple_active:
if _AXIS_TOKEN_RES['z'].search(code) or \
_AXIS_TOKEN_RES['a'].search(code):
return True
except Exception:
pass
return False
@@ -95,6 +142,170 @@ class AuxPreprocessor(object):
# Backwards-compat alias.
file_uses_w = file_uses_aux
# ------------------------------------------------------------------ Z-A coupling
#
# Track modal Z and A targets across the file. Whenever a line
# would put A above Z by more than `clearance_mm` (i.e. A_wc -
# Z_wc > K), we inject `G0 A<safe>` immediately before it so A is
# already at the safe position when Z descends. The injected move
# uses G0 (rapid) so it's quick.
#
# Endpoint-only check: gplan plans line endpoints. As long as
# (target_A_wc - target_Z_wc) <= K, the trajectory stays safe
# because Z's *minimum* during a single line is its endpoint (Z
# moves monotonically along a single line block in absolute
# mode) and A is held at the pre-positioned value during the move.
def _extract_g_codes(self, code):
"""Return the set of G-codes referenced on `code`. Numeric
only, e.g. {0, 1, 90, 17}. Used to track modal state."""
out = set()
for m in _G_CODE_RE.finditer(code):
try:
out.add(int(float(m.group(1))))
except Exception:
pass
return out
def _extract_axis(self, axis, code):
"""Return the last value of `axis` token on `code`, or None."""
rx = _AXIS_TOKEN_RES.get(axis)
if rx is None:
return None
last = None
for m in rx.finditer(code):
try:
last = float(m.group(1))
except Exception:
pass
return last
def _maybe_inject_a_down(self, code, fout):
"""Inspect `code` (with comments stripped) for an upcoming Z
descent; emit a `G0 A<safe>` line on `fout` if needed and
update self._a_wc accordingly. Returns True if anything was
injected.
On a violation that cannot be fixed by lowering A (e.g. the
operator wrote `G0 A0` while Z is too deep), raise
AuxPreprocessorError so the file load surfaces the problem -
per the rule we agreed: error, don't silently insert a Z-up.
"""
if self._coupling is None:
return False
# Distance mode tracking.
gs = self._extract_g_codes(code)
if 90 in gs: self._g90 = True
if 91 in gs:
if self._g90 and not self._g91_warned:
self._warn(
'AuxPreprocessor: G91 (incremental mode) detected; '
'Z-A coupling injection is disabled for the rest of '
'the file. The runtime check still applies.')
self._g91_warned = True
self._g90 = False
# G92 sets coordinate offsets. The new modal value of an
# axis is whatever value follows on the same word (e.g.
# G92 A0 sets A_wc = 0). Apply that and skip injection.
if 92 in gs:
new_a = self._extract_axis('a', code)
new_z = self._extract_axis('z', code)
if new_a is not None: self._a_wc = new_a
if new_z is not None: self._z_wc = new_z
return False
# In incremental mode we can still track approximately, but
# the user has been warned; skip injection.
if not self._g90:
return False
new_z_target = self._extract_axis('z', code)
new_a_target = self._extract_axis('a', code)
if new_z_target is None and new_a_target is None:
return False
# Modal values after the line executes.
a_after = new_a_target if new_a_target is not None else self._a_wc
z_after = new_z_target if new_z_target is not None else self._z_wc
eps = 1e-4
if a_after - z_after <= self._K + eps:
# Move is safe as authored. Update modal state.
self._a_wc = a_after
self._z_wc = z_after
return False
# Violation. Two cases:
#
# (a) The line lowers Z (z_after < self._z_wc) and A is
# held or moved upward, so A needs to drop to keep up.
# We can fix this by pre-positioning A at z_after + K
# BEFORE the line - at which point gplan's plan for the
# line is safe at every point along it.
#
# (b) The line raises A above the safe band while Z is
# held (z_after >= self._z_wc) - i.e. the operator
# wrote `G0 A0` while Z is parked deep. Auto-injecting
# a Z-up here is unsafe (Z could swing into a fixture
# or the part) so we error out and let the operator
# author the lift.
safe_a = z_after + self._K
# If the line itself targets an A above the safe band, the
# endpoint violates the rule no matter what we pre-position.
# Refuse rather than emit something that runs the gantry into
# the tool.
if new_a_target is not None and new_a_target > safe_a + eps:
raise AuxPreprocessorError(
'Z-A coupling violation: line targets A=%.3f at '
'Z=%.3f, but max A allowed is %.3f (clearance %.3f). '
'Lower the A target or add a Z-up move first.' % (
new_a_target, z_after, safe_a, self._K))
# If the line raises A above the current safe band but Z
# isn't dropping with it (no Z target on the line, or Z stays
# put), the violation is the operator's A-up, not a Z-down.
# Refuse rather than insert a Z-up (which could swing through
# a fixture or part).
if (new_a_target is not None and
new_a_target > self._a_wc + eps and
new_z_target is None):
raise AuxPreprocessorError(
'Z-A coupling violation at line raising A to %.3f '
'while Z is at %.3f (max A allowed is %.3f given '
'clearance %.3f). Add a Z-up move first.' % (
new_a_target, z_after, safe_a, self._K))
# Case (a): pre-position A.
# Don't move A *up* as part of pre-position - if the safe
# value is above where A already is, we'd lift A into a
# potential collision elsewhere. In practice safe_a < a_wc
# whenever we get here (otherwise no violation), but assert
# to be sure.
if safe_a > self._a_wc + eps:
raise AuxPreprocessorError(
'Z-A coupling: cannot fix line by lowering A '
'(safe A = %.3f > current A = %.3f).' % (
safe_a, self._a_wc))
fout.write('(injected by AuxPreprocessor: Z-A coupling)\n')
fout.write('G0 A%.4f\n' % safe_a)
self._a_wc = safe_a
# Don't update z_wc yet - the original line will do that
# when it runs. But our modal copy must reflect the post-line
# value so subsequent injections compute correctly.
self._z_wc = z_after
# If the original line also moved A, our pre-positioning
# supersedes it (we overwrite a_wc above with safe_a then
# the original line's A target may push it back up). Update
# a_wc to the line's authored A value so further checks see
# the post-line state.
if new_a_target is not None:
self._a_wc = new_a_target
return True
# ------------------------------------------------------------------ run
def process(self, src_path, dst_path):
@@ -124,6 +335,10 @@ class AuxPreprocessor(object):
'subsequent W tokens in this file)')
self._w_warned = True
# Z-A coupling injection BEFORE the line is emitted.
if self._maybe_inject_a_down(code, fout):
rewrote_any = True
# ATC M-codes (M100-M103). Each ATC M-code on the line
# is replaced with its (MSG,HOOK:<event>:) line and
# stripped from the residual.
@@ -156,15 +371,17 @@ class AuxPreprocessor(object):
return rewrote_any
def preprocess_file(src_path, log=None, **_unused):
def preprocess_file(src_path, log=None, coupling=None, **_unused):
"""Convenience: rewrite src_path in place if it contains ATC
M-codes. Returns True if the file was rewritten.
M-codes or needs Z-A coupling injection. Returns True if the
file was rewritten.
`coupling` is an optional dict (see AuxPreprocessor.__init__).
Extra keyword args are accepted for backwards compat (the old
w_first arg is no longer used)."""
if not AuxPreprocessor.file_uses_aux(src_path):
if not AuxPreprocessor.file_uses_aux(src_path, coupling=coupling):
return False
pre = AuxPreprocessor(log=log)
pre = AuxPreprocessor(log=log, coupling=coupling)
fd, tmp = tempfile.mkstemp(prefix='auxpre_', suffix='.nc',
dir=os.path.dirname(src_path) or None)
os.close(fd)

135
src/py/bbctrl/ExternalAxis.py
View File

@@ -185,6 +185,136 @@ class ExternalAxis(object):
'[%.3f, %.3f] mm' % (
self.axis_letter.upper(), target, lo, hi))
# ----------------------------------------------- Z-A coupling
#
# The auxiliary tool hangs below the Z spindle. Beyond a small
# Z descent the two collide unless A drops with Z. The
# constraint, in machine coords, is
#
# A_machine - Z_machine <= K
# K = (A_home_mm - z_home_mm) + couple_z_clearance_mm
#
# Enforced before any motion (planner blocks, MDI, jogs). The
# AuxPreprocessor injects pre-position A moves into uploaded
# files so well-formed gcode runs without having to think about
# this. Disabled when couple_z_enabled is false.
@property
def couple_z_enabled(self):
try:
return bool(self.aux._cfg.get('couple_z_enabled', False))
except Exception:
return False
@property
def couple_K(self):
"""Limit constant K (machine-coord units): the maximum value
of (A_machine - Z_machine) before the tool collides. Returns
None if the rule isn't applicable (coupling disabled or
config missing)."""
try:
cfg = self.aux._cfg
clearance = float(cfg.get('couple_z_clearance_mm', 0.0))
a_home = float(cfg.get('home_position_mm', 0.0))
z_home = float(cfg.get('z_home_mm', 0.0))
return (a_home - z_home) + clearance
except Exception:
return None
@property
def couple_clearance_mm(self):
"""Raw clearance from config: how far Z may travel below its
home before A has to start dropping with it. Used by the
AuxPreprocessor to inject pre-position A moves into uploaded
gcode."""
try:
return float(self.aux._cfg.get('couple_z_clearance_mm', 0.0))
except Exception:
return 0.0
def _z_machine_now(self):
"""Read Z's current machine position from State, or None if
Z isn't homed/reported yet. The AVR reports absolute machine
positions in <axis>p; the work-coord display is computed by
the UI as zp - offset_z, but here we want machine directly."""
try:
st = self.ctrl.state
zp = st.get('zp', None)
if zp is None:
return None
return float(zp)
except Exception:
return None
def _a_machine_now(self):
"""A's current machine position. ExternalAxis tracks this
directly in self._pos_mm (mm in machine coords - we don't
apply G92 to A internally; offset_a is informational)."""
try:
if self._pos_mm is not None:
return float(self._pos_mm)
# Fall back to whatever the ESP last reported.
return float(self.aux.position_mm)
except Exception:
return None
def coupling_for_preprocessor(self):
"""Return the dict the AuxPreprocessor wants for in-file
injection, or None when coupling is off. We assume the
operator authors gcode in a frame where the at-home position
is A_wc=0, Z_wc=0 - which matches our home-zeroed setup.
Files that use a different convention will fall through to
the runtime check."""
if not self.couple_z_enabled:
return None
return {
'enabled': True,
'clearance_mm': self.couple_clearance_mm,
'a_initial_wc': 0.0,
'z_initial_wc': 0.0,
}
def check_coupling(self, target_a_machine=None, target_z_machine=None):
"""Validate that a proposed motion respects the Z-A coupling.
Each argument is a target *machine* mm position; pass None to
keep the current value of that axis. Raises ExternalAxisError
if the resulting (A,Z) pair would violate the constraint.
Skipped when:
* coupling is disabled in aux.json,
* the auxiliary axis isn't homed (we haven't established a
reliable A position yet - the soft-limit gate uses the
same convention),
* Z's current position is unknown (no zp reported yet -
avoid spurious failures during boot before the AVR has
sent its first status frame).
"""
if not self.couple_z_enabled:
return
try:
homed = bool(self.aux._homed)
except Exception:
homed = False
if not homed:
return
K = self.couple_K
if K is None:
return
a_now = self._a_machine_now()
z_now = self._z_machine_now()
a = float(target_a_machine) if target_a_machine is not None else a_now
z = float(target_z_machine) if target_z_machine is not None else z_now
if a is None or z is None:
return
eps = 1e-4
if a - z > K + eps:
raise ExternalAxisError(
'Z-A coupling violation: A=%.3f mm and Z=%.3f mm '
'(machine) would put A above Z by %.3f mm; max '
'allowed is %.3f mm. Drop A or raise Z first.' % (
a, z, a - z, K))
# ----------------------------------------------------------- conversion
def mm_to_steps_delta(self, delta_mm):
@@ -324,6 +454,9 @@ class ExternalAxis(object):
'not connected)' % self.axis_letter)
self._check_soft_limit(ext_mm)
# Coupling: A is in machine coords directly (we don't apply
# a G92 offset to A), so target_a_machine == ext_mm.
self.check_coupling(target_a_machine=ext_mm)
steps, abs_mm = self._compute_move(ext_mm)
if steps == 0:
self._pos_mm = abs_mm
@@ -350,6 +483,7 @@ class ExternalAxis(object):
raise ExternalAxisError(
'External axis %r not available' % self.axis_letter)
self._check_soft_limit(ext_mm)
self.check_coupling(target_a_machine=ext_mm)
steps, abs_mm = self._compute_move(ext_mm)
# Internal mirror only - drives subsequent delta computation.
# state.<axis>p is left to the AVR's status reports.
@@ -383,6 +517,7 @@ class ExternalAxis(object):
raise ExternalAxisError(
'External axis %r not available' % self.axis_letter)
self._check_soft_limit(ext_mm)
self.check_coupling(target_a_machine=ext_mm)
steps, abs_mm = self._compute_move(ext_mm)
delta_mm = abs(abs_mm - (self._pos_mm if self._pos_mm is not None
else 0.0))

10
src/py/bbctrl/FileHandler.py
View File

@@ -109,9 +109,13 @@ class FileHandler(bbctrl.APIHandler):
try:
from bbctrl.AuxPreprocessor import preprocess_file
log = self.get_log('AuxPreprocessor')
if preprocess_file(filename.decode('utf8'), log=log):
log.info('Rewrote ATC M-codes in %s' %
self.uploadFilename)
ext = getattr(self.get_ctrl(), 'ext_axis', None)
coupling = (ext.coupling_for_preprocessor()
if ext is not None else None)
if preprocess_file(filename.decode('utf8'),
log=log, coupling=coupling):
log.info('Rewrote upload (ATC / Z-A coupling) in %s'
% self.uploadFilename)
except Exception:
self.get_log('AuxPreprocessor').exception(
'Aux preprocess failed; uploading unchanged')

28
src/py/bbctrl/Planner.py
View File

@@ -270,6 +270,27 @@ class Planner():
if type != 'set': self.log.info('Cmd:' + log_json(block))
if type == 'line':
# Z-A coupling check: every line block that touches Z (or
# A) is validated against the projected (A,Z) machine
# pair. The ExternalAxis check raises on violation so
# gplan aborts the program with a useful error rather
# than driving the gantry into the auxiliary tool. The
# check is skipped when coupling is disabled or A isn't
# homed (see ExternalAxis.check_coupling).
ext_check = getattr(self.ctrl, 'ext_axis', None)
if ext_check is not None:
target = block.get('target') or {}
z_target = target.get('z')
if z_target is None: z_target = target.get('Z')
a_letter = ext_check.axis_letter
a_target = target.get(a_letter)
if a_target is None:
a_target = target.get(a_letter.upper())
if z_target is not None or a_target is not None:
ext_check.check_coupling(
target_a_machine=a_target,
target_z_machine=z_target)
ext = self._external_axis_for_line(block)
if ext is not None:
# Side effect: enqueue the ESP move on the external-
@@ -476,8 +497,11 @@ class Planner():
# should use A directly.
try:
from bbctrl.AuxPreprocessor import preprocess_file
if preprocess_file(path, log = self.log):
self.log.info('Rewrote ATC M-codes in %s' % path)
ext = getattr(self.ctrl, 'ext_axis', None)
coupling = (ext.coupling_for_preprocessor()
if ext is not None else None)
if preprocess_file(path, log=self.log, coupling=coupling):
self.log.info('Rewrote (ATC / Z-A coupling) in %s' % path)
except Exception:
self.log.exception('Aux preprocess at load failed; '
'attempting to load file unchanged')

32
src/svelte-components/src/components/AAxisSettings.svelte
View File

@@ -32,6 +32,9 @@
step_accel_sps2: number;
step_start_sps: number;
limit_low: boolean;
couple_z_enabled: boolean;
couple_z_clearance_mm: number;
z_home_mm: number;
};
let cfg: AuxConfig | null = null;
@@ -166,6 +169,35 @@
</div>
</fieldset>
<h3>Z-A Coupling</h3>
<p class="tip">
The auxiliary tool hangs below the Z spindle. Beyond a small
Z descent the two collide unless A drops with Z. The rule
in machine coordinates is
<code>A &minus; Z &le; (A_home &minus; Z_home) + clearance</code>.
When enabled, the planner refuses moves that would violate
it and the gcode preprocessor injects pre-position A moves
into uploaded files.
</p>
<fieldset>
<div class="pure-control-group" title="Master switch for the Z-A interlock. When off, no checks are performed.">
<label for="couple_z_enabled">enable coupling</label>
<input id="couple_z_enabled" type="checkbox" bind:checked={cfg.couple_z_enabled} />
</div>
<div class="pure-control-group" title="How far Z may descend below its home position before A must move with it.">
<label for="couple_z_clearance_mm">Z clearance</label>
<input id="couple_z_clearance_mm" type="number" bind:value={cfg.couple_z_clearance_mm} step="any" />
<label for="" class="units">mm</label>
</div>
<div class="pure-control-group" title="Z's machine position when homed. Almost always 0.">
<label for="z_home_mm">Z home position</label>
<input id="z_home_mm" type="number" bind:value={cfg.z_home_mm} step="any" />
<label for="" class="units">mm</label>
</div>
</fieldset>
<h3>Planner Limits</h3>
<fieldset>
<div class="pure-control-group" title="Maximum velocity used by gplan trajectory planning.">