""" 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.bbd202 import MotionController 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[MotionController] = 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 (active requests can be slow) self.last_position_update_time = 0 self.position_update_interval = 0.2 # seconds between position requests # 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 = MotionController() self.controller.connect() # ACKs are sent reactively when enable_updates=True # Enable channels self.controller.set_channel_enable_state(self.controller.DEST_X_AXIS, True) self.controller.set_channel_enable_state(self.controller.DEST_Y_AXIS, True) # Request status updates for both axes to populate status bits (including homed state) # This is necessary because status bits are not sent automatically after connect self.controller.request_status_update(self.controller.DEST_X_AXIS) self.controller.request_status_update(self.controller.DEST_Y_AXIS) # Wait a moment for the asynchronous status update responses time.sleep(0.2) # Set initial velocity parameters for dest in [self.controller.DEST_X_AXIS, self.controller.DEST_Y_AXIS]: self.controller.set_velocity_params( dest, min_velocity=0.0, acceleration=self.acceleration, max_velocity=self.jog_speed ) 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: # Determine destination dest = self.controller.DEST_X_AXIS if axis == 'x' else self.controller.DEST_Y_AXIS # Calculate relative distance distance = self.step_size * direction # Set relative move parameters self.controller.set_move_rel_params(dest, distance) # Execute the move (non-blocking - we don't wait for completion) # Use a very short timeout since we're doing continuous jogging self.controller.move_relative(dest, timeout=0.5) # Update position self.update_position() self.move_completed.emit(axis) except Exception as e: # Don't emit errors for timeout - that's expected during continuous jog if "timeout" not in str(e).lower(): 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 = self.controller.DEST_X_AXIS if axis == 'x' else self.controller.DEST_Y_AXIS 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 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 [self.controller.DEST_X_AXIS, self.controller.DEST_Y_AXIS]: self.controller.set_velocity_params( dest, min_velocity=0.0, acceleration=self.acceleration, max_velocity=self.jog_speed ) 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 = self.controller.DEST_X_AXIS if axis == 'x' else self.controller.DEST_Y_AXIS self.controller.set_channel_enable_state(dest, enabled) 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 requests to avoid slowing down the main loop 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: # Actively request positions from the controller instead of relying on cached values # This ensures we always have up-to-date position data # Use longer timeout (1.5s) to accommodate high-speed scanning at 200mm/s x_pos = self.controller.get_position(self.controller.DEST_X_AXIS, timeout=1.5) y_pos = self.controller.get_position(self.controller.DEST_Y_AXIS, timeout=1.5) if x_pos is not None and y_pos is not None: # 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) # else: silently skip incomplete position data during busy scanning except RuntimeError as e: # RuntimeError indicates actual hardware error (overtemp, encoder fault, etc.) print(f"CRITICAL: Motor error detected: {e}") self.error_occurred.emit(str(e)) # Stop requesting position updates to avoid spam self.is_connected = False 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.is_homed_x y_homed = self.controller.is_homed_y # 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. NOTE: On BBD202 firmware v2.1.5, the is_in_motion_x/y properties do NOT reliably detect motion - they may always return False even during active movement. This is a known firmware limitation. For scan execution, use the ScanWorker.move_and_wait() method which uses position-based motion detection instead of status bits. This UI status display is best-effort only. """ if not self.is_connected or not self.controller: return try: # Actively poll for status updates self.controller.poll_status() # Check for any error conditions error_msg = self.controller.check_for_errors() if error_msg: print(f"Motor error detected: {error_msg}") self.error_occurred.emit(error_msg) self.controller.clear_last_error() # Read the cached status (may not accurately reflect motion on some firmware) x_moving = self.controller.is_in_motion_x y_moving = self.controller.is_in_motion_y # Also check if there are pending moves (more reliable) if self.controller.is_move_pending(): # If there are pending moves, we're likely still moving # This provides a backup indication when status bits fail pending = self.controller.get_pending_targets() if self.controller.DEST_X_AXIS in pending: x_moving = True if self.controller.DEST_Y_AXIS in pending: y_moving = True # 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'))