# Sensor Navigation rev_01

/********* Pleasedontcode.com **********

    Pleasedontcode thanks you for automatic code generation! Enjoy your code!

    - Terms and Conditions:
    You have a non-exclusive, revocable, worldwide, royalty-free license
    for personal and commercial use. Attribution is optional; modifications
    are allowed, but you're responsible for code maintenance. We're not
    liable for any loss or damage. For full terms,
    please visit pleasedontcode.com/termsandconditions.

    - Project: # Sensor Navigation
    - Version: 001
    - Source Code NOT compiled for: Arduino Mega
    - Source Code created on: 2026-03-10 05:35:10

********* Pleasedontcode.com **********/

/****** SYSTEM REQUIREMENTS *****/
/****** SYSTEM REQUIREMENT 1 *****/
    /* Read 5 IR sensors (Left, FrontLeft, Center, */
    /* FrontRight, Right) to detect path edges and walls */
    /* using analog input */
/****** SYSTEM REQUIREMENT 2 *****/
    /* Use PID controller to calculate steering */
    /* correction based on sensor difference between left */
    /* and right sides to keep car centered on path */
/****** SYSTEM REQUIREMENT 3 *****/
    /* Control two DC motors (left and right) with PWM */
    /* speed and direction pins to execute steering and */
    /* forward movement */
/****** SYSTEM REQUIREMENT 4 *****/
    /* Display current sensor readings, PID error, and */
    /* motor speeds on 16x4 I2C LCD in real-time for */
    /* debugging */
/****** SYSTEM REQUIREMENT 5 *****/
    /* When front sensor detects wall (distance < 18cm), */
    /* car STOPS and scans all sensors 5 times with 100ms */
    /* delays. Only turn if side opening detected. */
    /* Diagonal (FL/FR) and side (L/R) sensors must both */
    /* confirm opening before turning. */
/****** SYSTEM REQUIREMENT 6 *****/
    /* During turns, reduce speed to 60 PWM and monitor */
    /* front sensor. If front clears during turn, stop */
    /* turning and resume wall-following. If wall */
    /* detected while turning, complete 90° turn smoothly */
    /* with timing compensation. */
/****** SYSTEM REQUIREMENT 7 *****/
    /* Car must maintain 8-12cm distance from walls while */
    /* moving. Center between walls if corridor width */
    /* allows. Use low PID gains (Kp=3) to prevent jerky */
    /* corrections that cause corner hits. */
/****** END SYSTEM REQUIREMENTS *****/


// ===================================================
// MAZE ROBOT – ADVANCED CORNER-SAFE NAVIGATION
// Features: Pre-turn scanning, dual-sensor validation
// Reduced PID Kp=3, Turn monitoring with early abort
// ===================================================

#include <Wire.h>
#include <LiquidCrystal_I2C.h>

LiquidCrystal_I2C lcd(0x27, 16, 4);

// -------- Sensors --------
#define IR_LEFT        A0   // 180° (left side)
#define IR_FRONTLEFT   A1   // 135° (front-left diagonal)
#define IR_FRONT       A2   // 0°   (straight ahead)
#define IR_FRONTRIGHT  A3   // 45°  (front-right diagonal)
#define IR_RIGHT       A4   // 90°  (right side)

// -------- Motors --------
#define ENA 5   // Right motor enable
#define IN1 8   // Right motor input 1
#define IN2 9   // Right motor input 2
#define ENB 6   // Left motor enable
#define IN3 10  // Left motor input 1
#define IN4 11  // Left motor input 2

// ===================================================
// ROBOT PARAMETERS - CORNER-SAFE TUNING
// ===================================================

// Speed settings
#define BASE_SPEED       80  // Normal cruising speed
#define MIN_SPEED        65  // Minimum speed during correction
#define CORNER_SPEED     65  // Speed at corners
#define TURN_SPEED       60  // Speed during turns (REDUCED for safety)

// Distance thresholds (in cm)
#define WALL_DETECT      20  // Distance to detect a wall
#define FRONT_CLEAR      18  // Front is clear if > this
#define FRONT_TURN       18  // Distance to prepare for turn
#define FRONT_STOP       20  // Emergency stop distance
#define WALL_IDEAL       8   // TARGET distance from side wall
#define WALL_DEADBAND    2   // Deadband for corrections
#define WALL_DANGER      4   // Dangerously close to wall
#define WALL_FAR         12  // Too far from wall
#define OPEN_SPACE       28  // Consider as open space

// PID constants - REDUCED FOR GENTLE CORRECTIONS
#define KP               3.0 // Proportional gain (REDUCED from 5 to 3 for smooth gentle corrections)
#define KD               2.0 // Derivative gain (REDUCED for smoothness)
#define KI               0.05 // Integral gain (REDUCED for smoothness)
#define CORRECTION_LIMIT 8   // Max correction per cycle (REDUCED from 10 to 8)

// Motor trim
#define LEFT_MOTOR_TRIM  0
#define RIGHT_MOTOR_TRIM 0

// Turn timing
#define TURN_90_TIME     450
#define TURN_PAUSE       200

// Pre-turn scanning parameters
#define PRE_TURN_SCANS   5   // Number of consecutive reads for noise elimination
#define SCAN_DELAY       100 // Milliseconds between scans (100ms as specified)

// Turn monitoring timeout
#define TURN_TIMEOUT     500 // Maximum milliseconds for a 90 degree turn
#define TURN_CHECK_INTERVAL 50 // Check front sensor every 50ms during turn

// ===================================================
// GLOBAL VARIABLES
// ===================================================

float L, FL, F, FR, R;
float filteredL, filteredR;

// Arrays for pre-turn scanning - stores multiple sensor readings
float scanL[PRE_TURN_SCANS];
float scanFL[PRE_TURN_SCANS];
float scanF[PRE_TURN_SCANS];
float scanFR[PRE_TURN_SCANS];
float scanR[PRE_TURN_SCANS];

// Averaged scan values
float avgL, avgFL, avgF, avgFR, avgR;

// PID variables
float wallError = 0;
float prevWallError = 0;
float wallIntegral = 0;
float prevCorrection = 0;
float correction = 0;

// Wall following state
int followingWall = 0;  // 0 = none, 1 = left wall, 2 = right wall, 3 = both
float wallDistance = 0;
float targetDistance = WALL_IDEAL;

// Status strings
String moveStr = "STOP";
String stateStr = "IDLE";
String turnStr = "-";
String wallStr = "NONE";
int currentLeftSpeed = 0;
int currentRightSpeed = 0;

// Turn decision variables
bool turning = false;
unsigned long turnStartTime = 0;
int turnDecision = 0;
bool frontClearedDuringTurn = false;

// Anti-stall protection
unsigned long lastMoveTime = 0;
int stallCounter = 0;

// ===================================================
// DISTANCE MEASUREMENT
// ===================================================

float getDist(int pin) {
  int adc = analogRead(pin);
  if (adc < 30) adc = 30;
  return 4800.0 / (adc - 20.0);
}

// ===================================================
// READ ALL SENSORS (SINGLE READ)
// ===================================================

void readAllSensors() {
  L  = getDist(IR_LEFT);
  FL = getDist(IR_FRONTLEFT);
  F  = getDist(IR_FRONT);
  FR = getDist(IR_FRONTRIGHT);
  R  = getDist(IR_RIGHT);

  // Simple filtering
  static float lastL = 0, lastR = 0;
  if (lastL == 0) {
    lastL = L;
    lastR = R;
  } else {
    L = (L + lastL) / 2;
    R = (R + lastR) / 2;
    lastL = L;
    lastR = R;
  }

  // Cap values for display
  if (L > 99) L = 99;
  if (FL > 99) FL = 99;
  if (F > 99) F = 99;
  if (FR > 99) FR = 99;
  if (R > 99) R = 99;
}

// ===================================================
// PRE-TURN SENSOR SCANNING (NEW)
// Performs 5 consecutive reads with 100ms delays
// to eliminate noise and confirm wall detection
// ===================================================

void performPreTurnScanning() {
  Serial.println("\n=== PRE-TURN SCANNING (Noise Elimination) ===");

  // Perform 5 consecutive sensor reads with 100ms delays
  for (int i = 0; i < PRE_TURN_SCANS; i++) {
    readAllSensors();

    // Store readings in arrays
    scanL[i] = L;
    scanFL[i] = FL;
    scanF[i] = F;
    scanFR[i] = FR;
    scanR[i] = R;

    Serial.print("Scan "); Serial.print(i + 1);
    Serial.print(": F="); Serial.print(F);
    Serial.print(" FL="); Serial.print(FL);
    Serial.print(" FR="); Serial.print(FR);
    Serial.print(" L="); Serial.print(L);
    Serial.print(" R="); Serial.println(R);

    // Delay 100ms between scans (except after last scan)
    if (i < PRE_TURN_SCANS - 1) {
      delay(SCAN_DELAY);
    }
  }

  // Calculate averages from the 5 scans
  avgL = 0, avgFL = 0, avgF = 0, avgFR = 0, avgR = 0;

  for (int i = 0; i < PRE_TURN_SCANS; i++) {
    avgL += scanL[i];
    avgFL += scanFL[i];
    avgF += scanF[i];
    avgFR += scanFR[i];
    avgR += scanR[i];
  }

  avgL /= PRE_TURN_SCANS;
  avgFL /= PRE_TURN_SCANS;
  avgF /= PRE_TURN_SCANS;
  avgFR /= PRE_TURN_SCANS;
  avgR /= PRE_TURN_SCANS;

  Serial.println("\n=== AVERAGED RESULTS (After noise elimination) ===");
  Serial.print("Avg F="); Serial.print(avgF);
  Serial.print(" Avg FL="); Serial.print(avgFL);
  Serial.print(" Avg FR="); Serial.print(avgFR);
  Serial.print(" Avg L="); Serial.print(avgL);
  Serial.print(" Avg R="); Serial.println(avgR);
}

// ===================================================
// DUAL-SENSOR VALIDATION (NEW)
// Requires BOTH diagonal (FL/FR) AND side (L/R) sensors
// to confirm opening before allowing turn
// ===================================================

bool validateOpeningDualSensor(bool leftTurn) {
  Serial.println("\n=== DUAL-SENSOR VALIDATION ===");

  if (leftTurn) {
    // For LEFT turn, check LEFT side
    // BOTH diagonal (FL) AND side (L) must confirm opening
    bool diagonalOpen = (avgFL > OPEN_SPACE);
    bool sideOpen = (avgL > OPEN_SPACE);

    Serial.print("LEFT TURN - Diagonal (FL)="); Serial.print(avgFL);
    Serial.print(" ["); Serial.print(diagonalOpen ? "OPEN" : "BLOCKED");
    Serial.print("], Side (L)="); Serial.print(avgL);
    Serial.print(" ["); Serial.print(sideOpen ? "OPEN" : "BLOCKED");
    Serial.println("]");

    bool valid = (diagonalOpen && sideOpen);
    Serial.print("LEFT TURN VALIDATION: ");
    Serial.println(valid ? "VALID - PROCEED" : "INVALID - CANCEL");
    return valid;
  } else {
    // For RIGHT turn, check RIGHT side
    // BOTH diagonal (FR) AND side (R) must confirm opening
    bool diagonalOpen = (avgFR > OPEN_SPACE);
    bool sideOpen = (avgR > OPEN_SPACE);

    Serial.print("RIGHT TURN - Diagonal (FR)="); Serial.print(avgFR);
    Serial.print(" ["); Serial.print(diagonalOpen ? "OPEN" : "BLOCKED");
    Serial.print("], Side (R)="); Serial.print(avgR);
    Serial.print(" ["); Serial.print(sideOpen ? "OPEN" : "BLOCKED");
    Serial.println("]");

    bool valid = (diagonalOpen && sideOpen);
    Serial.print("RIGHT TURN VALIDATION: ");
    Serial.println(valid ? "VALID - PROCEED" : "INVALID - CANCEL");
    return valid;
  }
}

// ===================================================
// MOTOR CONTROL
// ===================================================

void setMotorSpeeds(int leftSpeed, int rightSpeed) {
  currentLeftSpeed = leftSpeed;
  currentRightSpeed = rightSpeed;

  leftSpeed += LEFT_MOTOR_TRIM;
  rightSpeed += RIGHT_MOTOR_TRIM;

  // ANTI-STALL PROTECTION - NEVER go below MIN_SPEED when moving forward
  if (leftSpeed > 0 && leftSpeed < MIN_SPEED) leftSpeed = MIN_SPEED;
  if (rightSpeed > 0 && rightSpeed < MIN_SPEED) rightSpeed = MIN_SPEED;

  leftSpeed = constrain(leftSpeed, -255, 255);
  rightSpeed = constrain(rightSpeed, -255, 255);

  // Left motor
  if (leftSpeed >= 0) {
    digitalWrite(IN3, HIGH);
    digitalWrite(IN4, LOW);
    analogWrite(ENB, leftSpeed);
  } else {
    digitalWrite(IN3, LOW);
    digitalWrite(IN4, HIGH);
    analogWrite(ENB, -leftSpeed);
  }

  // Right motor
  if (rightSpeed >= 0) {
    digitalWrite(IN1, HIGH);
    digitalWrite(IN2, LOW);
    analogWrite(ENA, rightSpeed);
  } else {
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, HIGH);
    analogWrite(ENA, -rightSpeed);
  }

  // Update last move time for anti-stall
  if (leftSpeed != 0 || rightSpeed != 0) {
    lastMoveTime = millis();
  }
}

void stopCar() {
  setMotorSpeeds(0, 0);
  moveStr = "STOP";
}

void moveForward(int leftSpeed, int rightSpeed) {
  setMotorSpeeds(leftSpeed, rightSpeed);
  moveStr = "FORWARD";
}

// ===================================================
// ANTI-STALL CHECK
// ===================================================

void checkForStall() {
  // If we're trying to move but not making progress (detected by sensors)
  if (!turning && (currentLeftSpeed > 0 || currentRightSpeed > 0)) {

    // Check if we're stuck against a wall
    if (F < FRONT_STOP && currentLeftSpeed > 0 && currentRightSpeed > 0) {
      stallCounter++;
      if (stallCounter > 5) {
        Serial.println("STALL DETECTED - Backing up");
        // Back up a little
        setMotorSpeeds(-MIN_SPEED, -MIN_SPEED);
        delay(300);
        stopCar();
        delay(200);
        stallCounter = 0;
      }
    } else {
      stallCounter = 0;
    }
  }
}

// ===================================================
// TURN FUNCTIONS WITH MONITORING (ENHANCED)
// Monitor front sensor during turn
// If front clears, stop turning and resume wall-following
// ===================================================

void turnLeft() {
  stopCar();
  delay(100);
  stateStr = "TURN LEFT";
  turnStr = "LEFT";
  Serial.println(">>> EXECUTING: LEFT TURN WITH MONITORING <<<");

  frontClearedDuringTurn = false;
  turnStartTime = millis();
  unsigned long lastCheckTime = turnStartTime;

  // Execute turn with periodic front sensor monitoring
  while (millis() - turnStartTime < TURN_90_TIME) {
    unsigned long currentTime = millis();

    // Check front sensor every 50ms during turn
    if (currentTime - lastCheckTime >= TURN_CHECK_INTERVAL) {
      readAllSensors();
      lastCheckTime = currentTime;

      // If front clears during turn, abort and resume wall-following
      if (F > FRONT_CLEAR + 5) {
        Serial.println("FRONT CLEARED DURING LEFT TURN - ABORTING TURN");
        frontClearedDuringTurn = true;
        break;
      }
    }

    // Continue turning at TURN_SPEED
    setMotorSpeeds(-TURN_SPEED, TURN_SPEED);
  }

  stopCar();
  delay(TURN_PAUSE);
  turning = false;
  turnDecision = 0;
  turnStr = "-";
}

void turnRight() {
  stopCar();
  delay(100);
  stateStr = "TURN RIGHT";
  turnStr = "RIGHT";
  Serial.println(">>> EXECUTING: RIGHT TURN WITH MONITORING <<<");

  frontClearedDuringTurn = false;
  turnStartTime = millis();
  unsigned long lastCheckTime = turnStartTime;

  // Execute turn with periodic front sensor monitoring
  while (millis() - turnStartTime < TURN_90_TIME) {
    unsigned long currentTime = millis();

    // Check front sensor every 50ms during turn
    if (currentTime - lastCheckTime >= TURN_CHECK_INTERVAL) {
      readAllSensors();
      lastCheckTime = currentTime;

      // If front clears during turn, abort and resume wall-following
      if (F > FRONT_CLEAR + 5) {
        Serial.println("FRONT CLEARED DURING RIGHT TURN - ABORTING TURN");
        frontClearedDuringTurn = true;
        break;
      }
    }

    // Continue turning at TURN_SPEED
    setMotorSpeeds(TURN_SPEED, -TURN_SPEED);
  }

  stopCar();
  delay(TURN_PAUSE);
  turning = false;
  turnDecision = 0;
  turnStr = "-";
}

void turnAround() {
  stopCar();
  delay(100);
  stateStr = "U-TURN";
  turnStr = "U";
  Serial.println(">>> EXECUTING: U-TURN WITH MONITORING <<<");

  frontClearedDuringTurn = false;
  turnStartTime = millis();
  unsigned long lastCheckTime = turnStartTime;

  // Execute U-turn (180 degrees) with periodic monitoring
  while (millis() - turnStartTime < (TURN_90_TIME * 2)) {
    unsigned long currentTime = millis();

    // Check front sensor every 50ms during turn
    if (currentTime - lastCheckTime >= TURN_CHECK_INTERVAL) {
      readAllSensors();
      lastCheckTime = currentTime;

      // For U-turn, only abort if front suddenly becomes very clear
      if (F > FRONT_CLEAR + 10) {
        Serial.println("FRONT CLEARED DURING U-TURN - CONTINUING (may be legitimate)");
        // For U-turns, we don't abort since we're going backwards anyway
      }
    }

    // Continue U-turning at TURN_SPEED
    setMotorSpeeds(-TURN_SPEED, TURN_SPEED);
  }

  stopCar();
  delay(TURN_PAUSE);
  turning = false;
  turnDecision = 0;
  turnStr = "-";
}

// ===================================================
// WALL DETECTION
// ===================================================

void selectWallToFollow() {
  bool leftWallDetected = (L < WALL_DETECT);
  bool rightWallDetected = (R < WALL_DETECT);

  if (leftWallDetected && rightWallDetected) {
    followingWall = 3;
    wallStr = "BOTH";
    wallDistance = (L + R) / 2;
  }
  else if (leftWallDetected) {
    followingWall = 1;
    wallStr = "LEFT";
    wallDistance = L;
  }
  else if (rightWallDetected) {
    followingWall = 2;
    wallStr = "RIGHT";
    wallDistance = R;
  }
  else {
    followingWall = 0;
    wallStr = "NONE";
  }
}

// ===================================================
// CONTINUOUS CENTERING CORRECTION
// Uses REDUCED Kp=3 for smooth gentle corrections
// ===================================================

void applyContinuousCentering() {
  // Don't apply centering if we're turning
  if (turning) return;

  int leftSpeed = BASE_SPEED;
  int rightSpeed = BASE_SPEED;

  // EMERGENCY: Dangerously close to wall
  if (L < WALL_DANGER || R < WALL_DANGER) {
    stateStr = "AVOID";
    if (L < WALL_DANGER) {
      // Too close to left wall - steer RIGHT
      rightSpeed = BASE_SPEED - 20;
    } else {
      // Too close to right wall - steer LEFT
      leftSpeed = BASE_SPEED - 20;
    }
    moveForward(leftSpeed, rightSpeed);
    return;
  }

  // CASE 1: BOTH WALLS - Center between them
  if (followingWall == 3) {
    stateStr = "CENTER";
    float centerError = R - L;

    if (abs(centerError) < WALL_DEADBAND) {
      correction = 0;
      wallIntegral = 0;
    } else {
      // Smooth P control with reduced Kp for gentle corrections
      correction = centerError * KP * 0.8;
      correction = constrain(correction, -CORRECTION_LIMIT, CORRECTION_LIMIT);
    }

    leftSpeed = BASE_SPEED - correction;
    rightSpeed = BASE_SPEED + correction;
  }

  // CASE 2: ONLY LEFT WALL - Follow at exact distance
  else if (followingWall == 1) {
    stateStr = "FOLLOW L";
    wallDistance = L;
    float error = wallDistance - targetDistance;

    if (abs(error) < WALL_DEADBAND) {
      correction = 0;
    } else {
      // Reduced Kp for smooth gentle wall following
      correction = error * KP * 0.8;
      correction = constrain(correction, -CORRECTION_LIMIT, CORRECTION_LIMIT);

      // Apply correction for LEFT wall
      rightSpeed = BASE_SPEED - correction;
    }
  }

  // CASE 3: ONLY RIGHT WALL - Follow at exact distance
  else if (followingWall == 2) {
    stateStr = "FOLLOW R";
    wallDistance = R;
    float error = wallDistance - targetDistance;

    if (abs(error) < WALL_DEADBAND) {
      correction = 0;
    } else {
      // Reduced Kp for smooth gentle wall following
      correction = error * KP * 0.8;
      correction = constrain(correction, -CORRECTION_LIMIT, CORRECTION_LIMIT);

      // Apply correction for RIGHT wall
      leftSpeed = BASE_SPEED - correction;
    }
  }

  // CASE 4: NO WALLS - Go straight
  else {
    stateStr = "OPEN";
    leftSpeed = BASE_SPEED;
    rightSpeed = BASE_SPEED;
    wallIntegral = 0;
  }

  // Ensure minimum speed
  leftSpeed = max(leftSpeed, MIN_SPEED);
  rightSpeed = max(rightSpeed, MIN_SPEED);

  // Apply movement
  moveForward(leftSpeed, rightSpeed);
}

// ===================================================
// TURN DECISION - ONLY TURN WHEN FRONT IS BLOCKED
// WITH DUAL-SENSOR VALIDATION AND PRE-SCAN NOISE ELIMINATION
// ===================================================

bool shouldTurn() {
  // Don't check if already turning
  if (turning) return false;

  // ONLY turn if front is actually blocked
  bool frontBlocked = (F < FRONT_TURN);

  // If front is clear, NEVER turn
  if (!frontBlocked) {
    return false;
  }

  // Front is blocked - check if sides are open
  bool leftOpen = (L > OPEN_SPACE || FL > OPEN_SPACE);
  bool rightOpen = (R > OPEN_SPACE || FR > OPEN_SPACE);

  // If at least one side is open, it's a turn
  if (leftOpen || rightOpen) {
    Serial.println("TURN NEEDED - Front blocked, side open");
    return true;
  }

  // If both sides are blocked, it's a dead end
  Serial.println("DEAD END - Front and sides blocked");
  return true;
}

int decideTurnDirection() {
  // Perform pre-turn scanning (5 reads with 100ms delays)
  // This eliminates noise and ensures wall detection is accurate
  performPreTurnScanning();

  Serial.println("\n=== TURN DECISION WITH DUAL-SENSOR VALIDATION ===");
  Serial.print("Avg F="); Serial.print(avgF);
  Serial.print(" Avg FL="); Serial.print(avgFL);
  Serial.print(" Avg FR="); Serial.print(avgFR);
  Serial.print(" Avg L="); Serial.print(avgL);
  Serial.print(" Avg R="); Serial.println(avgR);

  float leftSpace = max(avgFL, avgL);
  float rightSpace = max(avgFR, avgR);

  Serial.print("Left Space: "); Serial.print(leftSpace);
  Serial.print(" Right Space: "); Serial.println(rightSpace);

  // Dead end - all sides blocked
  if (leftSpace < FRONT_STOP && rightSpace < FRONT_STOP && avgF < FRONT_STOP) {
    Serial.println("DEAD END - U-TURN");
    return 3;
  }

  // Evaluate turn directions
  bool canTurnLeft = false;
  bool canTurnRight = false;

  // LEFT TURN: Validate using dual-sensor validation
  if (leftSpace > rightSpace + 2) {
    canTurnLeft = validateOpeningDualSensor(true);
  }

  // RIGHT TURN: Validate using dual-sensor validation
  if (rightSpace > leftSpace + 2) {
    canTurnRight = validateOpeningDualSensor(false);
  }

  // If equal space, check both directions
  if (abs(leftSpace - rightSpace) <= 2) {
    canTurnLeft = validateOpeningDualSensor(true);
    canTurnRight = validateOpeningDualSensor(false);
  }

  // Choose valid turn direction
  if (canTurnLeft && !canTurnRight) {
    Serial.println("TURN LEFT - Validated");
    return 1;
  } else if (canTurnRight && !canTurnLeft) {
    Serial.println("TURN RIGHT - Validated");
    return 2;
  } else if (canTurnLeft && canTurnRight) {
    // Both valid - choose direction with more space
    if (leftSpace > rightSpace) {
      Serial.println("TURN LEFT - Both valid, more space on left");
      return 1;
    } else {
      Serial.println("TURN RIGHT - Both valid, more space on right");
      return 2;
    }
  } else {
    // No valid direction - perform U-turn as fallback
    Serial.println("NO VALID DIRECTION - U-TURN");
    return 3;
  }
}

// ===================================================
// LCD DISPLAY
// ===================================================

void updateLCD() {
  lcd.clear();

  lcd.setCursor(0, 0);
  lcd.print("F:");
  lcd.print((int)F);
  lcd.print(" L:");
  lcd.print((int)L);
  lcd.print(" R:");
  lcd.print((int)R);

  lcd.setCursor(0, 1);
  lcd.print("FL:");
  lcd.print((int)FL);
  lcd.print(" FR:");
  lcd.print((int)FR);

  lcd.setCursor(0, 2);
  lcd.print(moveStr);
  lcd.print(" ");
  lcd.print(stateStr);
  lcd.print(" ");
  lcd.print(wallStr);

  lcd.setCursor(0, 3);
  lcd.print("SPD:");
  lcd.print(currentLeftSpeed);
  lcd.print("/");
  lcd.print(currentRightSpeed);

  // Serial debug - less frequent
  static unsigned long lastDebug = 0;
  if (millis() - lastDebug > 500) {
    Serial.print("State:");
    Serial.print(stateStr);
    Serial.print(" F:");
    Serial.print(F);
    Serial.print(" L:");
    Serial.print(L);
    Serial.print(" R:");
    Serial.print(R);
    Serial.print(" Speed:");
    Serial.print(currentLeftSpeed);
    Serial.print("/");
    Serial.println(currentRightSpeed);
    lastDebug = millis();
  }
}

// ===================================================
// SETUP
// ===================================================

void setup() {
  pinMode(ENA, OUTPUT);
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(ENB, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);

  lcd.init();
  lcd.backlight();
  lcd.clear();
  lcd.print("ADVANCED CORNER");
  lcd.setCursor(0, 1);
  lcd.print("SAFE NAVIGATOR");
  lcd.setCursor(0, 2);
  lcd.print("v5.0 FINAL");
  delay(2500);
  lcd.clear();

  Serial.begin(9600);
  Serial.println("\n=== ADVANCED CORNER-SAFE NAVIGATION ===");
  Serial.println("Features:");
  Serial.println("- Pre-turn scanning (5 reads, 100ms delays)");
  Serial.println("- Dual-sensor validation (diagonal + side)");
  Serial.println("- Reduced Kp=3 for smooth corrections");
  Serial.println("- Turn monitoring with early abort");
  Serial.println("====================================\n");

  readAllSensors();
  delay(500);
}

// ===================================================
// MAIN LOOP - ADVANCED CORNER-SAFE NAVIGATION
// ===================================================

void loop() {
  // Read sensors
  readAllSensors();

  // Select which wall to follow
  selectWallToFollow();

  // ANTI-STALL: Check if we're stuck
  checkForStall();

  // DECISION MAKING - WITH ADVANCED VALIDATION
  if (!turning) {
    // Check if we need to turn (ONLY when front is actually blocked)
    if (F < FRONT_TURN) {
      // Front is blocked - we need to make a decision
      turning = true;
      stopCar();
      delay(300);

      // Decide turn direction with pre-scan and dual-sensor validation
      turnDecision = decideTurnDirection();

      // Execute the turn with monitoring
      if (turnDecision == 1) {
        turnLeft();

        // If front cleared during turn, resume wall-following instead of continuing
        if (frontClearedDuringTurn) {
          Serial.println("Resuming wall-following after turn abort");
          stateStr = "RESUME";
        }
      } else if (turnDecision == 2) {
        turnRight();

        // If front cleared during turn, resume wall-following instead of continuing
        if (frontClearedDuringTurn) {
          Serial.println("Resuming wall-following after turn abort");
          stateStr = "RESUME";
        }
      } else {
        turnAround();
      }
    }
    else {
      // Front is clear - just do wall following/centering
      applyContinuousCentering();
    }
  }

  // Update display
  updateLCD();

  // Small delay for stability
  delay(50);
}

/* END CODE */