Thomas Ales
396 lines
14 KiB
Python
Executable File
396 lines
14 KiB
Python
Executable File
"""
|
|
Motion Controller Worker Thread
|
|
|
|
Handles all motion control operations in a separate thread to keep the UI responsive.
|
|
Provides async command queueing and position updates via Qt signals.
|
|
"""
|
|
|
|
from PyQt6 import QtCore
|
|
from hardware.pybbd202 import ThorlabsServoDriver, AXIS_X, AXIS_Y
|
|
import queue
|
|
import time
|
|
from typing import Optional, Dict, Any
|
|
|
|
|
|
class MotionCommand:
|
|
"""Represents a motion command"""
|
|
def __init__(self, cmd_type: str, **kwargs):
|
|
self.cmd_type = cmd_type
|
|
self.params = kwargs
|
|
|
|
|
|
class MotionWorker(QtCore.QObject):
|
|
"""
|
|
Worker object for handling motion control in a separate thread.
|
|
|
|
Signals:
|
|
connected: Emitted when controller connects successfully
|
|
disconnected: Emitted when controller disconnects
|
|
connection_failed: Emitted when connection fails (error_msg: str)
|
|
position_updated: Emitted when position changes (x: float, y: float)
|
|
homed_status: Emitted with home status (x_homed: bool, y_homed: bool)
|
|
move_completed: Emitted when a move completes (axis: str)
|
|
error_occurred: Emitted when an error occurs (error_msg: str)
|
|
"""
|
|
|
|
# Signals
|
|
connected = QtCore.pyqtSignal()
|
|
disconnected = QtCore.pyqtSignal()
|
|
connection_failed = QtCore.pyqtSignal(str)
|
|
position_updated = QtCore.pyqtSignal(float, float) # x, y in mm
|
|
homed_status = QtCore.pyqtSignal(bool, bool) # x_homed, y_homed
|
|
motion_status = QtCore.pyqtSignal(bool, bool) # x_moving, y_moving
|
|
move_completed = QtCore.pyqtSignal(str) # axis name
|
|
error_occurred = QtCore.pyqtSignal(str) # error message
|
|
|
|
def __init__(self):
|
|
super().__init__()
|
|
self.controller: Optional[ThorlabsServoDriver] = None
|
|
self.is_connected = False
|
|
self.command_queue = queue.Queue()
|
|
self.running = True
|
|
|
|
# Default parameters
|
|
self.jog_speed = 20.0 # mm/s
|
|
self.acceleration = 50.0 # mm/s^2
|
|
self.step_size = 1.0 # mm
|
|
|
|
# Position tracking
|
|
self.last_x = None
|
|
self.last_y = None
|
|
|
|
# Status tracking
|
|
self.last_x_homed = None
|
|
self.last_y_homed = None
|
|
self.last_x_moving = None
|
|
self.last_y_moving = None
|
|
|
|
# Position update throttling
|
|
self.last_position_update_time = 0
|
|
self.position_update_interval = 0.2 # seconds between position reads
|
|
|
|
# Flag to pause polling during scanning (scan worker handles its own position queries)
|
|
self.scanning_active = False
|
|
|
|
@QtCore.pyqtSlot()
|
|
def run(self):
|
|
"""Main worker loop - processes commands from queue"""
|
|
print("Motion worker thread started")
|
|
|
|
while self.running:
|
|
try:
|
|
# Check for commands with timeout to allow periodic position updates
|
|
try:
|
|
cmd = self.command_queue.get(timeout=0.05) # 50ms timeout
|
|
self.process_command(cmd)
|
|
except queue.Empty:
|
|
pass
|
|
|
|
# Periodically update position and status if connected
|
|
# Skip updates during scanning - scan worker handles its own position queries
|
|
if self.is_connected and self.controller and not self.scanning_active:
|
|
self.update_position()
|
|
self.update_home_status()
|
|
self.update_motion_status()
|
|
|
|
except Exception as e:
|
|
print(f"Error in motion worker loop: {e}")
|
|
self.error_occurred.emit(str(e))
|
|
|
|
# Cleanup on exit
|
|
if self.controller:
|
|
try:
|
|
self.controller.disconnect()
|
|
except:
|
|
pass
|
|
|
|
print("Motion worker thread stopped")
|
|
|
|
def process_command(self, cmd: MotionCommand):
|
|
"""Process a motion command"""
|
|
try:
|
|
if cmd.cmd_type == 'connect':
|
|
self.do_connect()
|
|
elif cmd.cmd_type == 'disconnect':
|
|
self.do_disconnect()
|
|
elif cmd.cmd_type == 'jog':
|
|
self.do_jog(cmd.params['axis'], cmd.params['direction'])
|
|
elif cmd.cmd_type == 'home':
|
|
self.do_home(cmd.params['axis'])
|
|
elif cmd.cmd_type == 'set_velocity':
|
|
self.do_set_velocity(cmd.params['speed'], cmd.params['accel'])
|
|
elif cmd.cmd_type == 'set_step_size':
|
|
self.step_size = cmd.params['step_size']
|
|
elif cmd.cmd_type == 'set_axis_enable':
|
|
self.do_set_axis_enable(cmd.params['axis'], cmd.params['enabled'])
|
|
elif cmd.cmd_type == 'stop':
|
|
self.running = False
|
|
|
|
except Exception as e:
|
|
print(f"Error processing command {cmd.cmd_type}: {e}")
|
|
self.error_occurred.emit(f"Command '{cmd.cmd_type}' failed: {str(e)}")
|
|
|
|
def do_connect(self):
|
|
"""Connect to the motion controller"""
|
|
try:
|
|
self.controller = ThorlabsServoDriver()
|
|
self.controller.connect()
|
|
|
|
# Enable channels
|
|
self.controller.enable_axis(AXIS_X)
|
|
self.controller.enable_axis(AXIS_Y)
|
|
|
|
# Start polling to populate cached state (positions, homed, moving, errors)
|
|
self.controller.start_polling(interval=0.2)
|
|
|
|
# Wait for first polling cycle to populate status
|
|
time.sleep(0.3)
|
|
|
|
# Set initial velocity parameters
|
|
for dest in [AXIS_X, AXIS_Y]:
|
|
self.controller.set_velocity_params(
|
|
dest,
|
|
max_velocity=self.jog_speed,
|
|
acceleration=self.acceleration
|
|
)
|
|
|
|
self.is_connected = True
|
|
# Force initial updates (they will be emitted because last values are None)
|
|
self.update_position()
|
|
self.update_home_status()
|
|
self.update_motion_status()
|
|
self.connected.emit()
|
|
|
|
print("Motion controller connected successfully")
|
|
|
|
except Exception as e:
|
|
print(f"Failed to connect to motion controller: {e}")
|
|
self.connection_failed.emit(str(e))
|
|
|
|
def do_disconnect(self):
|
|
"""Disconnect from the motion controller"""
|
|
if self.controller:
|
|
try:
|
|
self.controller.disconnect()
|
|
print("Motion controller disconnected")
|
|
except Exception as e:
|
|
print(f"Error during disconnect: {e}")
|
|
|
|
self.controller = None
|
|
self.is_connected = False
|
|
self.disconnected.emit()
|
|
|
|
def do_jog(self, axis: str, direction: int):
|
|
"""Execute a jog move"""
|
|
if not self.is_connected or not self.controller:
|
|
return
|
|
|
|
try:
|
|
dest = AXIS_X if axis == 'x' else AXIS_Y
|
|
|
|
# Calculate relative distance
|
|
distance = self.step_size * direction
|
|
|
|
# Execute the move (blocking, with short timeout for continuous jogging)
|
|
self.controller.move_axis_relative(dest, distance, timeout=0.5)
|
|
|
|
# Update position
|
|
self.update_position()
|
|
|
|
self.move_completed.emit(axis)
|
|
|
|
except TimeoutError:
|
|
# Timeout is expected during continuous jog - don't report as error
|
|
pass
|
|
except Exception as e:
|
|
print(f"Jog error: {e}")
|
|
self.error_occurred.emit(f"Jog failed: {str(e)}")
|
|
|
|
def do_home(self, axis: str):
|
|
"""Home an axis"""
|
|
if not self.is_connected or not self.controller:
|
|
return
|
|
|
|
try:
|
|
dest = AXIS_X if axis == 'x' else AXIS_Y
|
|
|
|
print(f"Homing {axis.upper()} axis...")
|
|
self.controller.home_axis(dest, timeout=60.0)
|
|
|
|
# Update position and status after homing
|
|
self.update_position()
|
|
self.update_home_status()
|
|
|
|
print(f"{axis.upper()} axis homed successfully")
|
|
|
|
except TimeoutError:
|
|
print(f"Home timeout: {axis.upper()} axis")
|
|
self.error_occurred.emit(f"Homing {axis.upper()} timed out")
|
|
except Exception as e:
|
|
print(f"Home error: {e}")
|
|
self.error_occurred.emit(f"Homing {axis.upper()} failed: {str(e)}")
|
|
|
|
def do_set_velocity(self, speed: float, accel: float):
|
|
"""Set velocity parameters"""
|
|
if not self.is_connected or not self.controller:
|
|
self.jog_speed = speed
|
|
self.acceleration = accel
|
|
return
|
|
|
|
try:
|
|
self.jog_speed = speed
|
|
self.acceleration = accel
|
|
|
|
for dest in [AXIS_X, AXIS_Y]:
|
|
self.controller.set_velocity_params(
|
|
dest,
|
|
max_velocity=self.jog_speed,
|
|
acceleration=self.acceleration
|
|
)
|
|
|
|
except Exception as e:
|
|
print(f"Set velocity error: {e}")
|
|
|
|
def do_set_axis_enable(self, axis: str, enabled: bool):
|
|
"""Enable or disable an axis for manual movement"""
|
|
if not self.is_connected or not self.controller:
|
|
return
|
|
|
|
try:
|
|
dest = AXIS_X if axis == 'x' else AXIS_Y
|
|
if enabled:
|
|
self.controller.enable_axis(dest)
|
|
else:
|
|
self.controller.disable_axis(dest)
|
|
state_str = "enabled" if enabled else "disabled"
|
|
print(f"{axis.upper()} axis {state_str}")
|
|
|
|
except Exception as e:
|
|
print(f"Set axis enable error: {e}")
|
|
self.error_occurred.emit(f"Failed to {'enable' if enabled else 'disable'} {axis.upper()} axis: {str(e)}")
|
|
|
|
def update_position(self):
|
|
"""Update current position and emit signal if changed"""
|
|
if not self.is_connected or not self.controller:
|
|
return
|
|
|
|
# Throttle position reads to avoid excessive signal emission
|
|
current_time = time.time()
|
|
if current_time - self.last_position_update_time < self.position_update_interval:
|
|
return
|
|
self.last_position_update_time = current_time
|
|
|
|
try:
|
|
# Read cached positions (populated by polling worker)
|
|
x_pos = self.controller.positions[0]
|
|
y_pos = self.controller.positions[1]
|
|
|
|
# Always emit on first update, or if position changed significantly (> 0.001mm)
|
|
if (self.last_x is None or self.last_y is None or
|
|
abs(x_pos - self.last_x) > 0.001 or abs(y_pos - self.last_y) > 0.001):
|
|
self.last_x = x_pos
|
|
self.last_y = y_pos
|
|
print(f"Position update: X={x_pos:.3f}mm, Y={y_pos:.3f}mm")
|
|
self.position_updated.emit(x_pos, y_pos)
|
|
|
|
except Exception as e:
|
|
print(f"Error updating position: {e}")
|
|
import traceback
|
|
traceback.print_exc()
|
|
|
|
def update_home_status(self):
|
|
"""Update home status and emit signal if changed"""
|
|
if not self.is_connected or not self.controller:
|
|
return
|
|
|
|
try:
|
|
x_homed = self.controller.am_homed[0]
|
|
y_homed = self.controller.am_homed[1]
|
|
|
|
# Only emit if status changed
|
|
if x_homed != self.last_x_homed or y_homed != self.last_y_homed:
|
|
self.last_x_homed = x_homed
|
|
self.last_y_homed = y_homed
|
|
self.homed_status.emit(x_homed, y_homed)
|
|
|
|
except Exception as e:
|
|
print(f"Error updating home status: {e}")
|
|
import traceback
|
|
traceback.print_exc()
|
|
|
|
def update_motion_status(self):
|
|
"""Update motion status and emit signal if changed.
|
|
|
|
The new driver's polling worker keeps am_moving[], am_error[]
|
|
up to date automatically via status update messages.
|
|
"""
|
|
if not self.is_connected or not self.controller:
|
|
return
|
|
|
|
try:
|
|
# Check for any error conditions
|
|
if self.controller.am_error[0]:
|
|
self.error_occurred.emit("X-axis error detected")
|
|
if self.controller.am_error[1]:
|
|
self.error_occurred.emit("Y-axis error detected")
|
|
|
|
# Read cached motion status (updated by polling worker)
|
|
x_moving = self.controller.am_moving[0]
|
|
y_moving = self.controller.am_moving[1]
|
|
|
|
# Only emit if status changed
|
|
if x_moving != self.last_x_moving or y_moving != self.last_y_moving:
|
|
self.last_x_moving = x_moving
|
|
self.last_y_moving = y_moving
|
|
self.motion_status.emit(x_moving, y_moving)
|
|
|
|
except Exception as e:
|
|
print(f"Error updating motion status: {e}")
|
|
import traceback
|
|
traceback.print_exc()
|
|
|
|
# Slot methods for queuing commands
|
|
@QtCore.pyqtSlot()
|
|
def queue_connect(self):
|
|
"""Queue a connect command"""
|
|
self.command_queue.put(MotionCommand('connect'))
|
|
|
|
@QtCore.pyqtSlot()
|
|
def queue_disconnect(self):
|
|
"""Queue a disconnect command"""
|
|
self.command_queue.put(MotionCommand('disconnect'))
|
|
|
|
@QtCore.pyqtSlot(str, int)
|
|
def queue_jog(self, axis: str, direction: int):
|
|
"""Queue a jog command"""
|
|
self.command_queue.put(MotionCommand('jog', axis=axis, direction=direction))
|
|
|
|
@QtCore.pyqtSlot(str)
|
|
def queue_home(self, axis: str):
|
|
"""Queue a home command"""
|
|
self.command_queue.put(MotionCommand('home', axis=axis))
|
|
|
|
@QtCore.pyqtSlot(float, float)
|
|
def queue_set_velocity(self, speed: float, accel: float):
|
|
"""Queue a set velocity command"""
|
|
self.command_queue.put(MotionCommand('set_velocity', speed=speed, accel=accel))
|
|
|
|
@QtCore.pyqtSlot(float)
|
|
def queue_set_step_size(self, step_size: float):
|
|
"""Queue a set step size command"""
|
|
self.command_queue.put(MotionCommand('set_step_size', step_size=step_size))
|
|
|
|
@QtCore.pyqtSlot(str, bool)
|
|
def queue_set_axis_enable(self, axis: str, enabled: bool):
|
|
"""Queue a command to enable or disable an axis"""
|
|
self.command_queue.put(MotionCommand('set_axis_enable', axis=axis, enabled=enabled))
|
|
|
|
@QtCore.pyqtSlot()
|
|
def stop(self):
|
|
"""Stop the worker thread"""
|
|
# Set running to False immediately so the main loop can exit
|
|
# even if it's blocked waiting for a response from the controller
|
|
self.running = False
|
|
# Also queue a stop command to ensure the command_queue.get() returns
|
|
self.command_queue.put(MotionCommand('stop'))
|