Files
a7-can-sdk/src/a7_can_sdk/startup.py
T

41 lines
1.6 KiB
Python

"""One-shot startup enable after feedback/identity readiness; no retry on failure."""
import time
class StartupEnable:
def __init__(self, runtime, requested=False, mode='monitor', timeout=5.):
if requested and mode not in ('fake', 'hardware'):
raise ValueError('Automatic enable requires fake or hardware mode')
self.runtime = runtime
self.deadline = time.monotonic() + timeout
self.state = 'waiting' if requested else 'off'
self.error = ''
def cancel(self):
if self.state == 'waiting':
self.state = 'cancelled'
def step(self):
"""Caller serializes this with operator commands. Returns current state."""
if self.state != 'waiting':
return self.state
try:
if self.runtime.fault or self.runtime.bus.fault:
raise RuntimeError(self.runtime.fault or self.runtime.bus.fault)
try:
self.runtime.snapshot()
ready = len(self.runtime.bus.diagnostics()['identities']) == len(self.runtime.motors)
except ValueError:
ready = False
if not ready:
if time.monotonic() >= self.deadline:
raise TimeoutError('Startup timed out waiting for all motor identities and fresh feedback')
return self.state
self.state = 'enabling'
self.runtime.enable()
self.state = 'enabled'
except Exception as exc:
self.error = str(exc)
self.state = 'failed'
return self.state