Wayfinder/wall_follower.py

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 = 280  # 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 = 500    # mm
CORRECTION_TIME = 0.5               # seconds (alignment timing)
FORWARD_SPEED = 30                # mm/s

# Right turn params
RIGHT_TURN_THRESHOLD = 800        # mm
RIGHT_TURN_SPEED = 30             # mm/s
TURN_RESET_DISTANCE = 800         # 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[60]) # 
        diag_back = sanitize_reading(scan[120]) # 



        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.1
                    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 to maintain target distance
                if abs(distance_error) > SIDE_ALIGNMENT_TOLERANCE:
                    if distance_error > 0:
                        logger.debug("📏 FOLLOWING correction: strafing right (too far from wall)")
                        self.chassis.set_velocity(STRAFE_SPEED, 0, 0)
                    else:
                        logger.debug("📏 FOLLOWING correction: strafing left (too close to 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
        logger.debug("🚶‍♂️ Moving forward to clear 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
        logger.debug("🔁 Rotating 90° (CW) to face new wall")
        rotate_degrees(self.chassis, 70)

        # Step 3: Drive forward into new alignment zone
        logger.debug("🚶‍♂️ Moving forward to align with new wall")
        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_FOLLOWING # 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(0.5)
        self.chassis.reset_motors()

        # Step 2: Rotate left (CCW) with extra compensation
        logger.info("🔁 Rotating -90° (CCW) to face new wall")
        rotate_degrees(self.chassis, -90)

        #step3: Move foward to avoid angle errors
        logger.debug("🚘 Move foward to avoid angle errors")
        self.chassis.set_velocity(RIGHT_TURN_SPEED, 90, 0)
        time.sleep(1.0)
        self.chassis.reset_motors()

        # Step 3: Pause to let LIDAR stabilize
        logger.debug("⏸️ Pausing briefly after turn...")
        #time.sleep(1.0)

        # 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()