From 226b44053c4b809b51d11f0ac9f975dfd1868ec4 Mon Sep 17 00:00:00 2001 From: Henrik Muehe Date: Sun, 3 May 2026 14:28:57 +0200 Subject: [PATCH] Z-A coupling interlock: prevent collision between Z and A tools 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' 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. --- src/py/bbctrl/AuxAxis.py | 18 +- src/py/bbctrl/AuxPreprocessor.py | 231 +++++++++++++++++- src/py/bbctrl/ExternalAxis.py | 135 ++++++++++ src/py/bbctrl/FileHandler.py | 10 +- src/py/bbctrl/Planner.py | 28 ++- .../src/components/AAxisSettings.svelte | 32 +++ 6 files changed, 441 insertions(+), 13 deletions(-) diff --git a/src/py/bbctrl/AuxAxis.py b/src/py/bbctrl/AuxAxis.py index d0d16ba..12da8f9 100644 --- a/src/py/bbctrl/AuxAxis.py +++ b/src/py/bbctrl/AuxAxis.py @@ -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']) diff --git a/src/py/bbctrl/AuxPreprocessor.py b/src/py/bbctrl/AuxPreprocessor.py index 3fee529..fcf9f28 100644 --- a/src/py/bbctrl/AuxPreprocessor.py +++ b/src/py/bbctrl/AuxPreprocessor.py @@ -59,15 +59,55 @@ _ATC_M_RE = re.compile( # A axis through gplan.) _W_TOKEN_RE = re.compile(r'(? K), we inject `G0 A` 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` 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::) 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) diff --git a/src/py/bbctrl/ExternalAxis.py b/src/py/bbctrl/ExternalAxis.py index ac2c691..9d2c424 100644 --- a/src/py/bbctrl/ExternalAxis.py +++ b/src/py/bbctrl/ExternalAxis.py @@ -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 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.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)) diff --git a/src/py/bbctrl/FileHandler.py b/src/py/bbctrl/FileHandler.py index 308958b..61a492e 100644 --- a/src/py/bbctrl/FileHandler.py +++ b/src/py/bbctrl/FileHandler.py @@ -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') diff --git a/src/py/bbctrl/Planner.py b/src/py/bbctrl/Planner.py index c2af59e..9d055f6 100644 --- a/src/py/bbctrl/Planner.py +++ b/src/py/bbctrl/Planner.py @@ -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') diff --git a/src/svelte-components/src/components/AAxisSettings.svelte b/src/svelte-components/src/components/AAxisSettings.svelte index 9c8c02f..7ed9920 100644 --- a/src/svelte-components/src/components/AAxisSettings.svelte +++ b/src/svelte-components/src/components/AAxisSettings.svelte @@ -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 @@ +

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 rule + in machine coordinates is + A − Z ≤ (A_home − Z_home) + clearance. + When enabled, the planner refuses moves that would violate + it and the gcode preprocessor injects pre-position A moves + into uploaded files. +

+
+
+ + +
+ +
+ + + +
+ +
+ + + +
+
+

Planner Limits