diff --git a/src/py/bbctrl/AuxAxis.py b/src/py/bbctrl/AuxAxis.py index 89dbcca..6ad693f 100644 --- a/src/py/bbctrl/AuxAxis.py +++ b/src/py/bbctrl/AuxAxis.py @@ -356,6 +356,64 @@ class AuxAxis(object): raise AuxAxisError('W move aborted by limit switch') raise AuxAxisError('W move aborted: %s' % line) + def _do_line(self, signed_steps, length_mm, + max_accel_mm_min2, max_jerk_mm_min3, + entry_vel_mm_min, exit_vel_mm_min, + times_min, ignore_limits=False, timeout=300.0): + """Run a 7-segment jerk-limited S-curve on the ESP that mirrors + gplan/buildbotics' planner output exactly. + + Parameters are in the same units the AVR/gplan use: + - length_mm: absolute travel in mm (>= 0) + - max_accel: mm/min^2 + - max_jerk: mm/min^3 + - entry/exit_vel: mm/min + - times_min: 7-tuple of section durations in minutes + + ignore_limits sets safe=0 on the ESP - used for jog/move + endpoints that may run before homing. + + Blocks until the ESP reports done or aborted. Updates the + position mirror and re-publishes state on every reply. + """ + if signed_steps == 0 or length_mm <= 0: + return + if not any(times_min): + raise AuxAxisError('LINE rejected: all section times are zero') + # Build the LINE command. Float formatting matches the AVR's + # printf precision (6 sig figs) - that's well above what the + # ESP needs given it integrates into a few thousand 4 ms + # segments per move. + parts = [ + 'LINE', + 'steps=%d' % int(signed_steps), + 'length=%.6f' % float(length_mm), + 'max_accel=%.6f' % float(max_accel_mm_min2), + 'max_jerk=%.6f' % float(max_jerk_mm_min3), + 'entry_vel=%.6f' % float(entry_vel_mm_min), + 'exit_vel=%.6f' % float(exit_vel_mm_min), + ] + for i, t in enumerate(times_min): + if t and t > 0: + parts.append('t%d=%.9f' % (i, float(t))) + if ignore_limits: + parts.append('safe=0') + cmd = ' '.join(parts) + line = self._rpc(cmd, topic='line', timeout=timeout) + # line: "done pos=P emitted=N" or "aborted pos=P emitted=N reason=..." + if line.startswith('done'): + self._pos_steps = self._parse_kv_int(line, 'pos', self._pos_steps) + self._publish_state() + return + # aborted + self._pos_steps = self._parse_kv_int(line, 'pos', self._pos_steps) + self._publish_state() + reason = self._parse_kv_str(line, 'reason') + if reason == 'limit': + self._homed = False + raise AuxAxisError('W move aborted by limit switch') + raise AuxAxisError('W move aborted: %s' % line) + # ------------------------------------------------------------ serial I/O def _open(self): diff --git a/src/py/bbctrl/ExternalAxis.py b/src/py/bbctrl/ExternalAxis.py index a963e16..4cca7b9 100644 --- a/src/py/bbctrl/ExternalAxis.py +++ b/src/py/bbctrl/ExternalAxis.py @@ -339,25 +339,13 @@ class ExternalAxis(object): self._busy.clear() def enqueue_target_mm(self, ext_mm): - """Non-blocking variant: post a target to the worker queue. - Used by Planner.__encode in the hot path. - - We deliberately do NOT mirror the new target into state.p - here. The AVR also receives the line block (we don't strip - the external axis from the AVR target) and its trapezoid - progressively updates ex.position[A] -> reports back as ap. - If we set state.ap here ahead of the AVR, the DRO would jump - to the target and then snap back to intermediate values as - AVR reports stream in. Instead we let the AVR drive state.ap. - We still mirror the new abs into our internal _pos_mm so - the next line block computes the correct delta. - - The Planner.__encode hook calls this so the AVR comm thread - is never blocked by serial RPCs to the ESP. + """Legacy non-blocking variant: post a fixed-rate STEPS move + to the worker queue. No longer used by Planner.__encode (which + uses enqueue_line for full S-curve mirroring), but kept for + UI jog endpoints that don't have planner timing data. Soft limits are enforced here (defense in depth on top of - gplan). Raising stops the planner via Planner.next's - exception handler.""" + gplan).""" if not self.enabled: raise ExternalAxisError( 'External axis %r not available' % self.axis_letter) @@ -370,6 +358,48 @@ class ExternalAxis(object): return self._work_q.put(('move', steps)) + def enqueue_line(self, ext_mm, max_accel_mm_min2, max_jerk_mm_min3, + entry_vel_mm_min, exit_vel_mm_min, times_ms): + """Post a full S-curve LINE block to the ESP worker. Mirrors + gplan's planned trajectory exactly (same 7-segment math, same + unit system) so the ESP's move duration matches what the AVR + would have produced for an A motor. + + Called by Planner.__encode for every line block that touches + the external axis. + + Parameters: + ext_mm: absolute target in mm (gplan target['a']) + max_accel_mm_min2:from block['max-accel'] + max_jerk_mm_min3: from block['max-jerk'] + entry_vel_mm_min: from block['entry-vel'] (typically 0 for + the first block, exit_vel of the prior + block otherwise) + exit_vel_mm_min: from block['exit-vel'] + times_ms: 7-tuple of section durations in ms + (block['times'] - the same units gplan uses) + """ + if not self.enabled: + raise ExternalAxisError( + 'External axis %r not available' % self.axis_letter) + self._check_soft_limit(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)) + # Update internal mirror; AVR drives state.p. + self._pos_mm = abs_mm + if steps == 0 or delta_mm <= 0: + return + # ms -> minutes (the unit gplan/AVR/ESP use internally for + # SCurve math). + times_min = tuple((t / 60000.0) if t else 0.0 for t in times_ms) + self._work_q.put(('line', steps, delta_mm, + float(max_accel_mm_min2), + float(max_jerk_mm_min3), + float(entry_vel_mm_min), + float(exit_vel_mm_min), + times_min)) + def _compute_move(self, ext_mm): """Return (signed_steps, absolute_mm) for a target in mm. Caches first-time position from the ESP.""" @@ -397,6 +427,15 @@ class ExternalAxis(object): if kind == 'move': steps = op[1] self.aux._do_steps(steps, ignore_limits=True) + elif kind == 'line': + (_, steps, length_mm, + max_accel, max_jerk, + entry_vel, exit_vel, + times_min) = op + self.aux._do_line( + steps, length_mm, max_accel, max_jerk, + entry_vel, exit_vel, times_min, + ignore_limits=True) elif kind == 'home': self.aux.home() # _pos_mm and DRO updated by the caller's enqueue. diff --git a/src/py/bbctrl/Planner.py b/src/py/bbctrl/Planner.py index 85b8367..c2af59e 100644 --- a/src/py/bbctrl/Planner.py +++ b/src/py/bbctrl/Planner.py @@ -404,11 +404,10 @@ class Planner(): physically a no-op for the steppers, while keeping the host-side state coherent. - For mixed XYZ + external moves the AVR runs XYZ at the - gplan-computed rate while the ESP runs the external delta - in parallel. Final positions agree; intermediate ap reports - from the AVR may briefly disagree with state.ap until the - block completes.""" + We pass the full S-curve parameters to the ESP so its move + duration matches the AVR's exactly. The ESP runs the same + 7-segment jerk-limited trajectory the AVR would have run + if A had been a real motor.""" target = block.get('target') or {} # Read the external target (case-insensitive) without modifying # the dict so the AVR still sees A. @@ -416,7 +415,14 @@ class Planner(): if ext_mm is None: ext_mm = target.get(ext.axis_letter.upper()) try: - ext.enqueue_target_mm(ext_mm) + ext.enqueue_line( + ext_mm, + block.get('max-accel', 0.0), + block.get('max-jerk', 0.0), + block.get('entry-vel', 0.0), + block.get('exit-vel', 0.0), + block.get('times', [0]*7), + ) except Exception as e: self.log.error('External axis enqueue failed: %s' % e) raise