Maze code :
#include <Servo.h>
#include <LiquidCrystal_I2C.h>
// Pin definitions for color sensor
#define COLOR_OUT 2
#define S0_PIN 3
#define S1_PIN 4
#define S2_PIN 5
#define S3_PIN 6
// Pin definitions for ultrasonic sensor
#define TRIG_PIN 7
#define ECHO_PIN 8
// Pin definition for servo
#define SERVO_PIN 9
// Pin definitions for L298N motor driver
#define IN1_PIN 10
#define IN2_PIN 11
#define IN3_PIN 12
#define IN4_PIN 13
// Constants
#define TURNING_SPEED 200
#define FORWARD_SPEED 255
#define MIN_DISTANCE 15 // Increased minimum distance for faster navigation
// LCD initialization
LiquidCrystal_I2C lcd(0x27, 16, 2);
Servo myservo;
// Global variables
int distance;
long duration;
int redValue, greenValue, blueValue;
int leftDistance, rightDistance, frontDistance;
void setup() {
// Initialize LCD
lcd.init();
lcd.backlight();
lcd.print("Fast Robot");
lcd.setCursor(0, 1);
lcd.print("Initializing...");
// Setup color sensor pins
pinMode(COLOR_OUT, INPUT);
pinMode(S0_PIN, OUTPUT);
pinMode(S1_PIN, OUTPUT);
pinMode(S2_PIN, OUTPUT);
pinMode(S3_PIN, OUTPUT);
// Set frequency scaling to 20% for color sensor
digitalWrite(S0_PIN, HIGH);
digitalWrite(S1_PIN, LOW);
// Setup ultrasonic sensor pins
pinMode(TRIG_PIN, OUTPUT);
pinMode(ECHO_PIN, INPUT);
// Setup motor control pins
pinMode(IN1_PIN, OUTPUT);
pinMode(IN2_PIN, OUTPUT);
pinMode(IN3_PIN, OUTPUT);
pinMode(IN4_PIN, OUTPUT);
// Initialize servo and set to center position
myservo.attach(SERVO_PIN);
myservo.write(90);
// Short delay for initialization
delay(1000);
lcd.clear();
}
void loop() {
// Quick check for blue objects
if (isBlueDetected()) {
lcd.clear();
lcd.print("Blue Detected!");
stopMotors();
delay(3000); // Wait 3 seconds before continuing
return;
}
// Get front distance quickly
frontDistance = getDistance();
// Display current distance
lcd.setCursor(0, 0);
lcd.print("Dist: ");
lcd.print(frontDistance);
lcd.print("cm ");
// Fast navigation logic
if (frontDistance > MIN_DISTANCE) {
// Front is clear, move forward
lcd.setCursor(0, 1);
lcd.print("Forward ");
moveForward();
} else {
// Need to check sides
stopMotors();
// Quick check left and right
leftDistance = getLeftDistance();
rightDistance = getRightDistance();
// Left hand rule logic
if (leftDistance > MIN_DISTANCE) {
lcd.setCursor(0, 1);
lcd.print("Turning Left ");
turnLeft();
} else if (rightDistance > MIN_DISTANCE) {
lcd.setCursor(0, 1);
lcd.print("Turning Right");
turnRight();
} else {
lcd.setCursor(0, 1);
lcd.print("Turn Around ");
turnAround();
}
}
// Short delay for stability
delay(50);
}
// Function to check if blue color is detected
bool isBlueDetected() {
// Set filters for blue reading
digitalWrite(S2_PIN, LOW);
digitalWrite(S3_PIN, HIGH);
blueValue = pulseIn(COLOR_OUT, LOW, 10000);
// Set filters for red reading
digitalWrite(S2_PIN, LOW);
digitalWrite(S3_PIN, LOW);
redValue = pulseIn(COLOR_OUT, LOW, 10000);
// Set filters for green reading
digitalWrite(S2_PIN, HIGH);
digitalWrite(S3_PIN, HIGH);
greenValue = pulseIn(COLOR_OUT, LOW, 10000);
// Lower values mean stronger color, so we invert for comparison
// Check if blue is the dominant color
return (blueValue < redValue && blueValue < greenValue && blueValue < 50);
}
// Function to get distance from ultrasonic sensor
int getDistance() {
digitalWrite(TRIG_PIN, LOW);
delayMicroseconds(2);
digitalWrite(TRIG_PIN, HIGH);
delayMicroseconds(10);
digitalWrite(TRIG_PIN, LOW);
duration = pulseIn(ECHO_PIN, HIGH, 20000); // 20ms timeout for faster response
// Check if reading was successful
if (duration == 0) {
return 100; // Return a large value if timeout occurred
}
distance = duration * 0.034 / 2;
return distance;
}
// Function to get left distance
int getLeftDistance() {
myservo.write(170); // Turn servo left (not full 180 for speed)
delay(150); // Reduced delay for faster operation
int dist = getDistance();
myservo.write(90); // Return to center
return dist;
}
// Function to get right distance
int getRightDistance() {
myservo.write(10); // Turn servo right (not full 0 for speed)
delay(150); // Reduced delay for faster operation
int dist = getDistance();
myservo.write(90); // Return to center
return dist;
}
// Function to move forward
void moveForward() {
// Right motor forward
digitalWrite(IN1_PIN, HIGH);
digitalWrite(IN2_PIN, LOW);
// Left motor forward
digitalWrite(IN3_PIN, HIGH);
digitalWrite(IN4_PIN, LOW);
}
// Function to stop motors
void stopMotors() {
digitalWrite(IN1_PIN, LOW);
digitalWrite(IN2_PIN, LOW);
digitalWrite(IN3_PIN, LOW);
digitalWrite(IN4_PIN, LOW);
}
// Function to turn left
void turnLeft() {
// Right motor forward
digitalWrite(IN1_PIN, HIGH);
digitalWrite(IN2_PIN, LOW);
// Left motor stop or reverse slightly
digitalWrite(IN3_PIN, LOW);
digitalWrite(IN4_PIN, HIGH);
delay(300); // Reduced turning time
moveForward(); // Continue moving forward after turn
}
// Function to turn right
void turnRight() {
// Right motor stop or reverse slightly
digitalWrite(IN1_PIN, LOW);
digitalWrite(IN2_PIN, HIGH);
// Left motor forward
digitalWrite(IN3_PIN, HIGH);
digitalWrite(IN4_PIN, LOW);
delay(300); // Reduced turning time
moveForward(); // Continue moving forward after turn
}
// Function to turn around
void turnAround() {
// Right motor forward
digitalWrite(IN1_PIN, HIGH);
digitalWrite(IN2_PIN, LOW);
// Left motor backward
digitalWrite(IN3_PIN, LOW);
digitalWrite(IN4_PIN, HIGH);
delay(600); // Reduced turn-around time
moveForward(); // Continue moving forward after turn
}
