mazeslolver.ino

// ===================================================================================
// FINAL MAZE SOLVER (Handling '0' Error as Clear Path)
// ===================================================================================

// --- 1. HARDWARE PINS ---
// Right Motor
#define ENA 5   
#define IN1 4
#define IN2 7

// Left Motor
#define ENB 6   
#define IN3 8
#define IN4 12

// Sensors
#define TRIG_L A0
#define ECHO_L A1
#define TRIG_F A2
#define ECHO_F A3
#define TRIG_R A4
#define ECHO_R A5

// --- 2. SETTINGS (Tuned for your Maze) ---

// Distances
#define TARGET_DIST 15    // Koshish karo k Left wall se 18cm door raho
#define WALL_THRESHOLD 90 // Agar faasla 80cm se barh jaye, tab hi Turn lena
#define FRONT_STOP 15     // Samne rukne ka faasla

// Speeds (L298N Voltage Drop Compensated)
#define BASE_SPEED 90    // Seedha chalne ki speed
#define TURN_SPEED 70    // Turn lene ki speed (Thori tez taake phans na jaye)
#define MAX_SPEED 90

// PID Variables
float Kp = 4.0;
float Kd = 14.0;
float lastError = 0;

// Variables
int distL, distF, distR;

// State Machine
enum RobotState {
  PID_WALL_FOLLOW,
  CHECK_INTERSECTION
};
RobotState currentState = PID_WALL_FOLLOW;

void setup() {
  Serial.begin(9600);

  // Pins Setup
  pinMode(ENA, OUTPUT); pinMode(IN1, OUTPUT); pinMode(IN2, OUTPUT);
  pinMode(ENB, OUTPUT); pinMode(IN3, OUTPUT); pinMode(IN4, OUTPUT);

  pinMode(TRIG_L, OUTPUT); pinMode(ECHO_L, INPUT);
  pinMode(TRIG_F, OUTPUT); pinMode(ECHO_F, INPUT);
  pinMode(TRIG_R, OUTPUT); pinMode(ECHO_R, INPUT);

  Serial.println("Robot Ready! Place inside Maze...");
  delay(2000); // 2 Second ka time taake aap hath hata lein
}

void loop() {
  readAllSensors(); // Sensors parho

  switch (currentState) {
    
    // --- STATE 1: SEEDHA CHALO (WALL FOLLOW) ---
    case PID_WALL_FOLLOW:
      
      // 1. Agar Samne Rasta Band Hai (15cm se qareeb) -> Ruk jao
      if (distF < FRONT_STOP) {
        stopMotors();
        currentState = CHECK_INTERSECTION;
      }
      
      // 2. Agar Left par Rasta Khul Gya (Gap agya)
      // (Yahan '0' wala masla solve hoga. Agar 0 aya to distL=200 hoga, jo 35 se bara hai)
      else if (distL > WALL_THRESHOLD) {
        moveForward(); // Thora aagay jao
        delay(400);    // Intersection k beech mein anay ka time
        stopMotors();
        currentState = CHECK_INTERSECTION;
      }
      
      // 3. Normal Deewar k sath sath chalo
      else {
        runPID();
      }
      break;

    // --- STATE 2: FAISLA KARO (LSRB Logic) ---
    case CHECK_INTERSECTION:
      readAllSensors(); // Dobara check karo
      char turn = decideTurn();
      performTurn(turn);
      currentState = PID_WALL_FOLLOW; // Wapis chalna shuru karo
      break;
  }
}

// ===================================================================================
// SENSOR READING (Error Handling Logic)
// ===================================================================================
void readAllSensors() {
  distL = getDistance(TRIG_L, ECHO_L);
  delay(5); // Crosstalk delay
  distF = getDistance(TRIG_F, ECHO_F);
  delay(5);
  distR = getDistance(TRIG_R, ECHO_R);

  // Serial Monitor par dekhein
  Serial.print("L:"); Serial.print(distL);
  Serial.print(" F:"); Serial.print(distF);
  Serial.print(" R:"); Serial.println(distR);
}

int getDistance(int trig, int echo) {
  digitalWrite(trig, LOW); delayMicroseconds(2);
  digitalWrite(trig, HIGH); delayMicroseconds(10);
  digitalWrite(trig, LOW);
  
  long duration = pulseIn(echo, HIGH, 12000); // Timeout (~200cm)
  int cm = duration * 0.034 / 2;
  
  // *** YE HAI ASAL FIX ***
  // Agar sensor ko kuch na mile (0), to samjho rasta saaf hai (200cm)
  if (cm == 0) return 200; 
  
  return cm;
}

// ===================================================================================
// PID LOGIC (Smooth Wall Following)
// ===================================================================================
void runPID() {
  // Target (18) - Actual (e.g. 22) = -4
  float error = TARGET_DIST - distL;
  
  // Error Limit
  if (error < -12) error = -12;
  if (error > 12) error = 12;

  float P = Kp * error;
  float D = Kd * (error - lastError);
  float correction = P + D;
  
  lastError = error;

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

  leftSpeed = constrain(leftSpeed, 0, MAX_SPEED);
  rightSpeed = constrain(rightSpeed, 0, MAX_SPEED);

  move(leftSpeed, rightSpeed);
}

// ===================================================================================
// NAVIGATION DECISION (Left Hand Rule)
// ===================================================================================
char decideTurn() {
  if (distL > WALL_THRESHOLD) return 'L'; // Left Open
  else if (distF > FRONT_STOP) return 'S'; // Front Open
  else if (distR > WALL_THRESHOLD) return 'R'; // Right Open
  else return 'B'; // Dead End (U-Turn)
}

// ===================================================================================
// MOTORS MOVEMENT
// ===================================================================================
void move(int leftSpeed, int rightSpeed) {
  digitalWrite(IN3, LOW); digitalWrite(IN4, HIGH); analogWrite(ENB, leftSpeed);
  digitalWrite(IN1, HIGH); digitalWrite(IN2, LOW); analogWrite(ENA, rightSpeed);
}

void stopMotors() {
  digitalWrite(IN1, LOW); digitalWrite(IN2, LOW); analogWrite(ENA, 0);
  digitalWrite(IN3, LOW); digitalWrite(IN4, LOW); analogWrite(ENB, 0);
  delay(200);
}

void moveForward() {
  move(BASE_SPEED, BASE_SPEED);
}

void performTurn(char turn) {
  switch(turn) {
    case 'L': // Left Turn
      digitalWrite(IN1, HIGH); digitalWrite(IN2, LOW); analogWrite(ENA, TURN_SPEED);
      digitalWrite(IN3, HIGH); digitalWrite(IN4, LOW); analogWrite(ENB, TURN_SPEED);
      delay(300); // Check 90 degree turn
      break;
    case 'R': // Right Turn
      digitalWrite(IN1, LOW); digitalWrite(IN2, HIGH); analogWrite(ENA, TURN_SPEED);
      digitalWrite(IN3, LOW); digitalWrite(IN4, HIGH); analogWrite(ENB, TURN_SPEED);
      delay(300); 
      break;
    case 'B': // U-Turn
      digitalWrite(IN1, HIGH); digitalWrite(IN2, LOW); analogWrite(ENA, TURN_SPEED);
      digitalWrite(IN3, HIGH); digitalWrite(IN4, LOW); analogWrite(ENB, TURN_SPEED);
      delay(250); 
      break;
    case 'S': // Straight
      moveForward();
      delay(300);
      break;
  }
  stopMotors();
  delay(100);
}