import time import threading from vision import calculate_focus_score_sobel class AutofocusController: """Manages autofocus operations""" # Default timing settings DEFAULT_SETTINGS = { 'sweep_move_time': 0.5, 'sweep_settle_time': 0.05, 'sweep_samples': 10, 'sweep_steps': 30, 'fine_move_time': 0.15, 'fine_settle_time': 0.1, 'fine_samples': 10, 'fine_max_no_improvement': 5, } def __init__(self, camera, motion_controller, on_log=None, on_focus_update=None): """ Args: camera: Camera instance for capturing frames motion_controller: MotionController for Z axis control on_log: Callback(message) for logging on_focus_update: Callback(score) for updating focus display """ self.camera = camera self.motion = motion_controller self.on_log = on_log self.on_focus_update = on_focus_update self.running = False self.settings = self.DEFAULT_SETTINGS.copy() self._thread = None def log(self, message): """Log a message""" if self.on_log: self.on_log(message) def update_focus_display(self, score): """Update focus display""" if self.on_focus_update: self.on_focus_update(score) # === Focus Measurement === def get_focus_score(self): """Get current focus score from camera""" try: frame = self.camera.capture_frame() return calculate_focus_score_sobel(frame) except Exception as e: self.log(f"Focus score error: {e}") return 0 def get_averaged_focus(self, samples=5, delay_between=0.05): """ Get averaged focus score from multiple samples. Removes outliers for more stable readings. """ scores = [] for _ in range(samples): scores.append(self.get_focus_score()) time.sleep(delay_between) # Remove outliers (top and bottom 20%) scores.sort() trim = len(scores) // 5 if trim > 0: scores = scores[trim:-trim] return sum(scores) / len(scores) if scores else 0 # === Autofocus Control === def start(self, speed_range=(1, 5), coarse=False, fine=False): """Start autofocus in background thread""" if self.running: self.log("Autofocus already running") return False # Determine speed range based on mode if coarse: speed_range = (4, 5) elif fine: speed_range = (1, 3) self.running = True self._thread = threading.Thread( target=self._autofocus_routine, args=(speed_range,), daemon=True ) self._thread.start() return True def stop(self): """Stop autofocus routine""" self.running = False self.motion.stop_z() self.log("Autofocus stopped") def is_running(self): """Check if autofocus is currently running""" return self.running # === Autofocus Algorithm === def _autofocus_routine(self, speed_range): """ Autofocus using sweep search + hill climbing. Phase 1: Sweep across range to find approximate peak Phase 2: Fine tune from best position found """ self.log(f"Starting autofocus (speed range: {speed_range})") min_speed_idx, max_speed_idx = speed_range s = self.settings # Shorthand try: # Phase 1: Sweep search best_step, best_score = self._sweep_search( min_speed_idx, max_speed_idx, s ) if not self.running: return # Move to best position found self._move_to_best_position(best_step, s) if not self.running: return # Phase 2: Fine tuning final_score = self._fine_tune(min_speed_idx, s) self.log(f"Autofocus complete. Final: {final_score:.1f}") except Exception as e: self.log(f"Autofocus error: {e}") finally: self.running = False def _sweep_search(self, min_speed_idx, max_speed_idx, s): """Phase 1: Sweep to find approximate best position""" self.log("Phase 1: Sweep search") # Use medium speed for sweep sweep_speed = (min_speed_idx + max_speed_idx) // 2 self.motion.set_speed(sweep_speed) time.sleep(0.1) # Record starting position time.sleep(s['sweep_settle_time']) start_score = self.get_averaged_focus(samples=s['sweep_samples']) self.update_focus_display(start_score) sweep_data = [(0, start_score)] self.log(f"Start position: {start_score:.1f}") if not self.running: return 0, start_score # Sweep DOWN first (away from slide to avoid contact) self.log(f"Sweeping down {s['sweep_steps']} steps...") for i in range(1, s['sweep_steps'] + 1): if not self.running: return 0, start_score self.motion.move_z_down() time.sleep(s['sweep_move_time']) self.motion.stop_z() time.sleep(s['sweep_settle_time']) score = self.get_averaged_focus(samples=s['sweep_samples']) sweep_data.append((-i, score)) self.update_focus_display(score) if i % 5 == 0: self.log(f" Step -{i}: {score:.1f}") if not self.running: return 0, start_score # Return to start self.log("Returning to start...") for _ in range(s['sweep_steps']): if not self.running: return 0, start_score self.motion.move_z_up() time.sleep(s['sweep_move_time']) self.motion.stop_z() time.sleep(0.1) time.sleep(s['sweep_settle_time']) if not self.running: return 0, start_score # Sweep UP self.log(f"Sweeping up {s['sweep_steps']} steps...") for i in range(1, s['sweep_steps'] + 1): if not self.running: return 0, start_score self.motion.move_z_up() time.sleep(s['sweep_move_time']) self.motion.stop_z() time.sleep(s['sweep_settle_time']) score = self.get_averaged_focus(samples=s['sweep_samples']) sweep_data.append((i, score)) self.update_focus_display(score) if i % 5 == 0: self.log(f" Step +{i}: {score:.1f}") # Find best position best_step, best_score = max(sweep_data, key=lambda x: x[1]) self.log(f"Best found at step {best_step}: {best_score:.1f}") # Log sweep curve sorted_data = sorted(sweep_data, key=lambda x: x[0]) curve_str = " ".join([f"{d[1]:.0f}" for d in sorted_data]) self.log(f"Sweep curve: {curve_str}") return best_step, best_score def _move_to_best_position(self, best_step, s): """Move from current position (+SWEEP_STEPS) to best_step""" steps_to_move = s['sweep_steps'] - best_step if steps_to_move > 0: self.log(f"Moving down {steps_to_move} steps to best position") move_func = self.motion.move_z_down else: self.log(f"Moving up {abs(steps_to_move)} steps to best position") move_func = self.motion.move_z_up steps_to_move = abs(steps_to_move) for _ in range(steps_to_move): if not self.running: return move_func() time.sleep(s['sweep_move_time']) self.motion.stop_z() time.sleep(0.1) time.sleep(s['sweep_settle_time']) current_score = self.get_averaged_focus(samples=s['sweep_samples']) self.log(f"At best position: {current_score:.1f}") def _fine_tune(self, min_speed_idx, s): """Phase 2: Fine hill-climbing from best position""" self.log("Phase 2: Fine tuning") self.motion.set_speed(min_speed_idx) time.sleep(0.1) best_score = self.get_averaged_focus(samples=s['fine_samples']) # Determine fine direction by testing both fine_direction = self._determine_fine_direction(best_score, s) if not self.running: return best_score # Fine search best_score, best_position_offset = self._fine_search( fine_direction, best_score, s ) if not self.running: return best_score # Return to best position if best_position_offset > 0: self._return_to_best(fine_direction, best_position_offset, s) # Final reading time.sleep(s['fine_settle_time']) final_score = self.get_averaged_focus(samples=s['fine_samples']) self.update_focus_display(final_score) return final_score def _determine_fine_direction(self, best_score, s): """Test both directions to find which improves focus""" # Try DOWN first self.motion.move_z_down() time.sleep(s['fine_move_time']) self.motion.stop_z() time.sleep(s['fine_settle_time']) down_score = self.get_averaged_focus(samples=s['fine_samples']) if down_score > best_score: self.log(f"Fine direction: DOWN ({down_score:.1f})") return 'down' # Go back and try UP self.motion.move_z_up() time.sleep(s['fine_move_time']) self.motion.stop_z() time.sleep(s['fine_settle_time']) self.motion.move_z_up() time.sleep(s['fine_move_time']) self.motion.stop_z() time.sleep(s['fine_settle_time']) up_score = self.get_averaged_focus(samples=s['fine_samples']) if up_score > best_score: self.log(f"Fine direction: UP ({up_score:.1f})") return 'up' # Already at peak self.log("Already at peak, minor adjustment only") self.motion.move_z_down() time.sleep(s['fine_move_time']) self.motion.stop_z() time.sleep(s['fine_settle_time']) return 'up' def _fine_search(self, direction, best_score, s): """Search in given direction until no improvement""" move_func = self.motion.move_z_up if direction == 'up' else self.motion.move_z_down no_improvement_count = 0 best_position_offset = 0 while self.running and no_improvement_count < s['fine_max_no_improvement']: move_func() time.sleep(s['fine_move_time']) self.motion.stop_z() time.sleep(s['fine_settle_time']) current_score = self.get_averaged_focus(samples=s['fine_samples']) self.update_focus_display(current_score) if current_score > best_score: best_score = current_score no_improvement_count = 0 best_position_offset = 0 self.log(f"Fine better: {current_score:.1f}") else: no_improvement_count += 1 best_position_offset += 1 return best_score, best_position_offset def _return_to_best(self, direction, steps, s): """Return to best position by reversing direction""" self.log(f"Returning {steps} steps") reverse_func = self.motion.move_z_down if direction == 'up' else self.motion.move_z_up for _ in range(steps): reverse_func() time.sleep(s['fine_move_time']) self.motion.stop_z() time.sleep(0.1)