Jog: enforce Z-A coupling on hold-to-jog
Pendant hold-to-jog now picks the more restrictive of the soft limit and the Z-A coupling bound when computing target_steps for the ESP. The coupling rule (a - z <= K) caps how high A may go for the current Z; only the +A direction (toward larger machine A) is constrained, -A jogs are unaffected. ExternalAxis already exposes couple_K and _z_machine_now; we project a_max_mm = z_now + K into step space via the same _mm_to_steps the rest of AuxAxis uses. The combined helper _a_combined_target_steps picks whichever of the two targets is reached first when moving in . The log line includes target_src so journalctl shows whether a stop was triggered by softlimit or coupling. Refusal-on-press logic was extended to use the combined target so we won't even start a jog when sitting on a coupling-blocked position. Limitation: the target is computed once at JOG start. If Z drops during the jog the bound moves with it; this version doesn't re-evaluate. Z motion during a manual A jog is rare in practice (both hands are on the pendant), but a periodic re-check is on the follow-up list.
This commit is contained in:
@@ -117,19 +117,10 @@ class Jog(inevent.JogHandler):
|
|||||||
self.log.warning('A-axis jog_stop failed: %s', e)
|
self.log.warning('A-axis jog_stop failed: %s', e)
|
||||||
|
|
||||||
def _a_soft_limit_target_steps(self, aux, direction):
|
def _a_soft_limit_target_steps(self, aux, direction):
|
||||||
"""Return a step-counter target that the ESP should ramp to
|
"""Return a step-counter target for the configured soft
|
||||||
a smooth stop at when jogging in `direction`. Returns None
|
limit (`min_mm` / `max_mm`) on the `direction` side of the
|
||||||
when no soft limits should be enforced (axis unhomed or
|
current position, or None when no limit applies (axis
|
||||||
limits not configured).
|
unhomed or limits not configured)."""
|
||||||
|
|
||||||
The ESP's `g_pos` is the raw signed step counter; the host
|
|
||||||
side mirror (`aux._pos_steps`) tracks it. Soft limits are
|
|
||||||
configured in machine-mm; we project them into step space
|
|
||||||
with the same `_mm_to_steps` used for ordinary moves.
|
|
||||||
|
|
||||||
Direction sign: when `dir_sign=+1` (typical), positive jog
|
|
||||||
direction increases g_pos. We pick the limit whose step
|
|
||||||
value is on the `direction` side of the current g_pos."""
|
|
||||||
try:
|
try:
|
||||||
if not bool(aux._homed):
|
if not bool(aux._homed):
|
||||||
return None
|
return None
|
||||||
@@ -140,25 +131,92 @@ class Jog(inevent.JogHandler):
|
|||||||
return None
|
return None
|
||||||
lo_steps = aux._mm_to_steps(lo_mm)
|
lo_steps = aux._mm_to_steps(lo_mm)
|
||||||
hi_steps = aux._mm_to_steps(hi_mm)
|
hi_steps = aux._mm_to_steps(hi_mm)
|
||||||
# _mm_to_steps applies dir_sign, so lo_steps may be
|
# _mm_to_steps applies dir_sign; sort so we know which
|
||||||
# numerically larger than hi_steps when dir_sign<0.
|
# is "more positive in g_pos".
|
||||||
# Sort so we know which is "more positive in g_pos".
|
|
||||||
top_steps = max(lo_steps, hi_steps)
|
top_steps = max(lo_steps, hi_steps)
|
||||||
bottom_steps = min(lo_steps, hi_steps)
|
bottom_steps = min(lo_steps, hi_steps)
|
||||||
return top_steps if direction > 0 else bottom_steps
|
return top_steps if direction > 0 else bottom_steps
|
||||||
except Exception:
|
except Exception:
|
||||||
return None
|
return None
|
||||||
|
|
||||||
|
def _a_coupling_target_steps(self, ext, direction):
|
||||||
|
"""Return a step-counter target that prevents the Z-A
|
||||||
|
coupling rule (a - z <= K) from being violated by this jog.
|
||||||
|
Returns None when coupling is disabled or doesn't constrain
|
||||||
|
motion in `direction`.
|
||||||
|
|
||||||
|
The constraint is on machine-mm: the rule limits how far A
|
||||||
|
may go *up* (toward larger machine A) for the current Z. So
|
||||||
|
only the +A jog direction can ever violate it; -A jogs are
|
||||||
|
unconstrained by coupling and we return None for them.
|
||||||
|
|
||||||
|
Note: 'direction' here refers to the gamepad axis sign, not
|
||||||
|
machine-mm. dir_sign in aux config maps gamepad+ to
|
||||||
|
machine+ steps. We translate via the existing
|
||||||
|
ext._a_machine_now / aux._mm_to_steps so the result is in
|
||||||
|
the same g_pos space as _a_soft_limit_target_steps."""
|
||||||
|
try:
|
||||||
|
if not ext.couple_z_enabled:
|
||||||
|
return None
|
||||||
|
if not bool(ext.aux._homed):
|
||||||
|
return None
|
||||||
|
K = ext.couple_K
|
||||||
|
if K is None:
|
||||||
|
return None
|
||||||
|
z_now = ext._z_machine_now()
|
||||||
|
if z_now is None:
|
||||||
|
return None
|
||||||
|
# Max permitted A in machine-mm: a_max = z_now + K.
|
||||||
|
a_max_mm = float(z_now) + float(K)
|
||||||
|
a_max_steps = ext.aux._mm_to_steps(a_max_mm)
|
||||||
|
# The coupling only caps the *upper* side (more-positive
|
||||||
|
# machine A). With dir_sign=+1 that's g_pos+; with
|
||||||
|
# dir_sign=-1 it's g_pos-. Jogs in the opposite gamepad
|
||||||
|
# direction don't approach the coupling bound, return
|
||||||
|
# None so the soft-limit target alone applies.
|
||||||
|
dir_sign = 1 if int(ext.aux._cfg.get('dir_sign', 1)) >= 0 else -1
|
||||||
|
# Gamepad+ moves toward larger machine-mm when dir_sign>0.
|
||||||
|
machine_dir = direction * dir_sign
|
||||||
|
if machine_dir <= 0:
|
||||||
|
return None
|
||||||
|
return a_max_steps
|
||||||
|
except Exception:
|
||||||
|
return None
|
||||||
|
|
||||||
|
def _a_combined_target_steps(self, ext, direction):
|
||||||
|
"""Pick the more restrictive of soft-limit and coupling
|
||||||
|
targets. Returns (target_steps, source_label) where
|
||||||
|
target_steps is None when neither rule applies."""
|
||||||
|
soft = self._a_soft_limit_target_steps(ext.aux, direction)
|
||||||
|
couple = self._a_coupling_target_steps(ext, direction)
|
||||||
|
if soft is None and couple is None:
|
||||||
|
return None, 'none'
|
||||||
|
if soft is None: return couple, 'coupling'
|
||||||
|
if couple is None: return soft, 'softlimit'
|
||||||
|
# Both present: pick whichever is reached first when moving
|
||||||
|
# in `direction` from the current g_pos.
|
||||||
|
try:
|
||||||
|
cur = int(ext.aux._pos_steps)
|
||||||
|
except Exception:
|
||||||
|
cur = 0
|
||||||
|
if direction > 0:
|
||||||
|
return ((soft, 'softlimit') if soft <= couple
|
||||||
|
else (couple, 'coupling'))
|
||||||
|
else:
|
||||||
|
return ((soft, 'softlimit') if soft >= couple
|
||||||
|
else (couple, 'coupling'))
|
||||||
|
|
||||||
def _a_start(self, direction):
|
def _a_start(self, direction):
|
||||||
ext = getattr(self.ctrl, 'ext_axis', None)
|
ext = getattr(self.ctrl, 'ext_axis', None)
|
||||||
ext_state = ('present' if (ext is not None and ext.enabled)
|
ext_state = ('present' if (ext is not None and ext.enabled)
|
||||||
else 'unavailable')
|
else 'unavailable')
|
||||||
scale = self._a_speed_scale()
|
scale = self._a_speed_scale()
|
||||||
target_steps = None
|
target_steps = None
|
||||||
|
target_src = 'none'
|
||||||
cur_steps = None
|
cur_steps = None
|
||||||
if ext is not None and ext.enabled:
|
if ext is not None and ext.enabled:
|
||||||
target_steps = self._a_soft_limit_target_steps(
|
target_steps, target_src = self._a_combined_target_steps(
|
||||||
ext.aux, direction)
|
ext, direction)
|
||||||
try: cur_steps = int(ext.aux._pos_steps)
|
try: cur_steps = int(ext.aux._pos_steps)
|
||||||
except Exception: cur_steps = None
|
except Exception: cur_steps = None
|
||||||
if A_DRY_RUN:
|
if A_DRY_RUN:
|
||||||
@@ -172,9 +230,9 @@ class Jog(inevent.JogHandler):
|
|||||||
self.log.info(
|
self.log.info(
|
||||||
'AJOG DRYRUN _a_start dir=%+d ext=%s speed=%d scale=%.4f '
|
'AJOG DRYRUN _a_start dir=%+d ext=%s speed=%d scale=%.4f '
|
||||||
'step_max=%d accel=%d cur_steps=%s target_steps=%s '
|
'step_max=%d accel=%d cur_steps=%s target_steps=%s '
|
||||||
'(would send JOG)',
|
'target_src=%s (would send JOG)',
|
||||||
direction, ext_state, self.speed, scale, step_max, accel,
|
direction, ext_state, self.speed, scale, step_max, accel,
|
||||||
cur_steps, target_steps)
|
cur_steps, target_steps, target_src)
|
||||||
return
|
return
|
||||||
if ext is None or not ext.enabled or direction == 0:
|
if ext is None or not ext.enabled or direction == 0:
|
||||||
return
|
return
|
||||||
@@ -182,31 +240,35 @@ class Jog(inevent.JogHandler):
|
|||||||
aux = ext.aux
|
aux = ext.aux
|
||||||
max_rate = max(1, int(int(aux._cfg['step_max_sps']) * scale))
|
max_rate = max(1, int(int(aux._cfg['step_max_sps']) * scale))
|
||||||
accel = int(aux._cfg['step_accel_sps2'])
|
accel = int(aux._cfg['step_accel_sps2'])
|
||||||
# If the axis is already at-or-past the soft-limit
|
# If the axis is already at-or-past the more-restrictive
|
||||||
# boundary in the requested direction, refuse the jog
|
# boundary (soft limit OR Z-A coupling) in the requested
|
||||||
# rather than sending a wrong-side target the ESP would
|
# direction, refuse the jog rather than sending a
|
||||||
# reject. The host knows position immediately whereas
|
# wrong-side target the ESP would reject.
|
||||||
# the ESP only learns g_pos via WPOS?.
|
|
||||||
if target_steps is not None and cur_steps is not None:
|
if target_steps is not None and cur_steps is not None:
|
||||||
at_limit = ((direction > 0 and cur_steps >= target_steps)
|
at_limit = ((direction > 0 and cur_steps >= target_steps)
|
||||||
or (direction < 0 and cur_steps <= target_steps))
|
or (direction < 0 and cur_steps <= target_steps))
|
||||||
if at_limit:
|
if at_limit:
|
||||||
self.log.info(
|
self.log.info(
|
||||||
'A-axis jog refused: at soft limit '
|
'A-axis jog refused: at %s limit '
|
||||||
'(cur=%d target=%d dir=%+d)',
|
'(cur=%d target=%d dir=%+d)',
|
||||||
cur_steps, target_steps, direction)
|
target_src, cur_steps, target_steps, direction)
|
||||||
return
|
return
|
||||||
# ignore_limits=True (safe=0) when the axis is unhomed:
|
# ignore_limits=True (safe=0) when the axis is unhomed:
|
||||||
# pendant jog is allowed before homing for setup. When
|
# pendant jog is allowed before homing for setup. When
|
||||||
# homed, soft limits are enforced via target_steps and
|
# homed, soft limits AND Z-A coupling are enforced via
|
||||||
# the ESP's hardware-limit abort still applies
|
# target_steps and the ESP's hardware-limit abort still
|
||||||
# unconditionally (movingTowardLimit in jogTask).
|
# applies unconditionally (movingTowardLimit in
|
||||||
|
# jogTask).
|
||||||
ignore = not bool(aux._homed)
|
ignore = not bool(aux._homed)
|
||||||
aux.jog_start(direction,
|
aux.jog_start(direction,
|
||||||
max_rate_sps=max_rate,
|
max_rate_sps=max_rate,
|
||||||
accel_sps2=accel,
|
accel_sps2=accel,
|
||||||
ignore_limits=ignore,
|
ignore_limits=ignore,
|
||||||
target_steps=target_steps)
|
target_steps=target_steps)
|
||||||
|
if target_steps is not None:
|
||||||
|
self.log.info(
|
||||||
|
'A-axis jog_start dir=%+d cur=%d target=%d (%s)',
|
||||||
|
direction, cur_steps, target_steps, target_src)
|
||||||
except Exception as e:
|
except Exception as e:
|
||||||
self.log.warning('A-axis jog_start failed: %s', e)
|
self.log.warning('A-axis jog_start failed: %s', e)
|
||||||
|
|
||||||
|
|||||||
Reference in New Issue
Block a user