Grid Follower using Arduino UNO R3.
code : not tested yet
int rmf=12;
int rmr=13;
int lmf=11;
int lmr=10;
int xxls=2;
int xls=3;
int ls=4;
int cs1=5;
int cs2=6;
int rs=7;
int xrs=8;
int xxrs=9;
int cx=0;
int cy=0;
int dir='N';
void setup()
{
pinMode(rmf,OUTPUT);
pinMode(rmr,OUTPUT);
pinMode(lmf,OUTPUT);
pinMode(lmr,OUTPUT);
pinMode(xxls,INPUT);
pinMode(xls,INPUT);
pinMode(ls,INPUT);
pinMode(cs1,INPUT);
pinMode(cs2,INPUT);
pinMode(rs,INPUT);
pinMode(xrs,INPUT);
pinMode(xxrs,INPUT);
Serial.begin(9600);
}
void loop()
{
int dx,dy;
if(dx-cx>0 && dy-cy==0)
{ //Move along positive x-axis
if(dir=='N')
{
grid_right();
grid_forward(dx-cx);
}
else if(dir=='E')
{
grid_forward(dx-cx);
}
else if(dir=='S')
{
grid_left();
grid_forward(dx-cx);
}
else if(dir=='W')
{
uturn();
grid_forward(dx-cx);
}
cx+=dx-cx;
}
if(dx-cx==0 && dy-cy>0)
{//Move along positive y-axis
if(dir=='N')
{
grid_forward(dy-cy);
}
else if(dir=='E')
{
grid_left();
grid_forward(dy-cy);
}
else if(dir=='W')
{
grid_right();
grid_forward(dy-cy);
}
else if(dir=='S')
{
uturn();
grid_forward(dy-cy);
}
cy+=dy-cy;
}
if(dx-cx<0 && dy-cy==0)
{//Move along negative x-axis
if(dir=='N')
{
grid_left();
grid_forward(cx-dx);
}
else if(dir=='S')
{
grid_right();
grid_forward(cx-dx);
}
else if(dir=='W')
{
grid_forward(cx-dx);
}
else if(dir=='E')
{
uturn();
grid_forward(cx-dx);
}
cx-=cx-dx;
}
if(dx-cx==0 && dy-cy<0)
{//Move along negative y-axis
if(dir=='W')
{
grid_left();
grid_forward(cy-dy);
}
else if(dir=='E')
{
grid_right();
grid_forward(cy-dy);
}
else if(dir=='S')
{
grid_forward(cy-dy);
}
else if(dir=='N')
{
uturn();
grid_forward(cy-dy);
}
cy-=cy-dy;
}
}
void grid_forward(int count)
{
int temp=0;
while(1)
{
int sxxls=digitalRead(xxls);
int sxxrs=digitalRead(xxrs);
if(sxxls==HIGH && sxxrs==HIGH)
{
stopp();
delay(1000);
temp++;
if(temp==count)
{
stopp();
return;
}
else
{
forward();
delay(1000);
}
}
line_follow();
}
}
void line_follow()
{
int sxls=digitalRead(xls);
int sls=digitalRead(ls);
int scs1=digitalRead(cs1);
int scs2=digitalRead(cs2);
int srs=digitalRead(rs);
int sxrs=digitalRead(xrs);
if( sxls==LOW && sls==LOW && scs1==LOW && scs2==LOW && srs==LOW && sxrs==LOW )
{
stopp();
delay(10);
}
if( sxls==LOW && sls==LOW && scs1==HIGH && scs2==HIGH && srs==LOW && sxrs==LOW )
{
forward();
delay(10);
}
if( sxls==HIGH && sls==HIGH && scs1==HIGH && scs2==HIGH && srs==LOW && sxrs==LOW )
{
left();
}
if( sxls==LOW && sls==LOW && scs1==HIGH && scs2==HIGH && srs==HIGH && sxrs==HIGH )
{
right();
}
}
void grid_right()
{
int sxxrs=digitalRead(xxrs);
right();
delay(1000);
while(sxxrs==HIGH)
right();
if(dir=='N')
dir='E';
else if(dir=='E')
dir='S';
else if(dir=='S')
dir='W';
else if(dir=='W')
dir='N';
}
void grid_left()
{
int sxxls=digitalRead(xxls);
left();
delay(1000);
while(sxxls==HIGH)
left();
if(dir=='N')
dir='W';
else if(dir=='E')
dir='N';
else if(dir=='S')
dir='E';
else if(dir=='W')
dir='S';
}
void uturn()
{
int sxxrs=digitalRead(xxrs);
grid_right();
right();
delay(2000);
while(sxxrs==HIGH)
right();
if(dir=='N')
dir='E';
else if(dir=='E')
dir='S';
else if(dir=='S')
dir='W';
else if(dir=='W')
dir='N';
}
void forward()
{
digitalWrite(lmf,HIGH);
digitalWrite(rmf,HIGH);
}
void stopp()
{
digitalWrite(lmf,LOW);
digitalWrite(rmf,LOW);
}
void left()
{
digitalWrite(rmf,HIGH);
digitalWrite(lmr,HIGH);
}
void right()
{
digitalWrite(lmf,HIGH);
digitalWrite(rmr,HIGH);
}
No comments:
Post a Comment