muehe/onefinity-firmware
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Wire ExternalAxis to send LINE blocks (full S-curve mirror to ESP)

Browse Source 
Replaces the legacy 'fixed-rate STEPS' path used in Planner.__encode
with a new ExternalAxis.enqueue_line() that hands the ESP the full
7-segment S-curve parameters of every gplan line block (max_accel,
max_jerk, entry_vel, exit_vel, times[7]).

The ESP's new LINE command (auxcnc commit 8acc6f7) integrates the
same SCurve math the AVR uses, so the W axis now physically moves
in lockstep with whatever the planner thinks A is doing. Result:
DRO stays in sync with the actual stepper, no more multi-second lag
between commanded and observed A position.

enqueue_target_mm is kept as a no-frills STEPS path for jog/move UI
endpoints that don't have planner timing context.

AuxAxis._do_line builds the LINE command with mm/min/min^2/min^3
units (matching gplan's internal unit system) and waits for
[line] done|aborted from the ESP. Limit aborts still flag _homed=False.
This commit is contained in:
Henrik Muehe
2026-05-03 12:29:43 +02:00
parent 53b65dc30e
commit 983e06b53d
3 changed files with 126 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files

58
src/py/bbctrl/AuxAxis.py
View File

@@ -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):

73
src/py/bbctrl/ExternalAxis.py
View File

@@ -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.<axis>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.<axis>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.

18
src/py/bbctrl/Planner.py
View File

@@ -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