when'd i last commit this pos?

scanengine/motion_worker.py

@@ -0,0 +1,426 @@
+"""
+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'))