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<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:
@@ -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))
|
||||
|
||||
Reference in New Issue
Block a user