""" 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=20.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'))