#include <Wire.h>
#include <LiquidCrystal_PCF8574.h>
LiquidCrystal_PCF8574 lcd(0x27); // set the LCD address to 0x27 for a 16 chars and 2 line display
//Install all required libraries
int motorL1=4;
int motorL2=5;
int motorR1=2;
int motorR2=3; // assigning motors pins
int echo_front=13;
int trig_front=12;
int echo_left=9;
int trig_left=8;
int echo_right=11;
int trig_right=10;// assigning Sensors pins
int pt=0;
long
time_front,distance_front,minimum_front,distance_left,minimum_left,time_left,distance_right,minimum_right,time_right;
void setup()
{
Serial.begin(115200); // See http://playground.arduino.cc/Main/I2cScanner
Wire.begin();
Wire.beginTransmission(0x27);
lcd.begin(16, 2); // initialize the lcd
Serial.begin(9600);
pinMode(trig_front,OUTPUT);
pinMode(echo_front,INPUT);
pinMode(trig_left,OUTPUT);
pinMode(echo_left,INPUT);
pinMode(trig_right,OUTPUT);
pinMode(echo_right,INPUT);
pinMode(motorL1,OUTPUT);
pinMode(motorL2,OUTPUT);
pinMode(motorR1,OUTPUT);
pinMode(motorR2,OUTPUT);
// indicating all input output pins
}
void loop()
{
// main loop stars from here
lcd.setBacklight(255);
lcd.home(); lcd.clear();
lcd.print("I AM YOUR ROBOT");
lcd.setCursor(0,1); lcd.print("WELCOME"); // intialising LCD display
sensor_front();
sensor_left();
sensor_right();
if(distance_front<=minimum_front)
{
stop_robot();
Serial.println("Robot Stop");
lcd.clear();
lcd.print("STOP");
delay(100);
if((distance_right<=minimum_right)&&(!(distance_left<=minimum_left)))
{
reverse_robot();
Serial.println("Robot Back-1");
lcd.clear();
lcd.print("MOVING BACK");
delay(100);
turn_left();
pt=-1;
Serial.println("Robot Left1");
lcd.clear();
lcd.print("MOVING LEFT");
delay(100);
forward_robot();
Serial.println("Robot Forward1");
lcd.clear();
lcd.print("MOVING FORWARD");
delay(100);
}
else if( (distance_left<=minimum_left)&&(!(distance_right<=minimum_right)))
{
reverse_robot();
Serial.println("Robot Back0");
lcd.clear();
lcd.print("MOVING BACK");
delay(100);
turn_right();
pt=1;
Serial.println("Robot Right1");
lcd.clear();
lcd.print("MOVING RIGHT");delay(500);
forward_robot();
Serial.println("Robot Forward11");
lcd.clear();
lcd.print("MOVING FORWARD");delay(500);
}
else if((distance_left>=minimum_right)&&(distance_right>=minimum_left))
{
reverse_robot();
Serial.println("Robot Back1");
lcd.clear();
lcd.print("MOVING BACK");
delay(100);
turn_left();
pt=-1;
Serial.println("Robot Left2");
lcd.clear();
lcd.print("MOVING LEFT");delay(500);
forward_robot();
Serial.println("Robot Forward111");
lcd.clear();
lcd.print("MOVING FORWARD");delay(500);
}
else
{
do
{
reverse_robot();
Serial.println("Robot Back2");
lcd.clear();
lcd.print("MOVING BACK");delay(500);
sensor_front();
sensor_left();
sensor_right();
}
while((distance_left<=minimum_left)&&(distance_right<=minimum_right));
stop_robot();
goto deadend;
}
}
else if((distance_right<=minimum_right)&&(!(distance_left<=minimum_left)))
{
reverse_robot();
Serial.println("Robot Back22");
lcd.clear();
lcd.print("MOVING BACK");delay(100);
turn_left();
pt=-1;
Serial.println("Robot Left4");
lcd.clear();
lcd.print("MOVING LEFT");delay(100);
forward_robot();
Serial.println("Robot Forward1111");
lcd.clear();
lcd.print("MOVING FORWARD");delay(100);
}
else if((distance_left<=minimum_left)&&(!(distance_right<=minimum_right)))
{
reverse_robot();
turn_right();
pt=1;
Serial.println("Robot Right2");
lcd.clear();
lcd.print("MOVING RIGHT");delay(100);
forward_robot();
Serial.println("Robot Forward11111");
lcd.clear();
lcd.print("MOVING FORWARD");delay(100);
}
else if((distance_left>=minimum_left)&&(distance_right>=minimum_right))
{
reverse_robot();
turn_left();
forward_robot();
pt=-1;
Serial.println("Robot Left5");
lcd.clear();
lcd.print("MOVING LEFT");delay(100);
forward_robot();
Serial.println("Robot Forward1.6");
lcd.clear();
lcd.print("MOVING FORWARD");delay(100);
}
else
{
forward_robot();
Serial.println("Robot Forward2");
lcd.clear();
lcd.print("MOVING FORWARD");delay(100);
}
if(0)
deadend:
{
if((distance_left>=minimum_left)&&(distance_right>=minimum_right))
{
if(pt==-1)
{
turn_left();
pt=-1;
Serial.println("Robot Left6");
lcd.clear();
lcd.print("MOVING LEFT");delay(100);
forward_robot();
Serial.println("Robot Forward3");
lcd.clear();
lcd.print("MOVING FORWARD");delay(100);
}
else if(pt==1)
{
turn_right();
pt=1;
Serial.println("Robot Right3");
lcd.clear();
lcd.print("MOVING RIGHT");delay(100);
forward_robot();
Serial.println("Robot Forward4");
lcd.clear();
lcd.print("MOVING FRONT");delay(100);
}
}
else if((distance_left>=minimum_left))
{
turn_left();
pt=-1;
Serial.println("Robot Left7");
lcd.clear();
lcd.print("MOVING LEFT");delay(100);
forward_robot();
Serial.println("MOVING FRONT");
lcd.clear();
lcd.print("MOVING FRONT");delay(100);
}
else if((distance_right>=minimum_right))
{
turn_right();
pt=1;
Serial.println("Robot Right4");
lcd.clear();
lcd.print("MOVING RIGHT");delay(100);
forward_robot();
Serial.println("Robot Forward6");
lcd.clear();
lcd.print("MOVING FRONT");delay(100);
}
}
}
void sensor_front()
{
digitalWrite(trig_front, LOW);
delayMicroseconds(5);
digitalWrite(trig_front, HIGH);
delay(20);
digitalWrite(trig_front, LOW);
time_front=pulseIn(echo_front,HIGH);
distance_front=time_front/58.2;
Serial.println("distance front=");
Serial.println(distance_front);
delay(10);
minimum_front=30;
}
void sensor_left()
{
digitalWrite(trig_left, LOW);
delayMicroseconds(5);
digitalWrite(trig_left, HIGH);
delay(20);
digitalWrite(trig_left, LOW);
time_left=pulseIn(echo_left,HIGH);
distance_left=time_left/58.2;
Serial.println("distance left=");
Serial.println(distance_left);
delay(10);
minimum_left=30;
}
void sensor_right()
{
digitalWrite(trig_right, LOW);
delayMicroseconds(5);
digitalWrite(trig_right, HIGH);
delay(20);
digitalWrite(trig_right, LOW);
time_right=pulseIn(echo_right,HIGH);
distance_right=time_right/58.2;
Serial.println("distance right=");
Serial.println(distance_right);
delay(10);
minimum_right=30;
}
void forward_robot()
{
digitalWrite(motorL1,LOW);
digitalWrite(motorL2,HIGH);
digitalWrite(motorR1,LOW);
digitalWrite(motorR2,HIGH);// if motors wont move front ,rearrange the pins
delay(2000);
}
void reverse_robot()
{
digitalWrite(motorL1,HIGH);
digitalWrite(motorL2,LOW);
digitalWrite(motorR1,HIGH);
digitalWrite(motorR2,LOW);
delay(2000);
}
void stop_robot()
{
digitalWrite(motorL1,LOW);
digitalWrite(motorL2,LOW);
digitalWrite(motorR1,LOW);
digitalWrite(motorR2,LOW);
delay(2000);
}
void turn_left()
{
digitalWrite(motorL1,LOW);
digitalWrite(motorL2,LOW);
digitalWrite(motorR1,LOW);
digitalWrite(motorR2,HIGH);
delay(2000);
}
void turn_right()
{
digitalWrite(motorL1,LOW);
digitalWrite(motorL2,HIGH);
digitalWrite(motorR1,LOW);
digitalWrite(motorR2,LOW);
delay(2000);
}