You are on page 1of 26

PROGRAM FOR SELF CONTROLLING ROBOT TO

ANALYZE AND TAKE TURNS ACCORDINGLY

By,
NIKHIL JOSHI,
14ME30020,
First Year Undergraduate Student,
Department of Mechanical Engineering,
Indian Institute of Technology Kharagpur,India

COMPLETE RUNNING PROGRAM ONLY


NEED IS TO SET THE SPEEDS OF BOTH
THE MOTORS

#include<Servo.h>
Servo n;
const int echo = 8, Trig = 9;//For UltraSonic sound Sensor
int Mp1=3;//Motor for turning Left
int Mp2=6;//Motor for turning Right
int flag=0;//For Selection of Cases(0,1,5,6,7,8)
long duration, inches, cm,cm1,cm2;//Measuring time and corresponding distances of Obstacles
int a1,a2,a3,b1=0;
int l,r;//Left theta and Right theta respectively
int ld,rd;//Left distance and Right distance respectively
int tt[10];//array for LTCDA(Case 7) and RTCDA(Case 8)
int min=500,p;
void setup() {
// put your setup code here, to run once:
Serial.begin(9600);
pinMode(Trig, OUTPUT);//UltraSonic Sensor OUTPUT
pinMode(echo, INPUT);//UltraSonic Sensor INPUT
n.attach(10);//For the Servo
n.write(90);//Setting Servo in the Forward direction
delay(500);//Time for Servo to set to the specified position
pinMode(Mp1,OUTPUT);
pinMode(Mp2,OUTPUT);
}

/*
SWITCH CASE is Used to make program Faster and easy to understand due to its division into
MODULES.
Case 0:Go Straight until obstacle is Detected
Case 1:Turn Detection
Case 5:Left turn initiation
Case 6:Right turn initiation
Case 7:RTCDA
Case 8:LTCDA
*/
void loop()
{
switch(flag)
{
case 0 :
{
{
digitalWrite(Trig, LOW);//Get the distance
delayMicroseconds(2);
digitalWrite(Trig, HIGH);
delayMicroseconds(10);
digitalWrite(Trig, LOW);
duration = pulseIn(echo, HIGH);
cm = duration/29/2;
Serial.print(cm);

Serial.println("cm case :0 initial");


delay(50);
if(cm==0)
{
digitalWrite(Trig, LOW);//Get the distance
delayMicroseconds(2);
digitalWrite(Trig, HIGH);
delayMicroseconds(10);
digitalWrite(Trig, LOW);
duration = pulseIn(echo, HIGH);
cm = duration/29/2;
Serial.print(cm);
Serial.println("cm case :0 initial");
}
}
if(cm<20)
{
digitalWrite(Mp1,LOW);
digitalWrite(Mp2,LOW);
flag=1;
break;
}
if(cm>=20 && cm<500)
{
Serial.println("::::: Going Straight :::::");

analogWrite(Mp1,145);
analogWrite(Mp2,120);
delay(100);
analogWrite(Mp1,0);
analogWrite(Mp2,0);
}

break;
}

/*Turning Decision*/
case 1:{
n.write(180);
delay(2000);
{
digitalWrite(Trig, LOW);//Get the distance
delayMicroseconds(2);
digitalWrite(Trig, HIGH);
delayMicroseconds(10);
digitalWrite(Trig, LOW);
duration = pulseIn(echo, HIGH);
cm = duration/29/2;
Serial.print(cm);
Serial.println("cm case :1 {180} :::::");
delay(50);

if(cm==0)
{
digitalWrite(Trig, LOW);//Get the distance
delayMicroseconds(2);
digitalWrite(Trig, HIGH);
delayMicroseconds(10);
digitalWrite(Trig, LOW);
duration = pulseIn(echo, HIGH);
cm = duration/29/2;
Serial.print(cm);
Serial.println("cm case :1 initial");
}
}
cm1=cm;
ld=cm;
n.write(0);
delay(2000);
{
digitalWrite(Trig, LOW);//Get the distance
delayMicroseconds(2);
digitalWrite(Trig, HIGH);
delayMicroseconds(10);
digitalWrite(Trig, LOW);
duration = pulseIn(echo, HIGH);
cm = duration/29/2;

Serial.print(cm);
Serial.println("cm case :0 initial");
delay(50);
if(cm==0)
{
digitalWrite(Trig, LOW);//Get the distance
delayMicroseconds(2);
digitalWrite(Trig, HIGH);
delayMicroseconds(10);
digitalWrite(Trig, LOW);
duration = pulseIn(echo, HIGH);
cm = duration/29/2;
Serial.print(cm);
Serial.println("cm case :0 initial");
}
}
cm2=cm;
rd=cm;
if(cm1>cm2)
{
n.write(90);
flag=5;//case 5
break;
}
if(cm2>cm1)

{
n.write(90);
flag=6;//case 6
break;
}
if(cm1==cm2)
{
n.write(90);
flag=0;
break;
}

}
/* Left Turn Preparation {LTP}*/
case 5:{
{
digitalWrite(Trig, LOW);//Get the distance
delayMicroseconds(2);
digitalWrite(Trig, HIGH);
delayMicroseconds(10);
digitalWrite(Trig, LOW);
duration = pulseIn(echo, HIGH);
cm = duration/29/2;
Serial.print(cm);

Serial.println("cm --->--->---> // TT0 case 5 \\");


delay(50);
if(cm==0)
{
digitalWrite(Trig, LOW);//Get the distance
delayMicroseconds(2);
digitalWrite(Trig, HIGH);
delayMicroseconds(10);
digitalWrite(Trig, LOW);
duration = pulseIn(echo, HIGH);
cm = duration/29/2;
Serial.print(cm);
Serial.println("cm case :5 initial");
}
}
if(cm>40)
{
digitalWrite(Mp2,LOW);
flag=8;
break;
}
if(cm<40){
digitalWrite(Mp2,HIGH);
delay(75);
digitalWrite(Mp2,LOW);

delay(100);
}
break;
}

/*Right Turn Preparation*/


case 6:{

{
digitalWrite(Trig, LOW);//Get the distance
delayMicroseconds(2);
digitalWrite(Trig, HIGH);
delayMicroseconds(10);
digitalWrite(Trig, LOW);
duration = pulseIn(echo, HIGH);
cm = duration/29/2;
Serial.print(cm);
Serial.println("cm --->--->---> ---TT0---RIGHT---case 6 ");
delay(50);
if(cm==0)
{
digitalWrite(Trig, LOW);//Get the distance
delayMicroseconds(2);
digitalWrite(Trig, HIGH);

delayMicroseconds(10);
digitalWrite(Trig, LOW);
duration = pulseIn(echo, HIGH);
cm = duration/29/2;
Serial.print(cm);
Serial.println("cm case :6 initial");
}
}
if(cm>40)
{
digitalWrite(Mp1,LOW);
flag=7;
break;
}
if(cm<40){
digitalWrite(Mp1,HIGH);
delay(100);
digitalWrite(Mp1,LOW);
delay(100);
}
break;
}

/*Right Turn Completion Detection Algorithm[RTCDA]*/


case 7:{

for(int i=180,j=0;i>90;i=i-10,j++)
{
n.write(i);
delay(500);
{
digitalWrite(Trig, LOW);//Get the distance
delayMicroseconds(2);
digitalWrite(Trig, HIGH);
delayMicroseconds(10);
digitalWrite(Trig, LOW);
duration = pulseIn(echo, HIGH);
cm = duration/29/2;
Serial.print(cm);
Serial.println("cm case 7 right { TT1 }");
delay(50);
if(cm==0)
{
digitalWrite(Trig, LOW);//Get the distance
delayMicroseconds(2);
digitalWrite(Trig, HIGH);
delayMicroseconds(10);
digitalWrite(Trig, LOW);
duration = pulseIn(echo, HIGH);
cm = duration/29/2;
Serial.print(cm);

Serial.println("cm case :7 initial");


}
}
tt[j]=cm;
}

n.write(90);
delay(1000);

for(int j=0;j<9;j++)
{
if(min>tt[j])
{
min=tt[j];
p=j;
}
}
l=p*10;
Serial.print("180-(p*10)::::: So,theta= ");
Serial.println((p*10));

if(min>25)
{

Serial.println("::::: GOT a PROBLEM , PROBABLY A OVERTURN :::::");


}
if(p*10!=0){digitalWrite(Mp1,HIGH);
delay(350);
digitalWrite(Mp1,LOW);}
min=500;
if(p==0)
{
for(int k=0;k<1;k++)
{

for(int i=180,j=0;i>90;i=i-10,j++)
{
n.write(i);
delay(500);
{
digitalWrite(Trig, LOW);//Get the distance
delayMicroseconds(2);
digitalWrite(Trig, HIGH);
delayMicroseconds(10);
digitalWrite(Trig, LOW);
duration = pulseIn(echo, HIGH);
cm = duration/29/2;
Serial.print(cm);
Serial.println("cm case :0 { TT1 ************777777}");

delay(50);
if(cm==0)
{
digitalWrite(Trig, LOW);//Get the distance
delayMicroseconds(2);
digitalWrite(Trig, HIGH);
delayMicroseconds(10);
digitalWrite(Trig, LOW);
duration = pulseIn(echo, HIGH);
cm = duration/29/2;
Serial.print(cm);
Serial.println("cm case :0 initial");
}
}
tt[j]=cm;
}

n.write(90);
delay(1000);

for(int j=0;j<9;j++)
{
if(min>tt[j])

{
min=tt[j];
p=j;
}
}
l=p*10;
Serial.print("180-(p*10)::::: So,theta= ");
Serial.println((p*10));

if(min>25)
{
Serial.println("::::: GOT a PROBLEM , PROBABLY A OVERTURN :::::");
}
if(p*10!=0){digitalWrite(Mp1,HIGH);
delay(300);
digitalWrite(Mp1,LOW);}
min=500;
}
flag=0;
{
digitalWrite(Trig, LOW);//Get the distance
delayMicroseconds(2);
digitalWrite(Trig, HIGH);
delayMicroseconds(10);
digitalWrite(Trig, LOW);

duration = pulseIn(echo, HIGH);


cm = duration/29/2;
Serial.print(cm);
Serial.println("cm case :0 initial");
delay(50);
if(cm==0)
{
digitalWrite(Trig, LOW);//Get the distance
delayMicroseconds(2);
digitalWrite(Trig, HIGH);
delayMicroseconds(10);
digitalWrite(Trig, LOW);
duration = pulseIn(echo, HIGH);
cm = duration/29/2;
Serial.print(cm);
Serial.println("cm case :0 initial");
}
}
if(cm<20)
{
digitalWrite(Mp1,LOW);
digitalWrite(Mp2,LOW);
flag=1;
break;
}

if(cm>=20 && cm<500)


{
Serial.println("::::: Going Straight :::::");
analogWrite(Mp1,255);
analogWrite(Mp2,120);
delay(300);
analogWrite(Mp1,0);
analogWrite(Mp2,0);
}
break;
}
break;
}

/*Left Turn Completion Detection Algorithm[LTCDA]*/


case 8:{
for(int i=0,j=0;i<90;i=i+10,j++)
{
n.write(i);
delay(500);
{
digitalWrite(Trig, LOW);//Get the distance
delayMicroseconds(2);
digitalWrite(Trig, HIGH);
delayMicroseconds(10);

digitalWrite(Trig, LOW);
duration = pulseIn(echo, HIGH);
cm = duration/29/2;
Serial.print(cm);
Serial.println("cm case :0 { TT1 }");
delay(50);
if(cm==0)
{
digitalWrite(Trig, LOW);//Get the distance
delayMicroseconds(2);
digitalWrite(Trig, HIGH);
delayMicroseconds(10);
digitalWrite(Trig, LOW);
duration = pulseIn(echo, HIGH);
cm = duration/29/2;
Serial.print(cm);
Serial.println("cm case :0 initial");
}
}
tt[j]=cm;
}

n.write(90);
delay(1000);

for(int j=0;j<9;j++)
{
if(min>tt[j])
{
min=tt[j];
p=j;
}
}
r=p*10;
Serial.print("180-(p*10)::::: So,theta= ");
Serial.println((p*10));

if(min>25)
{
Serial.println("::::: GOT a PROBLEM , PROBABLY A OVERTURN :::::");
}
if(p*10!=0){digitalWrite(Mp2,HIGH);
delay(300);
digitalWrite(Mp2,LOW);}
min=500;
}
if(p==0)
{

for(int y=0;y<1;y++)
{
for(int i=0,j=0;i<90;i=i+10,j++)
{
n.write(i);
delay(500);
{
digitalWrite(Trig, LOW);//Get the distance
delayMicroseconds(2);
digitalWrite(Trig, HIGH);
delayMicroseconds(10);
digitalWrite(Trig, LOW);
duration = pulseIn(echo, HIGH);
cm = duration/29/2;
Serial.print(cm);
Serial.println("cm case :0 { TT1 }");
delay(50);
if(cm==0)
{
digitalWrite(Trig, LOW);//Get the distance
delayMicroseconds(2);
digitalWrite(Trig, HIGH);
delayMicroseconds(10);
digitalWrite(Trig, LOW);
duration = pulseIn(echo, HIGH);

cm = duration/29/2;
Serial.print(cm);
Serial.println("cm case :0 initial");
}
}
tt[j]=cm;
}

n.write(90);
delay(1000);

for(int j=0;j<9;j++)
{
if(min>tt[j])
{
min=tt[j];
p=j;
}
}
r=p*10;
Serial.print("180-(p*10)::::: So,theta= ");
Serial.println((p*10));

if(min>25)
{
Serial.println("::::: GOT a PROBLEM , PROBABLY A OVERTURN :::::");
}
if(p*10!=0){digitalWrite(Mp2,HIGH);
delay(300);
digitalWrite(Mp2,LOW);}
min=500;
}
flag=0;
{
digitalWrite(Trig, LOW);//Get the distance
delayMicroseconds(2);
digitalWrite(Trig, HIGH);
delayMicroseconds(10);
digitalWrite(Trig, LOW);
duration = pulseIn(echo, HIGH);
cm = duration/29/2;
Serial.print(cm);
Serial.println("cm case :0 initial");
delay(50);
if(cm==0)
{
digitalWrite(Trig, LOW);//Get the distance
delayMicroseconds(2);

digitalWrite(Trig, HIGH);
delayMicroseconds(10);
digitalWrite(Trig, LOW);
duration = pulseIn(echo, HIGH);
cm = duration/29/2;
Serial.print(cm);
Serial.println("cm case :0 initial");
}
}
if(cm<20)
{
digitalWrite(Mp1,LOW);
digitalWrite(Mp2,LOW);
flag=1;
break;
}
if(cm>=20 && cm<500)
{
Serial.println("::::: Going Straight :::::");
analogWrite(Mp1,150);
analogWrite(Mp2,130);
delay(100);
analogWrite(Mp1,0);
analogWrite(Mp2,0);
}

break;
}
}
}