Wire ExternalAxis to send LINE blocks (full S-curve mirror to ESP)
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:
@@ -356,6 +356,64 @@ class AuxAxis(object):
|
|||||||
raise AuxAxisError('W move aborted by limit switch')
|
raise AuxAxisError('W move aborted by limit switch')
|
||||||
raise AuxAxisError('W move aborted: %s' % line)
|
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
|
# ------------------------------------------------------------ serial I/O
|
||||||
|
|
||||||
def _open(self):
|
def _open(self):
|
||||||
|
|||||||
@@ -339,25 +339,13 @@ class ExternalAxis(object):
|
|||||||
self._busy.clear()
|
self._busy.clear()
|
||||||
|
|
||||||
def enqueue_target_mm(self, ext_mm):
|
def enqueue_target_mm(self, ext_mm):
|
||||||
"""Non-blocking variant: post a target to the worker queue.
|
"""Legacy non-blocking variant: post a fixed-rate STEPS move
|
||||||
Used by Planner.__encode in the hot path.
|
to the worker queue. No longer used by Planner.__encode (which
|
||||||
|
uses enqueue_line for full S-curve mirroring), but kept for
|
||||||
We deliberately do NOT mirror the new target into state.<axis>p
|
UI jog endpoints that don't have planner timing data.
|
||||||
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.
|
|
||||||
|
|
||||||
Soft limits are enforced here (defense in depth on top of
|
Soft limits are enforced here (defense in depth on top of
|
||||||
gplan). Raising stops the planner via Planner.next's
|
gplan)."""
|
||||||
exception handler."""
|
|
||||||
if not self.enabled:
|
if not self.enabled:
|
||||||
raise ExternalAxisError(
|
raise ExternalAxisError(
|
||||||
'External axis %r not available' % self.axis_letter)
|
'External axis %r not available' % self.axis_letter)
|
||||||
@@ -370,6 +358,48 @@ class ExternalAxis(object):
|
|||||||
return
|
return
|
||||||
self._work_q.put(('move', steps))
|
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):
|
def _compute_move(self, ext_mm):
|
||||||
"""Return (signed_steps, absolute_mm) for a target in mm.
|
"""Return (signed_steps, absolute_mm) for a target in mm.
|
||||||
Caches first-time position from the ESP."""
|
Caches first-time position from the ESP."""
|
||||||
@@ -397,6 +427,15 @@ class ExternalAxis(object):
|
|||||||
if kind == 'move':
|
if kind == 'move':
|
||||||
steps = op[1]
|
steps = op[1]
|
||||||
self.aux._do_steps(steps, ignore_limits=True)
|
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':
|
elif kind == 'home':
|
||||||
self.aux.home()
|
self.aux.home()
|
||||||
# _pos_mm and DRO updated by the caller's enqueue.
|
# _pos_mm and DRO updated by the caller's enqueue.
|
||||||
|
|||||||
@@ -404,11 +404,10 @@ class Planner():
|
|||||||
physically a no-op for the steppers, while keeping the
|
physically a no-op for the steppers, while keeping the
|
||||||
host-side state coherent.
|
host-side state coherent.
|
||||||
|
|
||||||
For mixed XYZ + external moves the AVR runs XYZ at the
|
We pass the full S-curve parameters to the ESP so its move
|
||||||
gplan-computed rate while the ESP runs the external delta
|
duration matches the AVR's exactly. The ESP runs the same
|
||||||
in parallel. Final positions agree; intermediate ap reports
|
7-segment jerk-limited trajectory the AVR would have run
|
||||||
from the AVR may briefly disagree with state.ap until the
|
if A had been a real motor."""
|
||||||
block completes."""
|
|
||||||
target = block.get('target') or {}
|
target = block.get('target') or {}
|
||||||
# Read the external target (case-insensitive) without modifying
|
# Read the external target (case-insensitive) without modifying
|
||||||
# the dict so the AVR still sees A.
|
# the dict so the AVR still sees A.
|
||||||
@@ -416,7 +415,14 @@ class Planner():
|
|||||||
if ext_mm is None:
|
if ext_mm is None:
|
||||||
ext_mm = target.get(ext.axis_letter.upper())
|
ext_mm = target.get(ext.axis_letter.upper())
|
||||||
try:
|
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:
|
except Exception as e:
|
||||||
self.log.error('External axis enqueue failed: %s' % e)
|
self.log.error('External axis enqueue failed: %s' % e)
|
||||||
raise
|
raise
|
||||||
|
|||||||
Reference in New Issue
Block a user