pre uc480 integration
This commit is contained in:
Thomas Ales [M S E]
@@ -0,0 +1,395 @@
|
||||
"""
|
||||
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'))
|
||||
Reference in New Issue
Block a user