tka/scanengine-3
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fixed app to use new thorlabs driver

Browse Source
This commit is contained in:
Thomas Ales [M S E]
2026-02-09 16:28:47 -06:00
parent 23f6331ba2
commit 57613b7aca
12 changed files with 1474 additions and 3333 deletions
Download Patch File Download Diff File Expand all files Collapse all files
hardware/pybbd202/__init__.py
+9
View File
@@ -0,0 +1,9 @@
"""pybbd202 - Thorlabs BBD202 servo stage driver (pyserial-based)"""
from .bbd20x import ThorlabsServoDriver
from .apt_constants import TriggerBitsServo, StatusBits
# Axis address constants
AXIS_X = 0x21
AXIS_Y = 0x22
CONTROLLER = 0x11
hardware/pybbd202/apt_constants.py
+59
View File
@@ -0,0 +1,59 @@
'''
APT Constants and Registries
Thomas Ales | Feb 2026
'''
from enum import IntFlag
class StatusBits(IntFlag):
MOT_SB_CWHARDLIMIT = 0x01
MOT_SB_CCWHARDLIMIT = 0x02
MOT_SB_INMOTIONCW = 0x10
MOT_SB_INMOTIONCCW = 0x20
MOT_SB_HOMING = 0x200
MOT_SB_HOMED = 0x400
MOT_SB_TRACKING = 0x1000
MOT_SB_SETTLED = 0x2000
MOT_SB_POSITIONERROR = 0x4000
MOT_SB_INSTRERROR = 0x8000
MOT_SB_INTERLOCK = 0x10000
MOT_SB_OVERTEMP = 0x20000
MOT_SB_BUSVOLTFAULT = 0x40000
MOT_SB_COMMUTATIONERROR = 0x80000
MOT_SB_DIGIP1 = 0x100000
MOT_SB_OVERLOAD = 0x1000000
MOT_SB_POWEROK = 0x10000000
MOT_SB_ERROR = 0x40000000
MOT_SB_ENABLED = 0x80000000
# combined masks for checking various
# states
MOT_ANY_MOVE = MOT_SB_INMOTIONCW | MOT_SB_INMOTIONCCW
MOT_ANY_ERR = (MOT_SB_OVERTEMP | MOT_SB_BUSVOLTFAULT |
MOT_SB_COMMUTATIONERROR | MOT_SB_OVERLOAD |
MOT_SB_ERROR | MOT_SB_INSTRERROR)
class TriggerBitsStepper(IntFlag):
TRIGIN_ENABLE = 0x01,
TRIGOUT_ENABLE = 0x02,
TRIGOUT_MODEFOLLOW = 0x04,
TRIGOUT_MODEMOVEEND = 0x08,
TRIG_RELMOVE = 0x10,
TRIG_ABSMOVE = 0x20,
TRIG_HOMEMOVE = 0x40,
TRIGOUT_NOTRIGIN = 0x80
class TriggerBitsServo(IntFlag):
TRIGIN_HIGH = 0x01
TRIGIN_RELMOVE = 0x02
TRIGIN_ABSMOVE = 0x04
TRIGIN_HOMEMOVE = 0x08
TRIGOUT_HIGH = 0x10
TRIGOUT_INMOTION = 0x20
TRIGOUT_MOTIONCOMPLETE = 0x40
TRIGOUT_MAXVELOCITY = 0x80
TRIGOUT_MAXV = TRIGOUT_HIGH | TRIGOUT_MAXVELOCITY
hardware/pybbd202/apt_messages.py
+318
View File
@@ -0,0 +1,318 @@
'''
ThorLABS APT Protocol Message Registry
Thomas Ales | Feb 2026
Version 1
'''
import struct
from .apt_constants import StatusBits as sb
class APTProtocol():
ADDRESSES = { 'HOST_PC': 0x01, 'CONTROLLER': 0x11,
'X_AXIS': 0x21, 'Y_AXIS': 0x22
}
# N.B.: If the format is anything other than the following
# two, it is considered a 'long' message. In this case
# the format key refers only to the payload part of the
# message.
# BB - Short Message, Only Source/Destination
# BBBB - Short Message, Using Parameters 1 & 2
# Dictionary entries MUST BE in the order they
# appear in the thorlabs documentation, if you don't
# the unpacking logic goes all to 💩 and 'fun' things
# happen.
MSGS = {
0x0002: {
'name': 'MGMSG_HW_DISCONNECT',
'format': 'BB',
'response': None,
'fields': ['destination', 'source'],
'data_fields': None
},
0x0005: {
'name': 'MGMSG_HW_REQ_INFO',
'format': 'BB',
'response': 0x0006,
'fields': ['destination', 'source'],
'data_fields': None
},
0x0006: {
'name': 'MGMSG_HW_GET_INFO',
'format': '<I8sH3B61xHHH',
'response': None,
'fields': ['destination', 'source'],
'data_fields': ['serial', 'model', 'type',
'fw_minor', 'fw_interim', 'fw_major',
'hw_ver', 'mod_state', 'num_channels']
},
0x0011: {
'name': 'MGMSG_HW_START_UPDATEMSGS',
'format': 'BB',
'response': None,
'fields': ['destination', 'source'],
'data_fields': None
},
0x0012: {
'name': 'MGMSG_HW_STOP_UPDATEMSGS',
'format': 'BB',
'response': None,
'fields': ['destination', 'source'],
'data_fields': None
},
0x0060: {
'name': 'MGMSG_RACK_REQ_BAYUSED',
'format': 'BBBB',
'response': 0x0061,
'fields': ['bay_id', 'destination', 'source'],
'data_fields': None
},
0x0061: {
'name': 'MGMSG_RACK_GET_BAYUSED',
'format': 'BBBB',
'response': None,
'fields': ['bay_id', 'bay_state', 'destination', 'source'],
'data_fields': None
},
0x0210: {
'name': 'MGMSG_MOD_SET_CHANENABLESTATE',
'format': 'BBBB',
'response': None,
'fields': ['chan_ident', 'enable_state', 'destination', 'source'],
'data_fields': None
},
0x0211: {
'name': 'MGMSG_MOD_REQ_CHANENABLESTATE',
'format': 'BB',
'response': 0x0212,
'fields': ['destination', 'source'],
'data_fields': None
},
0x0212: {
'name': 'MGMSG_MOD_GET_CHANENABLESTATE',
'format': 'BBBB',
'response': None,
'fields': ['chan_ident', 'enable_state', 'destination', 'source'],
'data_fields': None
},
0x0413: {
'name': 'MGMSG_MOT_SET_VELPARAMS',
'format': '<Hlll',
'response': None,
'fields': ['destination', 'source'],
'data_fields': ['chan_ident', 'min_velocity', 'acceleration',
'max_velocity']
},
0x0414: {
'name': 'MGMSG_MOT_REQ_VELPARAMS',
'format': 'BBBB',
'response': 0x0415,
'fields': ['chan_ident', 'zero_this', 'destination', 'source'],
'data_fields': None
},
0x0415: {
'name': 'MGMSG_MOT_GET_VELPARAMS',
'format': '<Hlll',
'response': None,
'fields': ['destination', 'source'],
'data_fields': ['chan_ident', 'min_velocity', 'acceleration',
'max_velocity']
},
0x0443: {
'name': 'MGMSG_MOT_MOVE_HOME',
'format': 'BBBB',
'response': 0x0444,
'fields': ['chan_ident', 'destination', 'source'],
'data_fields': None
},
0x0444: {
'name': 'MGMSG_MOT_MOVE_HOMED',
'format': 'BBBB',
'response': None,
'fields': ['chan_ident', 'destination', 'source'],
'data_fields': None
},
0x0448: {
'name': 'MGMSG_MOT_MOVE_RELATIVE',
'format': '<Hl',
'response': 0x0464,
'fields': ['destination', 'source'],
'data_fields': ['chan_ident', 'relative_distance']
},
0x0453: {
'name': 'MGMSG_MOT_MOVE_ABSOLUTE',
'format': '<Hl',
'response': 0x0464,
'fields': ['destination', 'source'],
'data_fields': ['chan_ident', 'absolute_distance']
},
0x0464: {
'name': 'MGMSG_MOT_MOVE_COMPLETED',
'format': '<Hl2HI',
'response': None,
'fields': ['destination', 'source'],
'data_fields': ['chan_ident', 'position', 'velocity',
'motor_current', 'status_bits']
},
0x0490: {
'name': 'MGMSG_MOT_REQ_USTATUSUPDATE',
'format': 'BB',
'response': None,
'fields': ['destination', 'source'],
'data_fields': None
},
0x0491: {
'name': 'MGMSG_MOT_GET_USTATUSUPDATE',
'format': '<Hl2HI',
'response': 0x0492,
'fields': ['destination', 'source'],
'data_fields': ['chan_ident', 'position',
'velocity', 'motor_current', 'status_bits']
},
0x0492: {
'name': 'MGMSG_MOT_ACK_USTATUSUPDATE',
'format': 'BB',
'response': None,
'fields': ['destination', 'source'],
'data_fields': None
},
0x0500: {
'name': 'MGMSG_MOT_SET_TRIGGER',
'format': 'BBBB',
'response': None,
'fields': ['chan_ident', 'mode', 'destination', 'source'],
'data_fields': None
},
0x0501: {
'name': 'MGMSG_MOT_REQ_TRIGGER',
'format': 'BBBB',
'response': 0x0502,
'fields': ['chan_ident', 'mode', 'destination', 'source'],
'data_fields': None
},
0x0502: {
'name': 'MGMSG_MOT_GET_TRIGGER',
'format': 'BBBB',
'response': None,
'fields': ['chan_ident', 'mode', 'destination', 'source'],
'data_fields': None
}
}
def __init__(self):
pass
@classmethod
def build_message(cls, msg_id, **kwargs):
if msg_id not in APTProtocol.MSGS:
raise ValueError(f'Unknown Message ID: {hex(msg_id)}')
msg_spec = APTProtocol.MSGS[msg_id]
for param in msg_spec['fields']:
if param not in kwargs:
raise ValueError(f"Missing required parameter '{param}' for {msg_spec['name']}.")
msg_format = msg_spec.get('format')
if msg_format == 'BB':
# this is a simple source/destination command
dest = kwargs['destination']
src = kwargs['source']
message = struct.pack('<HBBBB', msg_id,
0x00, 0x00,
dest, src)
elif msg_format == 'BBBB':
# this is a source/destination plus parameters
# fields order: [param1, param2, destination, source] or
# [param1, destination, source] (param2 = 0)
fields = msg_spec['fields']
dest = kwargs['destination']
src = kwargs['source']
# Get param fields (everything except destination/source)
param_fields = [f for f in fields if f not in ('destination', 'source')]
p1 = kwargs.get(param_fields[0], 0x00) if len(param_fields) > 0 else 0x00
p2 = kwargs.get(param_fields[1], 0x00) if len(param_fields) > 1 else 0x00
message = struct.pack('<HBBBB', msg_id,
p1, p2, dest, src)
else:
data_fields = msg_spec.get('data_fields')
# verify data fields are available
if data_fields is None:
raise ValueError("I need data for a long message!")
# Extract the message specific values and
# calculate payload size
df_values = [kwargs[field] for field in data_fields]
data_payload = struct.pack(msg_format, *df_values)
payload_len = len(data_payload)
header = struct.pack('<HHBB', msg_id, payload_len,
kwargs['destination'] | 0x80,
kwargs['source'])
# put message together
message = header + data_payload
return message
@classmethod
def unpack_message(cls, bdata):
# Get the message ID
msg_id = struct.unpack("<H", bdata[0:2])[0]
# See if the message ID has been implemented
# in the registry
if msg_id not in APTProtocol.MSGS:
raise ValueError(f"This op-code {msg_id} isn't in the registry!")
msg_spec = APTProtocol.MSGS[msg_id]
fmt_string = msg_spec.get('format')
# Check if it's a long message, or just a header
if bdata[4] & 0x80:
# this is a long message.
payload_size = struct.unpack("<H", bdata[2:4])[0]
src = bdata[5]
dest = bdata[4] & 0x7F
payload = bdata[6:6+payload_size]
data_fields = msg_spec.get('data_fields')
if data_fields is None:
raise ValueError(f"No data fields have been defined for {msg_spec['name']}!")
unpacked_payload = struct.unpack(fmt_string, payload)
data = {field: value for field, value in zip(data_fields,
unpacked_payload)}
data['destination'] = dest
data['source'] = src
return msg_id, data
else:
# it is a header only message
# determine type
dest = bdata[4]
src = bdata[5]
if fmt_string == 'BB':
# simple source/dest command
data = {'destination': dest, 'source': src}
return msg_id, data
elif fmt_string == 'BBBB':
# simple command with parameters
# Use field names from spec
fields = msg_spec['fields']
param_fields = [f for f in fields if f not in ('destination', 'source')]
data = {'destination': dest, 'source': src}
if len(param_fields) > 0:
data[param_fields[0]] = bdata[2]
if len(param_fields) > 1:
data[param_fields[1]] = bdata[3]
return msg_id, data
else:
raise ValueError(f"Message format {fmt_string} isn't defined/valid!")
@classmethod
def get_name(cls, msg_id):
return cls.MSGS.get(msg_id, {}).get('name', f'UNKNOWN_{hex(msg_id)}')
hardware/pybbd202/bbd20x.py
+505
View File
@@ -0,0 +1,505 @@
'''
BBD20X Stage Driver for SRAS
Thomas Ales | Feb 2026
Version 2
'''
import time
from threading import Thread, Event
from queue import Queue, Empty
from .apt_constants import StatusBits, TriggerBitsServo
from .apt_messages import APTProtocol
from .serial_comms import SerialSnooper
class ThorlabsServoDriver():
# These are specific to the MLS203-1
# change for a different application
counts_per_mm = 20000
accel_scaling = 13.744
velocity_scaling = 134217.73
def __init__(self):
self.am_connected = False
self.am_enabled = [False, False]
self.am_homed = [False, False]
self.am_moving = [False, False]
self.am_error = [False, False]
self.serial_port = "/dev/ttyUSB1"
self.serial_spd = 115200
self.max_velocities = [100.0, 100.0] # mm/s
self.max_accels = [500.0, 500.0] # mm/s2
self.positions = [-1.0, -1.0]
self.act_velocities = [-1.0, -1.0]
self.current_demand = [0, 0]
self.serial_snoop = None
self.am_listening = False
self.pending_responses = {} # msg_id -> {'event': Event, 'data': None}
self.bays_present = [] # list of bay addresses that responded
self._tx_queue = Queue()
self._polling_active = False
self._poll_interval = 0.2 # seconds between poll cycles
def connect(self, port=None, spd=None):
if port is None:
port = self.serial_port
else:
self.serial_port = port
if spd is None:
spd = self.serial_spd
else:
self.serial_spd = spd
self.serial_snoop = SerialSnooper(port, spd)
self.serial_snoop.start()
self.am_listening = True
# Start worker threads
self._tx_thread = Thread(target=self._tx_worker, daemon=True)
self._tx_thread.start()
self._rx_thread = Thread(target=self._rx_worker, daemon=True)
self._rx_thread.start()
self._poll_thread = Thread(target=self._poll_worker, daemon=True)
self._poll_thread.start()
# Give threads time to start
time.sleep(0.3)
# Query which bays are present
self.bays_present = []
for bay_id in range(10): # bays 0-9
try:
result = self.send_and_wait(0x0060, timeout=0.5,
bay_id=bay_id,
destination=0x11,
source=0x01)
if result and result.get('bay_state') == 0x01:
bay_addr = 0x21 + bay_id
self.bays_present.append(bay_addr)
except TimeoutError:
# Bay not present or not responding
pass
if not self.bays_present:
print(" [WARN] No bays detected!")
self.am_connected = True
# ── Worker threads ───────────────────────────────────────────
def _tx_worker(self):
'''Single serial writer. All outbound messages flow through
_tx_queue so there are no write races on the serial port.'''
while self.am_listening:
try:
msg = self._tx_queue.get(timeout=0.05)
self.serial_snoop.serial_connection.write(msg)
self.serial_snoop.serial_connection.flush()
except Empty:
continue
except (OSError, TypeError):
break
def _rx_worker(self):
'''Listens for messages on the serial RX queue and dispatches them.'''
while self.am_listening:
try:
_msg = self.serial_snoop.rx_msg_queue.get(timeout=0.05)
# Try to parse the message, skip if unknown
try:
_msgid, data = APTProtocol.unpack_message(_msg)
except ValueError:
print(f" [WARN] Unknown message: {_msg.hex()}")
continue
# Check if someone is waiting for this response
if _msgid in self.pending_responses:
self.pending_responses[_msgid]['data'] = data
self.pending_responses[_msgid]['event'].set()
# Status update → ACK via TX queue, then update state
if _msgid == 0x0491:
if self.am_listening:
_ack = APTProtocol.build_message(0x0492, source=0x01,
destination=_msg[5])
self._tx_queue.put(_ack)
self._update0x491(data)
# Move completed → update state
elif _msgid == 0x0464:
self._update0x0464(data)
except Empty:
continue
return
def _poll_worker(self):
'''Periodically sends REQ_USTATUSUPDATE (0x0490) to each bay.
The 0x0491 responses are ACKed by _rx_worker through the
TX queue, which keeps the controller's comms watchdog alive.'''
while self.am_listening:
if self._polling_active:
for axis_addr in (self.bays_present or [0x21, 0x22]):
if not self.am_listening:
break
msg = APTProtocol.build_message(0x0490, source=0x01,
destination=axis_addr)
self._tx_queue.put(msg)
time.sleep(self._poll_interval)
else:
time.sleep(0.05)
# ── Polling control ──────────────────────────────────────────
def start_polling(self, interval=0.2):
'''Start periodic status polling (interval in seconds).'''
self._poll_interval = interval
self._polling_active = True
def stop_polling(self):
'''Stop periodic status polling.'''
self._polling_active = False
# ── Core messaging ───────────────────────────────────────────
def send_and_wait(self, msg_id, timeout=10.0, retries=1, **kwargs):
"""Send a message and wait for its expected response.
On timeout, drains serial buffer and retries up to `retries` times."""
msg_spec = APTProtocol.MSGS.get(msg_id)
if not msg_spec:
raise ValueError(f"Unknown message: {hex(msg_id)}")
expected = msg_spec.get('response')
msg = APTProtocol.build_message(msg_id, **kwargs)
for attempt in range(1 + retries):
# Set up listener before sending
if expected:
evt = Event()
self.pending_responses[expected] = {'event': evt, 'data': None}
self._tx_queue.put(msg)
if not expected:
return None # no response expected
# Wait for response
if evt.wait(timeout=timeout):
data = self.pending_responses[expected]['data']
del self.pending_responses[expected]
return data
else:
del self.pending_responses[expected]
if attempt < retries:
# Drain serial input buffer and message queue, then retry
self.serial_snoop.serial_connection.reset_input_buffer()
time.sleep(0.05)
while not self.serial_snoop.rx_msg_queue.empty():
try:
self.serial_snoop.rx_msg_queue.get_nowait()
except Empty:
break
print(f" [RETRY] {APTProtocol.get_name(msg_id)} attempt {attempt+2}")
raise TimeoutError(f"Timeout waiting for {hex(expected)}")
def send_message(self, msg_id, **kwargs):
'''Build and queue a message for transmission (fire-and-forget).'''
msg = APTProtocol.build_message(msg_id, **kwargs)
self._tx_queue.put(msg)
# ── Connection management ────────────────────────────────────
def disconnect(self):
if (self.serial_snoop and
self.am_listening is True):
# Stop polling first
self._polling_active = False
# Queue disconnect messages for the TX worker to send
for addr in [0x11, 0x21, 0x22]:
self._tx_queue.put(
APTProtocol.build_message(0x0002, destination=addr,
source=0x01))
time.sleep(0.2) # let TX worker flush them out
# Stop all worker loops, then wait for threads to exit
self.am_listening = False
self.serial_snoop.stop()
self._rx_thread.join()
self._tx_thread.join()
self._poll_thread.join()
self.serial_snoop.join()
# Close port only after all threads are done
self.serial_snoop.close()
# ── State update handlers ────────────────────────────────────
def _update0x491(self, msg):
'''
_update0x491 - Internal function for handling USTATUSUPDATE
messages and updating the data for that particular axis sending
the message.
'''
if msg['source'] == 0x21:
ch = 0
elif msg['source'] == 0x22:
ch = 1
else:
return
self.positions[ch] = msg['position'] / self.counts_per_mm
self.act_velocities[ch] = msg['velocity'] / self.velocity_scaling
self.current_demand[ch] = msg['motor_current']
if(msg['status_bits'] & StatusBits.MOT_ANY_ERR):
self.am_error[ch] = True
else:
self.am_error[ch] = False
if(msg['status_bits'] & StatusBits.MOT_ANY_MOVE):
self.am_moving[ch] = True
else:
self.am_moving[ch] = False
if(msg['status_bits'] & StatusBits.MOT_SB_HOMED):
self.am_homed[ch] = True
def _update0x0464(self, msg):
'''
_update0x0464 - internal function for MOVE_COMPLETED messages.
Updates position, velocity, and moving state.
'''
if msg['source'] == 0x21:
ch = 0
elif msg['source'] == 0x22:
ch = 1
else:
return
self.positions[ch] = msg['position'] / self.counts_per_mm
self.act_velocities[ch] = msg['velocity'] / self.velocity_scaling
self.current_demand[ch] = msg['motor_current']
self.am_moving[ch] = False
return
def _update0x0212(self, msg):
'''
_update0x0212 - internal function that listens for CHANENABLESTATE
messages.
'''
if msg['source'] == 0x21:
ch = 0
elif msg['source'] == 0x22:
ch = 1
else:
raise ValueError("Wherever this message came from, it's WRONG!")
if msg['enable_state'] == 0x01:
self.am_enabled[ch] = True # enabled
elif msg['enable_state'] == 0x02:
self.am_enabled[ch] = False # disabled
else:
raise ValueError("Am I a joke to you? WTF did this even come from?!")
# ── Axis control ─────────────────────────────────────────────
def enable_axis(self, axis):
'''Enable the specified axis (0x21 = X, 0x22 = Y).'''
self.send_message(0x0210, chan_ident=1, enable_state=0x01,
destination=axis, source=0x01)
def disable_axis(self, axis):
'''Disable the specified axis (0x21 = X, 0x22 = Y).'''
self.send_message(0x0210, chan_ident=1, enable_state=0x02,
destination=axis, source=0x01)
def toggle_enabled_state(self, axis):
'''
toggle_enabled_state(axis) - enables the axis if disabled. disables
if enabled. not much more to it.
'''
if axis == 0x21:
ch = 0
elif axis == 0x22:
ch = 1
else:
raise ValueError("I don't know that axis!")
# get the old state and flip it like a sample
new_state = not self.am_enabled[ch]
self.send_message(0x0210, chan_ident=1,
enable_state=0x01 if new_state else 0x02,
destination=axis, source=0x01)
def home_axis(self, axis, timeout=60.0):
'''
home_axis(axis, timeout=60): Blocking home command. Required at
power up. Default timeout is 60s, but 20-30s is fine as well if
you're in that much of a hurry.
'''
if axis == 0x21:
ch = 0
elif axis == 0x22:
ch = 1
else:
raise ValueError("I don't know that axis!")
self.send_and_wait(0x0443, timeout=timeout, chan_ident=1,
destination=axis, source=0x01)
return
def move_axis_relative(self, axis, distance_in_mm, timeout=10.0):
'''
move_axis_relative(axis, distance_in_mm, timeout=10):
moves the specified axis a specified distance in mm.
Timeout defaults to ten seconds.
'''
# sanity check
if axis == 0x21 and abs(distance_in_mm) > 110.0:
raise ValueError("You can't move farther than the stage is long.")
elif axis == 0x22 and abs(distance_in_mm) > 75.0:
raise ValueError("You can't move farther than the stage is wide.")
_distance_in_encoder = int(round(distance_in_mm * self.counts_per_mm))
self.send_and_wait(0x0448, timeout=timeout, chan_ident=1,
relative_distance=_distance_in_encoder,
destination=axis, source=0x01)
return
def move_axis_absolute(self, axis, position_in_mm, timeout=10.0):
'''
move_axis_absolute(axis, position_in_mm, timeout=10):
moves the specified axis to an absolute position in mm.
Timeout defaults to ten seconds.
'''
if axis == 0x21 and (position_in_mm < 0.0 or position_in_mm > 110.0):
raise ValueError("Position out of range for X axis (0-110 mm).")
elif axis == 0x22 and (position_in_mm < 0.0 or position_in_mm > 75.0):
raise ValueError("Position out of range for Y axis (0-75 mm).")
_position_in_encoder = int(round(position_in_mm * self.counts_per_mm))
self.send_and_wait(0x0453, timeout=timeout, chan_ident=1,
absolute_distance=_position_in_encoder,
destination=axis, source=0x01)
return
# ── Velocity parameters ──────────────────────────────────────
def get_velocity_params(self, axis, timeout=5.0):
'''
get_velocity_params(axis): Queries the current velocity parameters
for the specified axis. Returns a dict with keys:
min_velocity (mm/s), acceleration (mm/s2), max_velocity (mm/s)
'''
if axis == 0x21:
ch = 0
elif axis == 0x22:
ch = 1
else:
raise ValueError("I don't know that axis!")
result = self.send_and_wait(0x0414, timeout=timeout, chan_ident=1,
zero_this=0x00, destination=axis,
source=0x01)
params = {
'min_velocity': result['min_velocity'] / self.velocity_scaling,
'acceleration': result['acceleration'] / self.accel_scaling,
'max_velocity': result['max_velocity'] / self.velocity_scaling,
}
self.max_velocities[ch] = params['max_velocity']
self.max_accels[ch] = params['acceleration']
return params
def set_velocity_params(self, axis, max_velocity=None, acceleration=None):
'''
set_velocity_params(axis, max_velocity=None, acceleration=None):
Sets velocity and/or acceleration for the specified axis.
Values are in mm/s and mm/s2 respectively. Any parameter
left as None keeps its current value.
'''
if axis == 0x21:
ch = 0
elif axis == 0x22:
ch = 1
else:
raise ValueError("I don't know that axis!")
# Only query current params if we need to fill in a missing value
if max_velocity is None or acceleration is None:
current = self.get_velocity_params(axis)
if max_velocity is None:
max_velocity = current['max_velocity']
if acceleration is None:
acceleration = current['acceleration']
# Update the cached values
self.max_velocities[ch] = max_velocity
self.max_accels[ch] = acceleration
_min_v = 0
_accel = int(round(acceleration * self.accel_scaling))
_max_v = int(round(max_velocity * self.velocity_scaling))
self.send_message(0x0413, chan_ident=1,
min_velocity=_min_v,
acceleration=_accel,
max_velocity=_max_v,
destination=axis, source=0x01)
# ── Trigger control ───────────────────────────────────────
def set_trigger(self, axis, mode):
'''
set_trigger(axis, mode): Sets the trigger mode for the specified
axis. Mode should be a TriggerBitsServo value or combination.
'''
self.send_message(0x0500, chan_ident=1, mode=int(mode),
destination=axis, source=0x01)
def get_trigger(self, axis, timeout=5.0):
'''
get_trigger(axis): Queries the current trigger mode for the
specified axis. Returns the mode byte as a TriggerBitsServo.
'''
result = self.send_and_wait(0x0501, timeout=timeout,
chan_ident=1, mode=0x00,
destination=axis, source=0x01)
return TriggerBitsServo(result['mode'])
def set_trigger_trigin_high(self, axis):
'''Set trigger input to logic high.'''
self.set_trigger(axis, TriggerBitsServo.TRIGIN_HIGH)
def set_trigger_trigin_relmove(self, axis):
'''Set trigger input to initiate a relative move.'''
self.set_trigger(axis, TriggerBitsServo.TRIGIN_RELMOVE)
def set_trigger_trigin_absmove(self, axis):
'''Set trigger input to initiate an absolute move.'''
self.set_trigger(axis, TriggerBitsServo.TRIGIN_ABSMOVE)
def set_trigger_trigin_homemove(self, axis):
'''Set trigger input to initiate a home move.'''
self.set_trigger(axis, TriggerBitsServo.TRIGIN_HOMEMOVE)
def set_trigger_trigout_high(self, axis):
'''Set trigger output to logic high.'''
self.set_trigger(axis, TriggerBitsServo.TRIGOUT_HIGH)
def set_trigger_trigout_inmotion(self, axis):
'''Set trigger output high while axis is in motion.'''
self.set_trigger(axis, TriggerBitsServo.TRIGOUT_INMOTION)
def set_trigger_trigout_motioncomplete(self, axis):
'''Set trigger output to pulse when motion completes.'''
self.set_trigger(axis, TriggerBitsServo.TRIGOUT_MOTIONCOMPLETE)
def set_trigger_trigout_maxvelocity(self, axis):
'''Set trigger output to pulse at max velocity.'''
self.set_trigger(axis, TriggerBitsServo.TRIGOUT_MAXVELOCITY)
def set_trigger_trigout_maxv(self, axis):
'''Set trigger output high + pulse at max velocity (TRIGOUT_MAXV).'''
self.set_trigger(axis, TriggerBitsServo.TRIGOUT_MAXV)
hardware/pybbd202/serial_comms.py
+94
View File
@@ -0,0 +1,94 @@
'''
SRAS Serial Communication Tools
Thomas Ales | Feb 2026
Version 1
'''
from threading import Thread
from queue import Queue
import serial
import struct
import time
class SerialSnooper(Thread):
def __init__(self, _port, _spd):
super().__init__()
self.serial_connection = None
self.serial_port = _port
self.serial_speed = _spd
self.am_listening = False
self.rx_msg_queue = Queue()
def run(self):
'''
run() - Starts up the serial listener, checks if the port was
opened successfully, and if so begins
listening for APT messages.
'''
self.serial_connection = serial.Serial(port=self.serial_port,
baudrate=self.serial_speed,
rtscts=True, timeout=0.05)
if self.serial_connection.is_open is True:
# Send disconnect to stop any ongoing auto-updates from
# a previous session
for addr in [0x11, 0x21, 0x22]:
disconnect_msg = struct.pack('<HBBBB', 0x0002,
0x00, 0x00, addr, 0x01)
self.serial_connection.write(disconnect_msg)
# Wait for controller to process, then flush everything
time.sleep(0.2)
self.serial_connection.reset_output_buffer()
self.serial_connection.reset_input_buffer()
# Discard any remaining data that arrived
time.sleep(0.1)
if self.serial_connection.in_waiting > 0:
self.serial_connection.read(self.serial_connection.in_waiting)
self.am_listening = True
_rxbuf = bytearray()
while self.am_listening is True:
try:
# Read whatever is available (or wait up to timeout)
_chunk = self.serial_connection.read(
max(1, self.serial_connection.in_waiting))
if _chunk:
_rxbuf.extend(_chunk)
# Process complete messages from the buffer
while len(_rxbuf) >= 6:
# Check if this is a long message (bit 7 of byte 4)
if _rxbuf[4] & 0x80:
_msglen = struct.unpack("<H", _rxbuf[2:4])[0]
total = 6 + _msglen
if len(_rxbuf) < total:
break # need more bytes
_packet = bytes(_rxbuf[:total])
del _rxbuf[:total]
else:
_packet = bytes(_rxbuf[:6])
del _rxbuf[:6]
self.rx_msg_queue.put_nowait(_packet)
except (serial.SerialException, TypeError, OSError):
break
return
def stop(self):
'''
stop() - Signals the listener loop to stop. Call join() after
this to wait for the thread to exit, then call close() to
release the serial port.
'''
self.am_listening = False
def close(self):
'''
close() - Closes the serial port. Only call after the
thread has been joined.
'''
if self.serial_connection and self.serial_connection.is_open:
self.serial_connection.close()