import time from logger import get_logger # Constants for state management STATE_SEARCHING = 0 STATE_ALIGNING = 1 STATE_FOLLOWING = 2 STATE_RIGHT_TURNING = 3 STATE_LEFT_TURNING = 4 # alignment parameters DESIRED_DISTANCE_FROM_WALL = 250 # mm (200 seems to work very well) SIDE_ALIGNMENT_TOLERANCE = 20 # mm (for side distance correction) ALIGNMENT_ERROR_THRESHOLD = 50 # mm (for angle correction) STRAFE_SPEED = 30 # mm/s CORRECTION_AMOUNT = 2 # direction only, gets clamped anyway # Front obstacle detection FRONT_OBSTACLE_THRESHOLD = 400 # mm CORRECTION_TIME = 1.0 # seconds (alignment timing) FORWARD_SPEED = 30 # mm/s # Right turn params RIGHT_TURN_THRESHOLD = 500 # mm RIGHT_TURN_SPEED = 30 # mm/s TURN_RESET_DISTANCE = 500 # mm logger = get_logger("wall_follower") # πŸ’« Rotation helper using dumb timing def rotate_degrees(chassis, degrees): DEG_PER_SEC = 90 # confirmed from tuning test duration = abs(degrees) / DEG_PER_SEC direction = 1 if degrees > 0 else -1 logger.info(f"πŸ” Rotating {degrees}Β° ({'CW' if direction > 0 else 'CCW'}) for {duration:.2f}s") chassis.set_velocity(0, 0, direction) time.sleep(duration) chassis.reset_motors() def sanitize_reading(val): return val if isinstance(val, (int, float)) and val > 0 else 0 class WallFollower: def __init__(self, chassis): self.chassis = chassis self.state = STATE_SEARCHING self.last_alignment_time = time.time() self.realign_interval = 5.0 # seconds between realignment checks self._turning_phase_done = False # Used to track turn phases self.last_correction_time = 0 self.alignment_cooldown = 0.3 # seconds def stop(self): logger.info("πŸ›‘ WallFollower stopping motors.") self.chassis.reset_motors() def step(self, scan): front = sanitize_reading(scan[0]) # side = sanitize_reading(scan[90]) # diag_front = sanitize_reading(scan[45]) # diag_back = sanitize_reading(scan[135]) # if self.state == STATE_SEARCHING: self._searching(side) elif self.state == STATE_ALIGNING: self._aligning(side, diag_front, diag_back) elif self.state == STATE_FOLLOWING: self._following(front, diag_front, diag_back, side) elif self.state == STATE_RIGHT_TURNING: self._right_turning(diag_front, side) elif self.state == STATE_LEFT_TURNING: self._left_turning() def _searching(self, side_distance): logger.info("πŸ” SEARCHING for wall...") # Immediately transition to ALIGNING state for development/testing logger.info("βœ… Skipping search β€” switching to ALIGNING mode") self.state = STATE_ALIGNING self.last_alignment_time = time.time() def _aligning(self, side_distance, dist_45, dist_135): logger.info("πŸ“ ALIGNING to wall...") # Safety check if side_distance == 0 or dist_45 == 0 or dist_135 == 0: logger.warning("⚠️ Invalid LIDAR reading(s). Holding position.") #self.chassis.reset_motors() return # Calculate errors angle_error = dist_45 - dist_135 distance_error = side_distance - DESIRED_DISTANCE_FROM_WALL # πŸ”„ Step 1: Correct angle first if abs(angle_error) > ALIGNMENT_ERROR_THRESHOLD: logger.info(f"πŸ”„ Alignment error: {angle_error:.1f} mm") P_GAIN = 0.1 correction_degrees = P_GAIN * angle_error correction_degrees = max(min(correction_degrees, 2), -2) logger.debug(f"🎯 Rotating by {correction_degrees:.1f}Β° for alignment") rotate_degrees(self.chassis, correction_degrees) time.sleep(0.1) return # ↔️ Step 2: Then fix distance to wall if abs(distance_error) > SIDE_ALIGNMENT_TOLERANCE: if distance_error > 0: logger.debug("➑️ Strafe Right to correct wall distance") logger.debug(f"πŸ“ Distance: {side_distance:.1f} mm") logger.debug(f"πŸ“ Distance error: {distance_error:.1f} mm") self.chassis.set_velocity(STRAFE_SPEED, 0, 0) else: logger.debug("⬅️ Strafe Left to correct wall distance") self.chassis.set_velocity(-STRAFE_SPEED, 0, 0) time.sleep(0.3) self.chassis.reset_motors() time.sleep(0.2) return # βœ… If both are good logger.info("βœ… Aligned successfully β€” switching to FOLLOWING mode") self.state = STATE_FOLLOWING self.last_alignment_time = time.time() # βœ… If both are good logger.info("βœ… Aligned successfully β€” switching to FOLLOWING mode") self.state = STATE_FOLLOWING self.last_alignment_time = time.time() def _following(self, front_distance, dist_45, dist_135, side): time.sleep(1.0) # Allow LIDAR to settle logger.info("🚧 FOLLOWING state: driving forward...") # 🧭 Check for possible left-hand turn if 0 < front_distance < FRONT_OBSTACLE_THRESHOLD: logger.info("🧭 Potential left turn detected β€” entering LEFT_TURNING state") logger.debug(f"πŸ“ Front distance: {front_distance:.1f} mm") self.state = STATE_LEFT_TURNING return # πŸ” Timed angle + distance correction if time.time() - self.last_alignment_time > CORRECTION_TIME: if dist_45 > 0 and dist_135 > 0 and side > 0: angle_error = dist_45 - dist_135 distance_error = side - DESIRED_DISTANCE_FROM_WALL logger.info(f"πŸ› οΈ Periodic corrections | Angle error: {angle_error:.1f} mm | Distance error: {distance_error:.1f} mm") logger.debug(f"πŸ“ dist_45 = {dist_45} | dist_135 = {dist_135} | side = {side}") # πŸ“ Angle correction (rotation) if abs(angle_error) > ALIGNMENT_ERROR_THRESHOLD: P_GAIN = 0.05 correction_degrees = max(min(P_GAIN * angle_error, 5), -5) logger.debug(f"🎯 FOLLOWING correction: rotating by {correction_degrees:.2f}Β°") rotate_degrees(self.chassis, correction_degrees) # ➑️ Distance correction: strafe right if too far from wall if distance_error > SIDE_ALIGNMENT_TOLERANCE: logger.debug("πŸ“ FOLLOWING correction: strafing right to re-approach wall") self.chassis.set_velocity(STRAFE_SPEED, 0, 0) time.sleep(0.2) self.chassis.reset_motors() self.last_alignment_time = time.time() # 🧭 Check for possible right-hand turn if dist_45 > RIGHT_TURN_THRESHOLD and front_distance > FRONT_OBSTACLE_THRESHOLD: logger.info("🧭 Potential right turn detected β€” entering RIGHT_TURNING state") self.state = STATE_RIGHT_TURNING self.turn_stage = "checking_forward" return #if 0 < front_distance < FRONT_OBSTACLE_THRESHOLD: # logger.warning("πŸ›‘ Obstacle detected ahead! Stopping.") # self.chassis.reset_motors() # return # πŸš— Continue forward motion logger.debug("πŸš— Continuing forward after (or without) correction.") self.chassis.set_velocity(FORWARD_SPEED, 90, 0) def _right_turning(self, dist_45, dist_90): time.sleep(1.0) # Allow LIDAR to settle """ State 4: RIGHT_TURNING Handles logic for detecting and confirming a 90-degree right-hand turn. The robot first reverses until the 45Β° distance returns to normal, then moves forward slowly until both 45Β° and 90Β° are large, confirming a valid turn. It stops just past the corner. """ logger.info("β†ͺ️ RIGHT_TURNING state: verifying turn...") """ if self.turn_stage == "backing_up": logger.debug("πŸ”™ Backing up slowly to detect return of wall...") self.chassis.set_velocity(30, 270, 0) # reverse slowly if 0 < dist_45 < TURN_RESET_DISTANCE: logger.info("🧱 Wall detected again at 45Β° β€” switching to forward check") self.turn_stage = "checking_forward" time.sleep(0.5) """ """ if self.turn_stage == "checking_forward": logger.debug("πŸšΆβ€β™‚οΈ Moving forward slowly to confirm turn...") self.chassis.set_velocity(RIGHT_TURN_SPEED, 90, 0) logger.debug(f"πŸ“ˆ dist_45 = {dist_45} | dist_90 = {dist_90}") #if dist_45 > RIGHT_TURN_THRESHOLD and dist_90 > RIGHT_TURN_THRESHOLD: if dist_45 > RIGHT_TURN_THRESHOLD: logger.info("βœ… Verified right-hand turn β€” initiating reposition sequence") """ # Step 1: Drive forward to clear the corner self.chassis.set_velocity(RIGHT_TURN_SPEED, 90, 0) time.sleep(0.25) self.chassis.reset_motors() # Step 2: Rotate 90Β° to face next wall rotate_degrees(self.chassis, 70) # Step 3: Drive forward into new alignment zone self.chassis.set_velocity(RIGHT_TURN_SPEED, 90, 0) time.sleep(3.0) self.chassis.reset_motors() # After repositioning time.sleep(2.0) # Let LIDAR settle # Step 4: Switch to ALIGNING logger.info("πŸ“ Reposition complete β€” switching to ALIGNING state") self.state = STATE_ALIGNING # was previously set to STATE_ALIGNING self.last_alignment_time = time.time() def _left_turning(self): time.sleep(1.0) # Allow LIDAR to settle """ STATE_LEFT_TURNING β€” reverses, rotates left, then aligns. """ logger.info("↩️ LEFT_TURNING: initiating left turn sequence") # Step 1: Backup to clear corner logger.debug("πŸ”™ Reversing slightly before turning") self.chassis.set_velocity(RIGHT_TURN_SPEED, 270, 0) time.sleep(1.0) self.chassis.reset_motors() # Step 2: Rotate left (CCW) with extra compensation logger.info("πŸ” Rotating -100Β° (CCW) to face new wall") rotate_degrees(self.chassis, -95) # Step 3: Pause to let LIDAR stabilize logger.debug("⏸️ Pausing briefly after turn...") time.sleep(2.5) # Step 4: Switch to ALIGNING logger.info("πŸ“ Left turn complete β€” switching to ALIGNING") self.state = STATE_FOLLOWING # was previously set to STATE_ALIGNING self.last_alignment_time = time.time()