Initial commit: merge nuescan, pymso, pybbd202, and pypewpewhops into scanengine-3
- Merged four separate hardware control projects into unified platform - Created unified requirements.txt with all dependencies - Added comprehensive .gitignore - Added project overview README Co-Authored-By: Claude Sonnet 4.5 <noreply@anthropic.com>
This commit is contained in:
Thomas Ales [M S E]
@@ -0,0 +1,928 @@
|
||||
"""
|
||||
ThorLabs BBD203 3-Channel Motor Controller Driver
|
||||
Complete implementation of the APT protocol for BBD203
|
||||
|
||||
Copyright (C) 2025 Thomas Ales
|
||||
Licensed under GNU General Public License v2.0
|
||||
"""
|
||||
|
||||
import serial
|
||||
import serial.tools.list_ports
|
||||
import time
|
||||
import threading
|
||||
from typing import Dict, List, Optional, Callable, Tuple
|
||||
from queue import Queue, Empty
|
||||
|
||||
from hardware.bbd203_protocol import (
|
||||
APTProtocol, APTMessage, MessageID, StatusBits, TriggerMode, Destination
|
||||
)
|
||||
|
||||
|
||||
class BBD203Channel:
|
||||
"""Represents a single channel on the BBD203"""
|
||||
|
||||
def __init__(self, channel_num: int):
|
||||
"""
|
||||
Initialize channel
|
||||
|
||||
Args:
|
||||
channel_num: Channel number (1, 2, or 3)
|
||||
"""
|
||||
self.channel_num = channel_num
|
||||
self.enabled = False
|
||||
self.homed = False
|
||||
self.position_mm = 0.0
|
||||
self.encoder_count = 0
|
||||
self.status_bits = 0
|
||||
self.moving = False
|
||||
self.homing = False
|
||||
self.error = False
|
||||
|
||||
def update_from_status(self, position: int, encoder: int, status: int,
|
||||
protocol: APTProtocol):
|
||||
"""Update channel state from status update"""
|
||||
self.position_mm = protocol.apt_to_position(position)
|
||||
self.encoder_count = encoder
|
||||
self.status_bits = status
|
||||
|
||||
# Parse status bits
|
||||
self.homed = bool(status & StatusBits.HOMED)
|
||||
self.homing = bool(status & StatusBits.HOMING)
|
||||
self.enabled = bool(status & StatusBits.MOTOR_ENABLED)
|
||||
self.error = bool(status & StatusBits.MOTION_ERROR)
|
||||
|
||||
# Check if moving
|
||||
self.moving = bool(status & (
|
||||
StatusBits.IN_MOTION_FORWARD |
|
||||
StatusBits.IN_MOTION_REVERSE |
|
||||
StatusBits.JOGGING_FORWARD |
|
||||
StatusBits.JOGGING_REVERSE |
|
||||
StatusBits.HOMING
|
||||
))
|
||||
|
||||
def is_ready(self) -> bool:
|
||||
"""Check if channel is ready for operation"""
|
||||
return self.enabled and self.homed and not self.error
|
||||
|
||||
|
||||
class BBD203Driver:
|
||||
"""
|
||||
Complete driver for ThorLabs BBD203 3-Channel Motor Controller
|
||||
|
||||
Features:
|
||||
- 3 independent motor channels
|
||||
- Binary APT protocol communication
|
||||
- Automatic status updates
|
||||
- Thread-safe operation
|
||||
- Position and velocity control
|
||||
"""
|
||||
|
||||
def __init__(self, encoder_counts_per_mm: int = 20000, timeout: float = 1.0):
|
||||
"""
|
||||
Initialize BBD203 driver
|
||||
|
||||
Args:
|
||||
encoder_counts_per_mm: Encoder resolution (default: 20000 for MLS203)
|
||||
timeout: Serial communication timeout in seconds
|
||||
"""
|
||||
self.protocol = APTProtocol(encoder_counts_per_mm)
|
||||
self.timeout = timeout
|
||||
|
||||
# Serial connection
|
||||
self._serial: Optional[serial.Serial] = None
|
||||
self._port_name = ""
|
||||
self._connected = False
|
||||
|
||||
# Channels
|
||||
self.channels = {
|
||||
1: BBD203Channel(1),
|
||||
2: BBD203Channel(2),
|
||||
3: BBD203Channel(3)
|
||||
}
|
||||
|
||||
# Communication thread
|
||||
self._rx_thread: Optional[threading.Thread] = None
|
||||
self._stop_thread = threading.Event()
|
||||
self._rx_queue = Queue()
|
||||
|
||||
# Callbacks for asynchronous events
|
||||
self._move_complete_callbacks: Dict[int, List[Callable]] = {1: [], 2: [], 3: []}
|
||||
self._home_complete_callbacks: Dict[int, List[Callable]] = {1: [], 2: [], 3: []}
|
||||
|
||||
# Hardware info
|
||||
self._hw_info = {}
|
||||
|
||||
# ==================== Connection Management ====================
|
||||
|
||||
@staticmethod
|
||||
def list_available_ports() -> List[str]:
|
||||
"""
|
||||
List available serial ports
|
||||
|
||||
Returns:
|
||||
list: Available port names
|
||||
"""
|
||||
ports = serial.tools.list_ports.comports()
|
||||
return [port.device for port in ports]
|
||||
|
||||
@staticmethod
|
||||
def list_thorlabs_devices() -> List[Dict[str, str]]:
|
||||
"""
|
||||
List all ThorLabs APT devices connected via USB
|
||||
|
||||
Returns:
|
||||
list: List of dictionaries containing device information
|
||||
Each dict has: 'serial', 'port', 'description', 'vid', 'pid'
|
||||
"""
|
||||
thorlabs_devices = []
|
||||
ports = serial.tools.list_ports.comports()
|
||||
|
||||
# ThorLabs devices typically use FTDI chips
|
||||
# Common VID/PID combinations:
|
||||
# - FTDI: VID=0x0403, various PIDs
|
||||
thorlabs_vids = [0x0403] # FTDI vendor ID
|
||||
|
||||
for port in ports:
|
||||
# Check if this is a ThorLabs device by VID
|
||||
if port.vid in thorlabs_vids:
|
||||
device_info = {
|
||||
'serial': port.serial_number or 'Unknown',
|
||||
'port': port.device,
|
||||
'description': port.description or 'Unknown',
|
||||
'manufacturer': port.manufacturer or 'Unknown',
|
||||
'vid': f"0x{port.vid:04X}" if port.vid else 'Unknown',
|
||||
'pid': f"0x{port.pid:04X}" if port.pid else 'Unknown'
|
||||
}
|
||||
thorlabs_devices.append(device_info)
|
||||
print(f"DEBUG: Found ThorLabs device - Serial: {device_info['serial']}, "
|
||||
f"Port: {device_info['port']}")
|
||||
|
||||
return thorlabs_devices
|
||||
|
||||
@staticmethod
|
||||
def find_device_by_serial(serial_number: str) -> Optional[str]:
|
||||
"""
|
||||
Find ThorLabs device by serial number and return its port
|
||||
|
||||
Args:
|
||||
serial_number: Device serial number (e.g., '83123456')
|
||||
|
||||
Returns:
|
||||
str: COM port name if found, None otherwise
|
||||
"""
|
||||
devices = BBD203Driver.list_thorlabs_devices()
|
||||
|
||||
for device in devices:
|
||||
if device['serial'] == serial_number:
|
||||
print(f"INFO: Found device {serial_number} on port {device['port']}")
|
||||
return device['port']
|
||||
|
||||
print(f"WARNING: Device with serial number {serial_number} not found")
|
||||
print(f"Available devices: {[d['serial'] for d in devices]}")
|
||||
return None
|
||||
|
||||
def connect_by_serial(self, serial_number: str, baudrate: int = 115200) -> bool:
|
||||
"""
|
||||
Connect to BBD203 controller by serial number (auto-find port)
|
||||
|
||||
This is the preferred connection method - automatically finds the
|
||||
device by serial number over USB, similar to Kinesis library.
|
||||
|
||||
Args:
|
||||
serial_number: Device serial number (e.g., '83123456')
|
||||
baudrate: Baud rate (default: 115200)
|
||||
|
||||
Returns:
|
||||
bool: True if connection successful
|
||||
|
||||
Example:
|
||||
driver.connect_by_serial('83123456')
|
||||
"""
|
||||
# Find device port by serial number
|
||||
port = self.find_device_by_serial(serial_number)
|
||||
|
||||
if port is None:
|
||||
print(f"ERROR: Could not find BBD203 with serial number {serial_number}")
|
||||
print("Available ThorLabs devices:")
|
||||
for device in self.list_thorlabs_devices():
|
||||
print(f" Serial: {device['serial']}, Port: {device['port']}, "
|
||||
f"Description: {device['description']}")
|
||||
return False
|
||||
|
||||
# Connect using the found port
|
||||
return self.connect(port, baudrate)
|
||||
|
||||
def connect(self, port: str, baudrate: int = 115200) -> bool:
|
||||
"""
|
||||
Connect to BBD203 controller by port name
|
||||
|
||||
Note: It's recommended to use connect_by_serial() instead, which
|
||||
automatically finds the device by serial number.
|
||||
|
||||
Args:
|
||||
port: Serial port name (e.g., 'COM3' or '/dev/ttyUSB0')
|
||||
baudrate: Baud rate (default: 115200)
|
||||
|
||||
Returns:
|
||||
bool: True if connection successful
|
||||
"""
|
||||
try:
|
||||
print(f"INFO: Connecting to BBD203 on {port}")
|
||||
|
||||
self._serial = serial.Serial(
|
||||
port=port,
|
||||
baudrate=baudrate,
|
||||
bytesize=serial.EIGHTBITS,
|
||||
parity=serial.PARITY_NONE,
|
||||
stopbits=serial.STOPBITS_ONE,
|
||||
timeout=self.timeout,
|
||||
rtscts=False, # Disable hardware flow control
|
||||
xonxoff=False # Disable software flow control
|
||||
)
|
||||
|
||||
# Set DTR and RTS for ThorLabs FTDI devices
|
||||
# For BBD203, RTS should be LOW to enable communication
|
||||
self._serial.dtr = False
|
||||
self._serial.rts = False
|
||||
|
||||
self._port_name = port
|
||||
self._connected = True
|
||||
|
||||
# Give controller time to initialize after connection
|
||||
time.sleep(0.5)
|
||||
|
||||
# Start receive thread
|
||||
self._stop_thread.clear()
|
||||
self._rx_thread = threading.Thread(target=self._receive_loop, daemon=True)
|
||||
self._rx_thread.start()
|
||||
|
||||
# Initialize controller
|
||||
time.sleep(0.5) # Allow thread to start and controller to be ready
|
||||
|
||||
# Request hardware info
|
||||
self._send_command(self.protocol.cmd_req_hw_info())
|
||||
time.sleep(0.5)
|
||||
|
||||
# Start automatic status updates
|
||||
self._send_command(self.protocol.cmd_start_update_msgs())
|
||||
time.sleep(0.5)
|
||||
|
||||
print(f"INFO: Successfully connected to BBD203 on {port}")
|
||||
return True
|
||||
|
||||
except serial.SerialException as e:
|
||||
print(f"ERROR: Failed to connect to {port}: {e}")
|
||||
self._connected = False
|
||||
return False
|
||||
|
||||
def disconnect(self) -> bool:
|
||||
"""
|
||||
Disconnect from BBD203 controller
|
||||
|
||||
Returns:
|
||||
bool: True if disconnection successful
|
||||
"""
|
||||
if not self._connected:
|
||||
return True
|
||||
|
||||
try:
|
||||
print("INFO: Disconnecting from BBD203")
|
||||
|
||||
# Stop status updates
|
||||
self._send_command(self.protocol.cmd_stop_update_msgs())
|
||||
time.sleep(0.1)
|
||||
|
||||
# Stop receive thread
|
||||
self._stop_thread.set()
|
||||
if self._rx_thread:
|
||||
self._rx_thread.join(timeout=2.0)
|
||||
|
||||
# Close serial port
|
||||
if self._serial and self._serial.is_open:
|
||||
self._serial.close()
|
||||
|
||||
self._connected = False
|
||||
print("INFO: Disconnected from BBD203")
|
||||
return True
|
||||
|
||||
except Exception as e:
|
||||
print(f"ERROR: Error during disconnect: {e}")
|
||||
return False
|
||||
|
||||
def is_connected(self) -> bool:
|
||||
"""Check if controller is connected"""
|
||||
return self._connected and self._serial and self._serial.is_open
|
||||
|
||||
# ==================== Communication Methods ====================
|
||||
|
||||
def _send_command(self, cmd: bytes) -> bool:
|
||||
"""
|
||||
Send command to controller
|
||||
|
||||
Args:
|
||||
cmd: Command bytes to send
|
||||
|
||||
Returns:
|
||||
bool: True if send successful
|
||||
"""
|
||||
if not self.is_connected():
|
||||
print("ERROR: Cannot send command - not connected")
|
||||
return False
|
||||
|
||||
try:
|
||||
print(f"DEBUG: Sending {len(cmd)} bytes: {cmd.hex()}")
|
||||
self._serial.write(cmd)
|
||||
self._serial.flush() # Ensure data is sent
|
||||
return True
|
||||
except serial.SerialException as e:
|
||||
print(f"ERROR: Failed to send command: {e}")
|
||||
return False
|
||||
|
||||
def _receive_loop(self):
|
||||
"""Background thread to receive messages from controller"""
|
||||
buffer = bytearray()
|
||||
print("DEBUG: Receive thread started")
|
||||
|
||||
while not self._stop_thread.is_set():
|
||||
try:
|
||||
if self._serial.in_waiting > 0:
|
||||
data = self._serial.read(self._serial.in_waiting)
|
||||
print(f"DEBUG: Received {len(data)} bytes: {data.hex()}")
|
||||
buffer.extend(data)
|
||||
|
||||
# Process complete messages
|
||||
while len(buffer) >= 6:
|
||||
# Parse header
|
||||
msg_id, data_len, dest, source = APTMessage.parse_header(buffer)
|
||||
|
||||
# Determine total message length
|
||||
if data_len == 0 or data_len > 255:
|
||||
# Header-only message
|
||||
msg_len = 6
|
||||
else:
|
||||
# Message with data
|
||||
msg_len = 6 + data_len
|
||||
|
||||
# Wait for complete message
|
||||
if len(buffer) < msg_len:
|
||||
break
|
||||
|
||||
# Extract message
|
||||
msg = bytes(buffer[:msg_len])
|
||||
buffer = buffer[msg_len:]
|
||||
|
||||
# Process message
|
||||
self._process_message(msg_id, msg)
|
||||
|
||||
else:
|
||||
time.sleep(0.001) # Small delay to prevent busy waiting
|
||||
|
||||
except Exception as e:
|
||||
if not self._stop_thread.is_set():
|
||||
print(f"ERROR: Exception in receive loop: {e}")
|
||||
time.sleep(0.1)
|
||||
|
||||
def _process_message(self, msg_id: int, msg: bytes):
|
||||
"""
|
||||
Process received message
|
||||
|
||||
Args:
|
||||
msg_id: Message ID
|
||||
msg: Complete message bytes
|
||||
"""
|
||||
try:
|
||||
print(f"DEBUG: Processing message ID 0x{msg_id:04X}, len={len(msg)}, data={msg.hex()}")
|
||||
|
||||
if msg_id == MessageID.MGMSG_MOT_GET_STATUSUPDATE:
|
||||
# Status update
|
||||
channel, position, encoder, status = APTMessage.parse_status_update(msg)
|
||||
# Determine which channel this is for (from destination byte)
|
||||
dest = msg[4]
|
||||
channel_num = dest - 0x20 # 0x21->1, 0x22->2, 0x23->3
|
||||
|
||||
if channel_num in self.channels:
|
||||
self.channels[channel_num].update_from_status(
|
||||
position, encoder, status, self.protocol
|
||||
)
|
||||
|
||||
elif msg_id == MessageID.MGMSG_MOT_MOVE_COMPLETED:
|
||||
# Move completed
|
||||
dest = msg[4]
|
||||
channel_num = dest - 0x20
|
||||
|
||||
if channel_num in self.channels:
|
||||
self.channels[channel_num].moving = False
|
||||
|
||||
# Call callbacks
|
||||
for callback in self._move_complete_callbacks.get(channel_num, []):
|
||||
callback(channel_num)
|
||||
|
||||
elif msg_id == MessageID.MGMSG_MOT_MOVE_HOMED:
|
||||
# Homing completed
|
||||
dest = msg[4]
|
||||
channel_num = dest - 0x20
|
||||
|
||||
if channel_num in self.channels:
|
||||
self.channels[channel_num].homed = True
|
||||
self.channels[channel_num].homing = False
|
||||
|
||||
# Call callbacks
|
||||
for callback in self._home_complete_callbacks.get(channel_num, []):
|
||||
callback(channel_num)
|
||||
|
||||
elif msg_id == MessageID.MGMSG_MOT_MOVE_STOPPED:
|
||||
# Motion stopped
|
||||
dest = msg[4]
|
||||
channel_num = dest - 0x20
|
||||
|
||||
if channel_num in self.channels:
|
||||
self.channels[channel_num].moving = False
|
||||
|
||||
elif msg_id == MessageID.MGMSG_MOD_GET_CHANENABLESTATE:
|
||||
# Channel enable state
|
||||
channel, enabled = APTMessage.parse_channel_enable_state(msg)
|
||||
print(f"DEBUG: Received CHANENABLESTATE - parsed channel={channel}, enabled={enabled}, msg={msg.hex()}")
|
||||
|
||||
# Use the channel number from the parsed message
|
||||
if channel in self.channels:
|
||||
self.channels[channel].enabled = enabled
|
||||
print(f"DEBUG: Set channel {channel} enabled={enabled}")
|
||||
|
||||
elif msg_id == MessageID.MGMSG_HW_RESPONSE:
|
||||
# Hardware response (error or acknowledgement)
|
||||
print(f"DEBUG: Received HW_RESPONSE: {msg.hex()}")
|
||||
|
||||
elif msg_id == MessageID.MGMSG_HW_GET_INFO:
|
||||
# Hardware info
|
||||
print(f"DEBUG: Received hardware info")
|
||||
|
||||
except Exception as e:
|
||||
print(f"ERROR: Failed to process message {msg_id:04X}: {e}")
|
||||
|
||||
def _set_and_verify_enable(self, channel: int, enable: bool, retries: int = 3) -> bool:
|
||||
"""
|
||||
Set channel enable state and verify it was set correctly
|
||||
|
||||
Args:
|
||||
channel: Channel number (1, 2, or 3)
|
||||
enable: True to enable, False to disable
|
||||
retries: Number of retry attempts
|
||||
|
||||
Returns:
|
||||
bool: True if value was set and verified
|
||||
"""
|
||||
for attempt in range(retries):
|
||||
# Send enable command
|
||||
cmd = self.protocol.cmd_enable_channel(channel, enable)
|
||||
if not self._send_command(cmd):
|
||||
continue
|
||||
|
||||
time.sleep(0.5) # Wait for controller to process
|
||||
|
||||
# Request channel enable state to verify
|
||||
req_cmd = self.protocol.cmd_req_channel_enable_state(channel)
|
||||
self._send_command(req_cmd)
|
||||
time.sleep(0.5) # Wait for response
|
||||
|
||||
# Check if state matches expected
|
||||
if self.channels[channel].enabled == enable:
|
||||
return True
|
||||
|
||||
if attempt < retries - 1:
|
||||
print(f"DEBUG: Enable verification failed for channel {channel}, "
|
||||
f"retrying ({attempt + 1}/{retries})")
|
||||
time.sleep(0.2)
|
||||
|
||||
print(f"ERROR: Failed to set and verify enable state for channel {channel} "
|
||||
f"after {retries} attempts")
|
||||
return False
|
||||
|
||||
# ==================== Channel Control ====================
|
||||
|
||||
def enable_channel(self, channel: int, enable: bool = True) -> bool:
|
||||
"""
|
||||
Enable or disable a motor channel
|
||||
|
||||
Args:
|
||||
channel: Channel number (1, 2, or 3)
|
||||
enable: True to enable, False to disable
|
||||
|
||||
Returns:
|
||||
bool: True if command sent successfully
|
||||
"""
|
||||
if channel not in [1, 2, 3]:
|
||||
print(f"ERROR: Invalid channel number: {channel}")
|
||||
return False
|
||||
|
||||
action = "Enabling" if enable else "Disabling"
|
||||
print(f"DEBUG: {action} channel {channel}")
|
||||
|
||||
# Use set and verify to ensure command was processed
|
||||
return self._set_and_verify_enable(channel, enable)
|
||||
|
||||
def identify(self, channel: int) -> bool:
|
||||
"""
|
||||
Flash front panel LEDs to identify controller
|
||||
|
||||
Args:
|
||||
channel: Channel number (1, 2, or 3)
|
||||
|
||||
Returns:
|
||||
bool: True if command sent successfully
|
||||
"""
|
||||
print(f"DEBUG: Identifying channel {channel}")
|
||||
cmd = self.protocol.cmd_identify(channel)
|
||||
return self._send_command(cmd)
|
||||
|
||||
# ==================== Homing ====================
|
||||
|
||||
def home_channel(self, channel: int, wait: bool = False, timeout: float = 30.0) -> bool:
|
||||
"""
|
||||
Home a motor channel
|
||||
|
||||
Args:
|
||||
channel: Channel number (1, 2, or 3)
|
||||
wait: If True, block until homing complete
|
||||
timeout: Timeout in seconds if waiting
|
||||
|
||||
Returns:
|
||||
bool: True if homing initiated (or completed if wait=True)
|
||||
"""
|
||||
if channel not in [1, 2, 3]:
|
||||
print(f"ERROR: Invalid channel number: {channel}")
|
||||
return False
|
||||
|
||||
print(f"DEBUG: Homing channel {channel}")
|
||||
|
||||
self.channels[channel].homing = True
|
||||
self.channels[channel].homed = False
|
||||
|
||||
cmd = self.protocol.cmd_move_home(channel)
|
||||
if not self._send_command(cmd):
|
||||
return False
|
||||
|
||||
if wait:
|
||||
# Wait for homing to complete
|
||||
start_time = time.time()
|
||||
while time.time() - start_time < timeout:
|
||||
if self.channels[channel].homed and not self.channels[channel].homing:
|
||||
print(f"INFO: Channel {channel} homing completed")
|
||||
return True
|
||||
time.sleep(0.1)
|
||||
|
||||
print(f"ERROR: Homing timeout for channel {channel}")
|
||||
return False
|
||||
|
||||
return True
|
||||
|
||||
def home_all_channels(self, wait: bool = False, timeout: float = 30.0) -> bool:
|
||||
"""
|
||||
Home all enabled channels
|
||||
|
||||
Args:
|
||||
wait: If True, block until all homing complete
|
||||
timeout: Timeout in seconds if waiting
|
||||
|
||||
Returns:
|
||||
bool: True if all homing operations successful
|
||||
"""
|
||||
success = True
|
||||
for channel in [1, 2, 3]:
|
||||
if self.channels[channel].enabled:
|
||||
if not self.home_channel(channel, wait=False):
|
||||
success = False
|
||||
|
||||
if wait:
|
||||
start_time = time.time()
|
||||
while time.time() - start_time < timeout:
|
||||
all_homed = all(
|
||||
ch.homed for ch in self.channels.values() if ch.enabled
|
||||
)
|
||||
if all_homed:
|
||||
print("INFO: All channels homed successfully")
|
||||
return True
|
||||
time.sleep(0.1)
|
||||
|
||||
print("ERROR: Timeout waiting for all channels to home")
|
||||
return False
|
||||
|
||||
return success
|
||||
|
||||
# ==================== Motion Control ====================
|
||||
|
||||
def move_absolute(self, channel: int, position_mm: float,
|
||||
wait: bool = False, timeout: float = 30.0) -> bool:
|
||||
"""
|
||||
Move to absolute position
|
||||
|
||||
Args:
|
||||
channel: Channel number (1, 2, or 3)
|
||||
position_mm: Target position in mm
|
||||
wait: If True, block until move complete
|
||||
timeout: Timeout in seconds if waiting
|
||||
|
||||
Returns:
|
||||
bool: True if move initiated (or completed if wait=True)
|
||||
"""
|
||||
if channel not in [1, 2, 3]:
|
||||
print(f"ERROR: Invalid channel number: {channel}")
|
||||
return False
|
||||
|
||||
if not self.channels[channel].is_ready():
|
||||
print(f"ERROR: Channel {channel} not ready for movement")
|
||||
return False
|
||||
|
||||
print(f"DEBUG: Moving channel {channel} to {position_mm} mm")
|
||||
|
||||
self.channels[channel].moving = True
|
||||
|
||||
cmd = self.protocol.cmd_move_absolute(channel, position_mm)
|
||||
if not self._send_command(cmd):
|
||||
return False
|
||||
|
||||
if wait:
|
||||
# Wait for move to complete
|
||||
start_time = time.time()
|
||||
while time.time() - start_time < timeout:
|
||||
if not self.channels[channel].moving:
|
||||
print(f"INFO: Channel {channel} move completed")
|
||||
return True
|
||||
time.sleep(0.01)
|
||||
|
||||
print(f"ERROR: Move timeout for channel {channel}")
|
||||
return False
|
||||
|
||||
return True
|
||||
|
||||
def move_relative(self, channel: int, distance_mm: float,
|
||||
wait: bool = False, timeout: float = 30.0) -> bool:
|
||||
"""
|
||||
Move relative distance
|
||||
|
||||
Args:
|
||||
channel: Channel number (1, 2, or 3)
|
||||
distance_mm: Distance to move in mm (positive or negative)
|
||||
wait: If True, block until move complete
|
||||
timeout: Timeout in seconds if waiting
|
||||
|
||||
Returns:
|
||||
bool: True if move initiated (or completed if wait=True)
|
||||
"""
|
||||
if channel not in [1, 2, 3]:
|
||||
print(f"ERROR: Invalid channel number: {channel}")
|
||||
return False
|
||||
|
||||
if not self.channels[channel].is_ready():
|
||||
print(f"ERROR: Channel {channel} not ready for movement")
|
||||
return False
|
||||
|
||||
print(f"DEBUG: Moving channel {channel} by {distance_mm} mm")
|
||||
|
||||
self.channels[channel].moving = True
|
||||
|
||||
cmd = self.protocol.cmd_move_relative(channel, distance_mm)
|
||||
if not self._send_command(cmd):
|
||||
return False
|
||||
|
||||
if wait:
|
||||
# Wait for move to complete
|
||||
start_time = time.time()
|
||||
while time.time() - start_time < timeout:
|
||||
if not self.channels[channel].moving:
|
||||
print(f"INFO: Channel {channel} move completed")
|
||||
return True
|
||||
time.sleep(0.01)
|
||||
|
||||
print(f"ERROR: Move timeout for channel {channel}")
|
||||
return False
|
||||
|
||||
return True
|
||||
|
||||
def stop(self, channel: int, immediate: bool = True) -> bool:
|
||||
"""
|
||||
Stop motion
|
||||
|
||||
Args:
|
||||
channel: Channel number (1, 2, or 3), or 0 for all channels
|
||||
immediate: If True, stop immediately; if False, decelerate
|
||||
|
||||
Returns:
|
||||
bool: True if stop command sent successfully
|
||||
"""
|
||||
if channel == 0:
|
||||
# Stop all channels
|
||||
success = True
|
||||
for ch in [1, 2, 3]:
|
||||
if not self.stop(ch, immediate):
|
||||
success = False
|
||||
return success
|
||||
|
||||
if channel not in [1, 2, 3]:
|
||||
print(f"ERROR: Invalid channel number: {channel}")
|
||||
return False
|
||||
|
||||
print(f"DEBUG: Stopping channel {channel}")
|
||||
|
||||
cmd = self.protocol.cmd_move_stop(channel, immediate)
|
||||
return self._send_command(cmd)
|
||||
|
||||
# ==================== Parameter Setting ====================
|
||||
|
||||
def set_velocity_params(self, channel: int, max_vel_mm_s: float,
|
||||
accel_mm_s2: float) -> bool:
|
||||
"""
|
||||
Set velocity and acceleration parameters
|
||||
|
||||
Args:
|
||||
channel: Channel number (1, 2, or 3)
|
||||
max_vel_mm_s: Maximum velocity in mm/s
|
||||
accel_mm_s2: Acceleration in mm/s²
|
||||
|
||||
Returns:
|
||||
bool: True if parameters set successfully
|
||||
"""
|
||||
if channel not in [1, 2, 3]:
|
||||
print(f"ERROR: Invalid channel number: {channel}")
|
||||
return False
|
||||
|
||||
print(f"DEBUG: Setting velocity params for channel {channel}: "
|
||||
f"vel={max_vel_mm_s} mm/s, accel={accel_mm_s2} mm/s²")
|
||||
|
||||
cmd = self.protocol.cmd_set_velocity_params(channel, max_vel_mm_s, accel_mm_s2)
|
||||
if self._send_command(cmd):
|
||||
time.sleep(0.1) # Wait for controller to process
|
||||
|
||||
# Request status update to confirm parameters were accepted
|
||||
self.request_status_update(channel)
|
||||
time.sleep(0.1) # Wait for status response
|
||||
return True
|
||||
return False
|
||||
|
||||
# ==================== Status and Position ====================
|
||||
|
||||
def get_position(self, channel: int) -> Optional[float]:
|
||||
"""
|
||||
Get current position of channel
|
||||
|
||||
Args:
|
||||
channel: Channel number (1, 2, or 3)
|
||||
|
||||
Returns:
|
||||
float: Current position in mm, or None if unavailable
|
||||
"""
|
||||
if channel not in [1, 2, 3]:
|
||||
return None
|
||||
|
||||
return self.channels[channel].position_mm
|
||||
|
||||
def get_channel_status(self, channel: int) -> Optional[Dict]:
|
||||
"""
|
||||
Get detailed status of channel
|
||||
|
||||
Args:
|
||||
channel: Channel number (1, 2, or 3)
|
||||
|
||||
Returns:
|
||||
dict: Channel status dictionary
|
||||
"""
|
||||
if channel not in [1, 2, 3]:
|
||||
return None
|
||||
|
||||
ch = self.channels[channel]
|
||||
return {
|
||||
'channel': channel,
|
||||
'enabled': ch.enabled,
|
||||
'homed': ch.homed,
|
||||
'homing': ch.homing,
|
||||
'moving': ch.moving,
|
||||
'error': ch.error,
|
||||
'ready': ch.is_ready(),
|
||||
'position_mm': ch.position_mm,
|
||||
'encoder_count': ch.encoder_count,
|
||||
'status_bits': ch.status_bits
|
||||
}
|
||||
|
||||
def request_status_update(self, channel: int) -> bool:
|
||||
"""
|
||||
Request immediate status update for channel
|
||||
|
||||
Args:
|
||||
channel: Channel number (1, 2, or 3)
|
||||
|
||||
Returns:
|
||||
bool: True if request sent successfully
|
||||
"""
|
||||
if channel not in [1, 2, 3]:
|
||||
return False
|
||||
|
||||
cmd = self.protocol.cmd_req_status_update(channel)
|
||||
return self._send_command(cmd)
|
||||
|
||||
# ==================== Callbacks ====================
|
||||
|
||||
def register_move_complete_callback(self, channel: int, callback: Callable):
|
||||
"""Register callback for move complete event"""
|
||||
if channel in [1, 2, 3]:
|
||||
self._move_complete_callbacks[channel].append(callback)
|
||||
|
||||
def register_home_complete_callback(self, channel: int, callback: Callable):
|
||||
"""Register callback for home complete event"""
|
||||
if channel in [1, 2, 3]:
|
||||
self._home_complete_callbacks[channel].append(callback)
|
||||
|
||||
# ==================== Trigger Configuration ====================
|
||||
|
||||
def set_trigger_mode(self, channel: int, mode: int, polarity: int = 0x01,
|
||||
start_pos_fwd: float = 0.0, start_pos_rev: float = 0.0,
|
||||
interval_fwd: float = 0.0, interval_rev: float = 0.0) -> bool:
|
||||
"""
|
||||
Set trigger configuration for a channel
|
||||
|
||||
Args:
|
||||
channel: Channel number (1, 2, or 3)
|
||||
mode: Trigger mode (TriggerMode enum value)
|
||||
polarity: Trigger polarity (0x01 = active high, 0x02 = active low)
|
||||
start_pos_fwd: Start position for forward trigger (mm)
|
||||
start_pos_rev: Start position for reverse trigger (mm)
|
||||
interval_fwd: Interval for forward trigger (mm)
|
||||
interval_rev: Interval for reverse trigger (mm)
|
||||
|
||||
Returns:
|
||||
bool: True if trigger configuration set successfully
|
||||
|
||||
Example:
|
||||
# Disable trigger
|
||||
driver.set_trigger_mode(1, TriggerMode.DISABLED)
|
||||
|
||||
# Enable trigger output on motion
|
||||
driver.set_trigger_mode(1, TriggerMode.OUT_ONLY)
|
||||
|
||||
# Trigger at specific positions
|
||||
driver.set_trigger_mode(1, TriggerMode.OUT_POSITION,
|
||||
start_pos_fwd=10.0, interval_fwd=1.0)
|
||||
"""
|
||||
if channel not in [1, 2, 3]:
|
||||
print(f"ERROR: Invalid channel number: {channel}")
|
||||
return False
|
||||
|
||||
print(f"DEBUG: Setting trigger mode for channel {channel}: mode={mode}")
|
||||
|
||||
cmd = self.protocol.cmd_set_trigger(
|
||||
channel, mode, polarity, start_pos_fwd, start_pos_rev,
|
||||
interval_fwd, interval_rev
|
||||
)
|
||||
|
||||
if self._send_command(cmd):
|
||||
time.sleep(0.1) # Wait for controller to process
|
||||
return True
|
||||
return False
|
||||
|
||||
def get_trigger_config(self, channel: int) -> Optional[Dict]:
|
||||
"""
|
||||
Get current trigger configuration for a channel
|
||||
|
||||
Args:
|
||||
channel: Channel number (1, 2, or 3)
|
||||
|
||||
Returns:
|
||||
dict: Trigger configuration or None if unavailable
|
||||
"""
|
||||
if channel not in [1, 2, 3]:
|
||||
return None
|
||||
|
||||
cmd = self.protocol.cmd_req_trigger(channel)
|
||||
if not self._send_command(cmd):
|
||||
return None
|
||||
|
||||
# Note: In a complete implementation, would wait for response
|
||||
# For now, returning None as response handling would need queue
|
||||
print("WARNING: get_trigger_config not fully implemented (requires response queue)")
|
||||
return None
|
||||
|
||||
# ==================== Digital I/O ====================
|
||||
|
||||
def set_digital_outputs(self, output_bits: int) -> bool:
|
||||
"""
|
||||
Set digital output states
|
||||
|
||||
Args:
|
||||
output_bits: Bit pattern for outputs (0x00 to 0xFF)
|
||||
|
||||
Returns:
|
||||
bool: True if digital outputs set successfully
|
||||
|
||||
Note:
|
||||
Digital outputs share pins with trigger outputs. Ensure
|
||||
trigger mode is disabled before using digital outputs.
|
||||
"""
|
||||
if not (0 <= output_bits <= 0xFF):
|
||||
print(f"ERROR: Invalid output bits: {output_bits}")
|
||||
return False
|
||||
|
||||
print(f"DEBUG: Setting digital outputs: 0x{output_bits:02X}")
|
||||
|
||||
cmd = self.protocol.cmd_set_digital_outputs(output_bits)
|
||||
if self._send_command(cmd):
|
||||
time.sleep(0.05)
|
||||
return True
|
||||
return False
|
||||
Reference in New Issue
Block a user