//Arduino Uno x1
//Ultrasonic range finder (HC-SR04) to detect obstacle.
//Motor drive L298N to drive twin dc geared motors. Remember to remove the voltage regulator jumper when supply voltage is greater than 12V. Remove the 'enA' & 'enB' jumpers if you want to regulate motor speed. Arduino and the L298N MUST share common gound if they are using their own separate power sources.
//Servo motor x1. Red = +5v, brown/black = ground, yellow/orange/white = signal
//DC geared motor x2
//9v Battery x2
//Creator: Manish
#include <Servo.h>
#include <NewPing.h>
const int trigPin = 12;
const int echoPin = 11;
const long maxRange = 400;
long distance, L, F, R, temp;
long safeRange = 50;
const int turningSpeed = 80;
const int enA = 3;
const int enB = 5;
const int IN1 = 2;
const int IN2 = 4;
const int IN3 = 7;
const int IN4 = 8;
const int servoPin = 6;
NewPing myPing(trigPin, echoPin, maxRange);
Servo myServo;
void setup()
{
Serial.begin(9600);
myServo.attach(servoPin);
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
pinMode(enA, OUTPUT);
pinMode(enB, OUTPUT);
pinMode(IN1, OUTPUT);
pinMode(IN2, OUTPUT);
pinMode(IN3, OUTPUT);
pinMode(IN4, OUTPUT);
}
long getRange()
{
distance = myPing.ping_cm();
return distance;
}
void forward()
{
Serial.print("No obstacles detected, keep driving. (Obstacle in ");
Serial.print(F);
Serial.println(" cm.)");
analogWrite(enA, 250);
analogWrite(enB, 250);
digitalWrite(IN1, LOW);
digitalWrite(IN2, HIGH);
digitalWrite(IN3, LOW);
digitalWrite(IN4, HIGH);
}
void halt()
{
Serial.println("Halting the rover.\n");
analogWrite(enA, LOW);
analogWrite(enB, LOW);
digitalWrite(IN1, LOW);
digitalWrite(IN2, LOW);
digitalWrite(IN3, LOW);
digitalWrite(IN4, LOW);
delay(70);
}
void resetServo()
{
myServo.write(90);
}
void scanToTheFront()
{
myServo.write(90);
getRange();
F = distance;
Serial.print("Scanning to the front. (Object in : ");
Serial.print(F);
Serial.println(" cm.)");
delay(80);
}
void scanToTheRight()
{
myServo.write(30); //look 60 degrees to the right
getRange();
R = distance;
Serial.print("Scanning to the right. (Object in : ");
Serial.print(distance);
Serial.println(" cm.)");
delay(80);
resetServo();
}
void scanToTheLeft()
{
myServo.write(150); //look 60 degrees to the left
getRange();
L = distance;
Serial.print("Scanning to the left. (Object in : ");
Serial.print(distance);
Serial.println(" cm.)");
delay(80);
resetServo();
}
void turnRight()
{
Serial.println("Turning to the right.");
analogWrite(enA, turningSpeed);
analogWrite(enB, turningSpeed);
digitalWrite(IN1, LOW);
digitalWrite(IN2, HIGH);
digitalWrite(IN3, HIGH);
digitalWrite(IN4, LOW);
delay(60);
}
void turnLeft()
{
Serial.println("Turning to the left.");
analogWrite(enA, turningSpeed);
analogWrite(enB, turningSpeed);
digitalWrite(IN1, HIGH);
digitalWrite(IN2, LOW);
digitalWrite(IN3, LOW);
digitalWrite(IN4, HIGH);
delay(60);
}
void turnAround()
{
Serial.println("Turning the rover around.");
analogWrite(enA, turningSpeed);
analogWrite(enB, turningSpeed);
digitalWrite(IN1, LOW);
digitalWrite(IN2, HIGH);
digitalWrite(IN3, HIGH);
digitalWrite(IN4, LOW);
delay(80);
}
void loop()
{
scanToTheLeft();
scanToTheFront();
scanToTheRight();
temp = max(L, F);
temp = max(temp, R);
if (temp == F && F >= safeRange)
{
Serial.println("Max range in the front.");
while (F >= safeRange)
{
Serial.println("Max range on the front is greater than the safe range");
forward();
scanToTheFront();
}
{
Serial.print("WARNING: Obstacle detected within the safe range! (Obstacle in ");
Serial.print(F);
Serial.println(" cm.)");
halt();
}
}
else if (temp == R && R >= safeRange)
{
Serial.println("Max range in the right.");
Serial.println("Max range in the right is greater than the safe range.");
turnRight();
scanToTheFront();
while (F >= safeRange)
{
forward();
scanToTheFront();
}
{
Serial.print("WARNING: Obstacle detected within the safe range! (Obstacle in ");
Serial.print(F);
Serial.println(" cm.)");
halt();
}
}
else if (temp == L && L >= safeRange)
{
Serial.println("Max range in the left.");
Serial.println("Max range in the left is greater than the safe range.");
turnLeft();
scanToTheFront();
while (F >= safeRange)
{
forward();
scanToTheFront();
}
{
Serial.print("WARNING: Obstacle detected within the safe range! (Obstacle in ");
Serial.print(F);
Serial.println(" cm.)");
halt();
}
}
else
{
Serial.print("WARNING: Obstacle detected within the safe range! (Obstacle in ");
Serial.print(L);
Serial.println(" cm.)");
halt();
turnAround();
}
}
Comments