From b59091007cb6be3045caecaee5abe7f4847b8a33 Mon Sep 17 00:00:00 2001 From: Henrik Muehe Date: Sun, 3 May 2026 18:08:48 +0200 Subject: [PATCH] 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. --- src/py/bbctrl/Jog.py | 122 ++++++++++++++++++++++++++++++++----------- 1 file changed, 92 insertions(+), 30 deletions(-) diff --git a/src/py/bbctrl/Jog.py b/src/py/bbctrl/Jog.py index ce045ed..8193cfd 100644 --- a/src/py/bbctrl/Jog.py +++ b/src/py/bbctrl/Jog.py @@ -117,19 +117,10 @@ class Jog(inevent.JogHandler): self.log.warning('A-axis jog_stop failed: %s', e) def _a_soft_limit_target_steps(self, aux, direction): - """Return a step-counter target that the ESP should ramp to - a smooth stop at when jogging in `direction`. Returns None - when no soft limits should be enforced (axis 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.""" + """Return a step-counter target for the configured soft + limit (`min_mm` / `max_mm`) on the `direction` side of the + current position, or None when no limit applies (axis + unhomed or limits not configured).""" try: if not bool(aux._homed): return None @@ -140,25 +131,92 @@ class Jog(inevent.JogHandler): return None lo_steps = aux._mm_to_steps(lo_mm) hi_steps = aux._mm_to_steps(hi_mm) - # _mm_to_steps applies dir_sign, so lo_steps may be - # numerically larger than hi_steps when dir_sign<0. - # Sort so we know which is "more positive in g_pos". + # _mm_to_steps applies dir_sign; sort so we know which + # is "more positive in g_pos". top_steps = max(lo_steps, hi_steps) bottom_steps = min(lo_steps, hi_steps) return top_steps if direction > 0 else bottom_steps except Exception: 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): ext = getattr(self.ctrl, 'ext_axis', None) ext_state = ('present' if (ext is not None and ext.enabled) else 'unavailable') scale = self._a_speed_scale() target_steps = None + target_src = 'none' cur_steps = None if ext is not None and ext.enabled: - target_steps = self._a_soft_limit_target_steps( - ext.aux, direction) + target_steps, target_src = self._a_combined_target_steps( + ext, direction) try: cur_steps = int(ext.aux._pos_steps) except Exception: cur_steps = None if A_DRY_RUN: @@ -172,9 +230,9 @@ class Jog(inevent.JogHandler): self.log.info( 'AJOG DRYRUN _a_start dir=%+d ext=%s speed=%d scale=%.4f ' '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, - cur_steps, target_steps) + cur_steps, target_steps, target_src) return if ext is None or not ext.enabled or direction == 0: return @@ -182,31 +240,35 @@ class Jog(inevent.JogHandler): aux = ext.aux max_rate = max(1, int(int(aux._cfg['step_max_sps']) * scale)) accel = int(aux._cfg['step_accel_sps2']) - # If the axis is already at-or-past the soft-limit - # boundary in the requested direction, refuse the jog - # rather than sending a wrong-side target the ESP would - # reject. The host knows position immediately whereas - # the ESP only learns g_pos via WPOS?. + # If the axis is already at-or-past the more-restrictive + # boundary (soft limit OR Z-A coupling) in the requested + # direction, refuse the jog rather than sending a + # wrong-side target the ESP would reject. if target_steps is not None and cur_steps is not None: at_limit = ((direction > 0 and cur_steps >= target_steps) or (direction < 0 and cur_steps <= target_steps)) if at_limit: self.log.info( - 'A-axis jog refused: at soft limit ' + 'A-axis jog refused: at %s limit ' '(cur=%d target=%d dir=%+d)', - cur_steps, target_steps, direction) + target_src, cur_steps, target_steps, direction) return # ignore_limits=True (safe=0) when the axis is unhomed: # pendant jog is allowed before homing for setup. When - # homed, soft limits are enforced via target_steps and - # the ESP's hardware-limit abort still applies - # unconditionally (movingTowardLimit in jogTask). + # homed, soft limits AND Z-A coupling are enforced via + # target_steps and the ESP's hardware-limit abort still + # applies unconditionally (movingTowardLimit in + # jogTask). ignore = not bool(aux._homed) aux.jog_start(direction, max_rate_sps=max_rate, accel_sps2=accel, ignore_limits=ignore, 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: self.log.warning('A-axis jog_start failed: %s', e)