muehe/onefinity-firmware
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
da619bd56c672354833b7a3c6bbe88c7c0c90db8
onefinity-firmware/src/py/bbctrl/Mach.py
Henrik Muehe da619bd56c ATC: M100..M103 preprocessor + Mach MDI rewrite + hook handlers 
ATC pneumatics in g-code (drop tool / grab tool / release clamp /
engage clamp) are expressed as M100..M103. AuxPreprocessor rewrites
those into (MSG,HOOK:droptool:) etc on file upload + on planner
load + on MDI input, so the Hooks layer (B1) can dispatch them via
registered ATC handlers in Ctrl.

- AuxPreprocessor.py: regex-based file rewriter, idempotent.
- FileHandler: invoke preprocessor on every upload.
- Planner.init: also re-preprocess on load (catches files written
  before this version).
- Mach.mdi: same rewrite for ad-hoc MDI input so M101 typed at the
  console produces a HOOK message.
- Ctrl: register the four ATC hooks (droptool/grabtool/release/clamp)
  with block_unpause + auto_resume so programs using them pause at
  the right point and resume cleanly. aux_home retained as a legacy
  alias for older preprocessed files.
2026-05-03 15:14:25 +02:00

629 lines
24 KiB
Python
Raw Blame History

################################################################################
# #
# This file is part of the Buildbotics firmware. #
# #
# Copyright (c) 2015 - 2018, Buildbotics LLC #
# All rights reserved. #
# #
# This file ("the software") is free software: you can redistribute it #
# and/or modify it under the terms of the GNU General Public License, #
# version 2 as published by the Free Software Foundation. You should #
# have received a copy of the GNU General Public License, version 2 #
# along with the software. If not, see <http://www.gnu.org/licenses/>. #
# #
# The software is distributed in the hope that it will be useful, but #
# WITHOUT ANY WARRANTY; without even the implied warranty of #
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU #
# Lesser General Public License for more details. #
# #
# You should have received a copy of the GNU Lesser General Public #
# License along with the software. If not, see #
# <http://www.gnu.org/licenses/>. #
# #
# For information regarding this software email: #
# "Joseph Coffland" <joseph@buildbotics.com> #
# #
################################################################################
import bbctrl
from bbctrl.Comm import Comm
import bbctrl.Cmd as Cmd
# Axis homing procedure:
#
# Mark axis unhomed
# Set feed rate to search_vel
# Seek closed by search_dist
# Set feed rate to latch_vel
# Seek open latch_backoff
# Seek closed latch_backoff * -1.5
# Rapid to zero_backoff
# Mark axis homed and set absolute position
axis_homing_procedure = '''
G28.2 %(axis)s0 F[#<_%(axis)s_search_velocity>]
G38.6 %(axis)s[#<_%(axis)s_home_travel>]
G38.8 %(axis)s[#<_%(axis)s_latch_backoff>] F[#<_%(axis)s_latch_velocity>]
G38.6 %(axis)s[#<_%(axis)s_latch_backoff> * -8]
G91 G0 G53 %(axis)s[#<_%(axis)s_zero_backoff>]
G90 G28.3 %(axis)s[#<_%(axis)s_home_position>]
'''
# The stepper drivers have a stall flag that is reset
# by moving the motors without a stall condition.
# The "wiggle" in the stall procedure is to clear the flag.
# This was to correct the issue where the stepper motors
# may already be in a stall condition when homing is started.
# For example, if a user tried to home twice in a row
# the second homing attempt would immediately think it
# was stalled if we didn't first back it up a bit
stall_homing_procedure = '''
G91 G1 %(axis)s [#<_%(axis)s_zero_backoff>] F[#<_%(axis)s_search_velocity>]
G4 P0.25
G28.2 %(axis)s0 F[#<_%(axis)s_search_velocity>]
G38.6 %(axis)s[#<_%(axis)s_home_travel>]
G91 G1 G53 %(axis)s[#<_%(axis)s_zero_backoff>] F100
G90 G28.3 %(axis)s[#<_%(axis)s_home_position>]
'''
motor_fault_error = '''\
Motor %d driver fault. A potentially damaging electrical condition was \
detected and the motor driver was shutdown. Please power down the controller \
and check your motor cabling. See the "Motor Faults" table on the "Indicators" \
for more information.\
'''
def overrides(interface_class):
def overrider(method):
if not method.__name__ in dir(interface_class):
raise Exception('%s does not override %s' % (
method.__name__, interface_class.__name__))
return method
return overrider
class Mach(Comm):
def __init__(self, ctrl, avr):
super().__init__(ctrl, avr)
self.ctrl = ctrl
self.mlog = self.ctrl.log.get('Mach')
self.planner = bbctrl.Planner(ctrl)
self.unpausing = False
self.stopping = False
# Guard against overlapping deferred-external-homing threads
# if the user clicks Home (All) again while the previous run
# is still waiting for the AVR cycle to finish.
self._ext_home_thread = None
ctrl.state.set('cycle', 'idle')
ctrl.state.add_listener(self._update)
super().reboot()
def _get_state(self): return self.ctrl.state.get('xx', '')
def _is_estopped(self): return self._get_state() == 'ESTOPPED'
def _is_holding(self): return self._get_state() == 'HOLDING'
def _is_ready(self): return self._get_state() == 'READY'
def _get_pause_reason(self): return self.ctrl.state.get('pr', '')
def _get_cycle(self): return self.ctrl.state.get('cycle', 'idle')
def _is_paused(self):
if not self._is_holding() or self.unpausing: return False
return self._get_pause_reason() in (
'User pause', 'Program pause', 'Optional pause')
def _set_cycle(self, cycle): self.ctrl.state.set('cycle', cycle)
def _begin_cycle(self, cycle):
current = self._get_cycle()
if current == cycle: return # No change
if current != 'idle':
raise Exception('Cannot enter %s cycle while in %s cycle' %
(cycle, current))
# TODO handle jogging during pause
# if current == 'idle' or (cycle == 'jogging' and self._is_paused()):
self._set_cycle(cycle)
def process_log(self, log):
# When a probe has failed, we have to e-stop or things
# end up in a bad state, where positions and offsets are incorrect
if log['msg'] == 'Switch not found':
self.estop()
def _end_cycle(self):
if (self._get_cycle() != 'idle' and self._is_ready()
and not self.planner.is_busy() and not super().is_active()):
self.planner.position_change()
self._set_cycle('idle')
def _update(self, update):
# Detect motor faults
for motor in range(4):
key = '%ddf' % motor
if key in update and update[key] & 0x1f:
self.mlog.error(motor_fault_error % motor)
# Get state
state_changed = 'xc' in update
state = self._get_state()
# Handle EStop
if state_changed and state == 'ESTOPPED':
self.planner.reset(stop = False)
# Exit cycle if state changed to READY
if (state_changed and self._get_cycle() != 'idle' and
self._is_ready() and not self.planner.is_busy() and
not super().is_active()):
self.planner.position_change()
self._set_cycle('idle')
# Planner stop
if state == 'READY' and self.stopping:
self.planner.stop()
self.ctrl.state.set('line', 0)
self.stopping = False
# Unpause sync
if state_changed and state != 'HOLDING':
self.unpausing = False
# Entering HOLDING state
if state_changed and state == 'HOLDING':
# Always flush queue after pause
super().i2c_command(Cmd.FLUSH)
super().resume()
# Automatically unpause after seek or stop hold
# Must be after holding commands above
op = self.ctrl.state.get('optional_pause', False)
pr = self._get_pause_reason()
if ((state_changed or 'pr' in update) and self._is_holding() and
(pr in ('Switch found', 'User stop') or
(pr == 'Optional pause' and not op))):
self._unpause()
def _unpause(self):
pause_reason = self._get_pause_reason()
self.mlog.info('Unpause: ' + pause_reason)
if pause_reason == 'User stop':
self.planner.stop()
self.ctrl.state.set('line', 0)
else: self.planner.restart()
super().i2c_command(Cmd.UNPAUSE)
self.unpausing = True
def _i2c_block(self, block):
super().i2c_command(block[0], block = block[1:])
def _i2c_set(self, name, value): self._i2c_block(Cmd.set(name, value))
@overrides(Comm)
def comm_next(self):
if self.planner.is_running() and not self._is_holding():
return self.planner.next()
@overrides(Comm)
def comm_error(self):
self.planner.reset()
@overrides(Comm)
def connect(self):
self.planner.reset()
super().connect()
def _query_var(self, cmd):
equal = cmd.find('=')
if equal == -1:
self.mlog.info('%s=%s' % (cmd, self.ctrl.state.get(cmd[1:])))
else:
name, value = cmd[1:equal], cmd[equal + 1:]
if value.lower() == 'true': value = True
elif value.lower() == 'false': value = False
else:
try:
value = float(value)
except: pass
self.ctrl.state.config(name, value)
def mdi(self, cmd, with_limits = True):
try:
if not len(cmd): return
if cmd[0] == '$': self._query_var(cmd)
elif cmd[0] == '\\': super().queue_command(cmd[1:])
else:
# Rewrite ATC M-codes in MDI input the same way the
# FileHandler rewrites uploaded files. Motion (X/Y/Z/A)
# is left unchanged: the planner handles it natively
# now that the auxcnc stepper is exposed as a virtual
# A axis (see ExternalAxis).
cmd = self._rewrite_aux_mdi(cmd)
self._begin_cycle('mdi')
self.planner.mdi(cmd, with_limits)
super().resume()
except BaseException as err:
self.mlog.info("Exception during MDI: %s" % err)
pass
def _rewrite_aux_mdi(self, cmd):
"""Apply the ATC M-code preprocessor to a single MDI line.
Returns possibly-multi-line G-code with HOOK: comments inserted."""
try:
from bbctrl.AuxPreprocessor import AuxPreprocessor, _ATC_M_RE
if not _ATC_M_RE.search(cmd):
return cmd
import io, tempfile, os
# AuxPreprocessor.process is file-based; route through
# tempfiles so we don't fork the regex/state logic.
pre = AuxPreprocessor(log=self.mlog)
with tempfile.NamedTemporaryFile('w', suffix='.nc',
delete=False) as fi:
fi.write(cmd if cmd.endswith('\n') else cmd + '\n')
ipath = fi.name
opath = ipath + '.out'
try:
pre.process(ipath, opath)
rewritten = open(opath).read()
finally:
try: os.unlink(ipath)
except OSError: pass
try: os.unlink(opath)
except OSError: pass
return rewritten
except Exception as e:
self.mlog.warning('Aux MDI rewrite failed: %s' % e)
return cmd
def set(self, code, value):
super().queue_command('${}={}'.format(code, value))
def jog(self, axes):
# Strip the external axis from the jog request before sending
# to the AVR. v1 doesn't support continuous-rate jogging on
# the ESP-driven axis - users jog A via /api/aux/jog (relative
# mm steps) instead. Sending A to the AVR is harmless (no
# motor maps to it) but cleaner to strip.
ext = getattr(self.ctrl, 'ext_axis', None)
if ext is not None and isinstance(axes, dict):
axes = {k: v for k, v in axes.items()
if k.lower() != ext.axis_letter}
if not axes:
return
self._begin_cycle('jogging')
self.planner.position_change()
super().queue_command(Cmd.jog(axes))
def home(self, axis, position = None):
state = self.ctrl.state
if axis is None:
is_rotary_active = state.get('2an', None) == 3
axes = 'zxybc' if is_rotary_active else 'zxyabc' # TODO This should be configurable
else: axes = '%c' % axis
# Collect external axes here and process them *after* every
# AVR axis above has finished its homing cycle. Without this,
# the AVR is still running Z/X/Y homing G-code in the
# planner queue while ext.home() synchronously drives the ESP
# to home A in parallel - which is unsafe (the gantry and W
# axis can move at the same time) and visually confusing.
# We defer external homing to a background thread that
# polls cycle until the AVR cycle completes.
external_pending = []
for axis in axes:
enabled = state.is_axis_enabled(axis)
mode = state.axis_homing_mode(axis)
# External axes (e.g. the auxcnc-driven A axis) home via
# their own ESP-side homing routine; the standard
# G28.2 / G38.6 / latch sequence doesn't apply.
#
# After homing we want a deterministic outcome regardless
# of where the user was before:
# physical position = home_position_mm (e.g. 134 mm)
# work-coord origin = home position (user A = 0)
# work offset = home_position_mm (so abs - off = 0)
#
# ext.home() blocks on the ESP and updates state.ap to
# home_position_mm. We then need to tell the AVR (so its
# ex.position[A] matches physical reality) and gplan
# (so trajectory planning sees abs at home).
#
# We deliberately avoid G28.3 here: gplan's G28.3 keeps the
# current user-coord position fixed and adjusts the offset
# to match the new abs, which means re-homing after a move
# accumulates offset (134 -> 268 -> ...). Using G92 a0
# *after* syncing abs gives the desired "user A = 0 here"
# outcome with offset = home_position every time.
ext = getattr(self.ctrl, 'ext_axis', None)
if ext is not None and ext.enabled \
and ext.axis_letter == axis.lower():
if 1 < len(axes) and not enabled:
continue
# Defer until AVR axes are done. We capture the axis
# letter and ext reference; the actual homing runs
# in _run_external_homing below.
external_pending.append((axis, ext))
continue
# If this is not a request to home a specific axis and the
# axis is disabled or in manual homing mode, don't show any
# warnings
if 1 < len(axes) and (not enabled or mode == 'manual'):
continue
# Error when axes cannot be homed
reason = state.axis_home_fail_reason(axis)
if reason is not None:
self.mlog.error('Cannot home %s axis: %s' % (
axis.upper(), reason))
continue
if mode == 'manual':
if position is None: raise Exception('Position not set')
self.mdi('G28.3 %c%f' % (axis, position))
continue
# Home axis
self.mlog.info('Homing %s axis' % axis)
self._begin_cycle('homing')
if mode.startswith('stall-'): procedure = stall_homing_procedure
else: procedure = axis_homing_procedure
gcode = procedure % {'axis': axis}
self.planner.mdi(gcode, False)
super().resume()
# Kick off the deferred external-axis homing on a background
# thread so we don't block the HTTP handler (which is on the
# IOLoop) waiting for the AVR cycle to finish.
if external_pending:
prev = self._ext_home_thread
if prev is not None and prev.is_alive():
self.mlog.info(
'External homing already in progress; ignoring '
'duplicate request')
else:
import threading
t = threading.Thread(
target=self._run_external_homing,
args=(list(external_pending),),
name='ext-home-deferred',
daemon=True)
self._ext_home_thread = t
t.start()
def _run_external_homing(self, pending):
"""Background worker: wait for the AVR cycle to drop to idle
(meaning all queued AVR-side homing is done), then run each
deferred external-axis home in order.
We split the work between two threads:
- this background thread blocks on the ESP serial RPC
(ext.home(), which can take 5-10 seconds while the
carriage seeks the limit and backs off twice);
- small bookkeeping operations that touch gplan, the AVR
command queue, or shared State are scheduled back onto
the IOLoop via ctrl.ioloop.add_callback() so we don't
race with the rest of the controller.
"""
import time
# Wait up to 5 minutes for the AVR cycle to leave 'homing'.
# Long enough for any reasonable Onefinity full-travel home
# (Y axis at slow rate covers ~800 mm).
deadline = time.time() + 300.0
while time.time() < deadline:
cycle = self._get_cycle()
# 'homing' is the AVR's homing cycle; we wait for it to
# return to idle. If the user estopped or the cycle was
# aborted, cycle goes to idle too - we still proceed and
# the external home will fail-soft if conditions are wrong.
if cycle == 'idle':
break
time.sleep(0.1)
else:
self.mlog.error(
'External axis homing aborted: AVR cycle did not '
'return to idle within timeout')
return
for axis, ext in pending:
self.mlog.info('Homing external %s axis via auxcnc' %
axis.upper())
# Begin the cycle on the IOLoop so cycle-state writes go
# through the same thread that all other state writes do.
self.ctrl.ioloop.add_callback(self._begin_cycle, 'homing')
try:
# ext.home() runs on this background thread - it
# blocks on serial I/O and is fully thread-safe (the
# AuxAxis driver has its own RPC lock).
ext.home()
home_mm = ext.home_position_mm
# All of the post-home bookkeeping touches gplan and
# the AVR command queue, both of which run on the
# IOLoop. Schedule it there in a single callback so
# the steps run in order without intervening events.
self.ctrl.ioloop.add_callback(
self._finish_external_home, axis, home_mm)
except Exception as e:
self.mlog.error(
'External axis homing failed: %s' % e)
# Cycle reset must also happen on the IOLoop. Without
# this the UI stays locked at 'homing' since the AVR
# never moved (no state change to drive _update's
# cycle-end path).
self.ctrl.ioloop.add_callback(
self._abort_external_home_cycle)
def _finish_external_home(self, axis, home_mm):
"""IOLoop-side completion of an external axis home.
Synchronizes AVR position, refreshes the planner, and emits
a G92 to set the user-coord origin at the home position.
"""
try:
# 1) Update AVR: no motor steps, just position sync.
super().queue_command(Cmd.set_axis(axis, home_mm))
# 2) Force planner to resync abs from State on the next
# planner call (which is the MDI below).
self.planner.position_change()
# 3) G92 <axis>0: with abs already at home_mm, sets
# user-coord A = 0 and offset = home_mm. Use
# planner.mdi (not Mach.mdi) so we don't flip cycle
# to 'mdi' inside the 'homing' cycle.
self.planner.mdi('G92 %c0' % axis, False)
super().resume()
except Exception:
self.mlog.exception(
'Post-home bookkeeping failed for external axis')
self._abort_external_home_cycle()
def _abort_external_home_cycle(self):
"""Reset cycle to idle from the IOLoop after a failed
external axis home. The AVR never moved so _update's normal
cycle-end path won't fire; do it explicitly here.
"""
if self._get_cycle() == 'homing':
try:
self._set_cycle('idle')
except Exception:
self.mlog.exception(
'Failed to reset cycle to idle after external '
'homing error')
def unhome(self, axis):
# External axes don't have AVR-side homed state to clear; the
# ESP holds its own homed flag. We don't have an explicit
# "unhome" verb on the ESP, but a stale homed flag is harmless
# because the next absolute move will fail-soft via
# ExternalAxis._pos_mm sync. Still mirror the cleared flag
# into State for the UI.
ext = getattr(self.ctrl, 'ext_axis', None)
if ext is not None and ext.enabled \
and chr(axis).lower() == ext.axis_letter:
from bbctrl.ExternalAxis import EXTERNAL_MOTOR_INDEX
self.ctrl.state.set('%dh' % EXTERNAL_MOTOR_INDEX, 0)
self.ctrl.state.set(ext.axis_letter + '_homed', False)
return
self.mdi('G28.2 %c0' % axis)
def estop(self): super().estop()
def clear(self):
if self._is_estopped():
self.planner.reset()
super().clear()
def start(self):
filename = self.ctrl.state.get('selected', '')
if not filename: return
self._begin_cycle('running')
self.planner.load(filename)
super().resume()
def step(self):
raise Exception('NYI') # TODO
if self._get_cycle() != 'running': self.start()
else: super().i2c_command(Cmd.UNPAUSE)
def stop(self):
if self._get_state() != 'jogging': self.stopping = True
super().i2c_command(Cmd.STOP)
# Drain the external-axis worker queue so post-stop resumption
# doesn't replay queued moves that the user wanted cancelled.
ext = getattr(self.ctrl, 'ext_axis', None)
if ext is not None:
try: ext.abort()
except Exception: pass
def pause(self): super().pause()
def unpause(self):
if self._is_paused():
# Gate unpause on hook completion
if hasattr(self.ctrl, 'hooks') and \
not self.ctrl.hooks.can_unpause():
return
self.ctrl.state.set('optional_pause', False)
self._unpause()
def optional_pause(self, enable = True):
self.ctrl.state.set('optional_pause', enable)
def set_position(self, axis, position, set_config = None):
axis = axis.lower()
state = self.ctrl.state
if state.is_axis_homed(axis):
# If homed, change the offset rather than the absolute position
self.mdi('G92%s%f' % (axis, position))
#storing the offset to config
if set_config is not None:
self.ctrl.config.set('axes', {'offset_' + axis : state.get(axis + 'p')})
self.log.info('set_position: {} = {} '.format(axis, state.get(axis + 'p')))
elif state.is_axis_enabled(axis):
if self._get_cycle() != 'idle' and not self._is_paused():
raise Exception('Cannot set position during ' +
self._get_cycle())
# Set the absolute position both locally and via the AVR
target = position + state.get('offset_' + axis)
state.set(axis + 'p', target)
super().queue_command(Cmd.set_axis(axis, target))
def override_feed(self, override):
self._i2c_set('fo', int(1000 * override))
def override_speed(self, override):
self._i2c_set('so', int(1000 * override))
def modbus_read(self, addr): self._i2c_block(Cmd.modbus_read(addr))
def modbus_write(self, addr, value):
self._i2c_block(Cmd.modbus_write(addr, value))
Reference in New Issue View Git Blame Copy Permalink