dhanapune.c

Autonomous navigating robot(自动驾驶机器人)

/*---------------------------------------------------------- 
NAVIGATING ROBOT 

Spring 2004: Fianl Project ECE476 
Presented by: 
Pranay Ahlawat (pa63) 
Abhijeet Dhanapune (ad284) 


---------------------------------------------------------*/ 

#include<Mega32.h> 
#include< delay.h > 
//define directions 
#define straight 0 
#define back 1 
#define left 2 
#define right 3 
#define stop 4 
#define stepleft 5 
#define stepright 6 

//the turning var 旋转变量
#define leftturnvar 300 
#define rightturnvar 300 
#define forwardvar 400 
#define t1 3 

//define sensors 传感器
#define LS PIND.5 //top left sensor 
#define RS PIND.4 // top right sensor 
#define obstacle PINB.3 //distance sensor 远距离传感器

//define varriables 使能
char time=t1; 
char a=0; 
unsigned char x=1+8; 
unsigned char stepL ; 
unsigned char stepR ; 
unsigned char dir; 

//hardcode the final destinations for the sensors ******//hardcode 最后目的地给传感器
#define ultimatex 3 
#define ultimatey -2 
char xfinal =3; 
char yfinal =-2; 
char interx =0; 
char intery =0; 
char temp1x=0; //for one of the history 
char temp1y=0; 
char temp2x=0; //for the other history 
char temp2y=0; 

//define directions as numbers - this will be used as the orientation refference 
#define plusx 1 
#define minusx 2 
#define plusy 3 
#define minusy 4 

// varriables to keep track of the current location ******//varriables 跟踪现行存储单元
char currentx =0; 
char currenty =0; 
//define initial varriables 
char orientation=1; //the list of orientation is given in the#difines above 
char temp; 
int count,i ; 

//define the functions 
void initialize(void); 
char backward(char x); 
char forward(char x); 
void move (void); 
void ai (void); 
void rightturn (void); 
void leftturn (void); 
unsigned char obstacleflag =0; 
void sound(void); 
void force( int ); 

//ISR 0 definition 
interrupt [TIM0_COMP] intaaaa (void) 
{ 
--time; 
} 


//initialization of the data direction bits and also the timer constants 
void initialize(void) 
{ 
DDRC = 0xff; // MSB for L and LSB for R 
DDRD = 0x00; // higher byte debug with led and lower byte for line sensors 
DDRB=0x00; 
DDRA = 0xff;//00000010; 
TCCR0= 0b00001011; 
OCR0=249; 
TIMSK=0b00000010; 
# asm 
sei 
# endasm 
PORTD.4 = 1; 
PORTD.5 = 1; 
PORTD.6 = 1; 
for( i =0;i<50;i++) 
sound(); 
//for motor 
dir = back; 
count = 0; 
} 


//force function - force moves in a particular direction 
void force( int a) 
{ 
for( i =0;i< a;i ++) 
{ 
move(); 
delay_ms (2); 
} 
} 


//begin the main routine 
void main(void) 
{ 
initialize(); 

// Intellifent determination of the final coordiantes 
// interx =char(ultimatex/2); 
// intery =char(ultimatey/2); 

interx =1; 
intery =1; 
while(1) 
{ 
if (time==0) 
{ 
time=t1; 

//realign 
PORTA.6=0; 
//realign 
if (!LS) PORTA.5=1; //LS:PIND.5
else PORTA.5=0; 
if (!RS)PORTA.4=1;  //RS:PIND.4
else PORTA.4=0; 

if (obstacle)        //obstacle:PINB.3
{ 
sound(); 


switch(orientation) //方向
{ 
case( plusx ): 
  { 
if ( yfinal > currenty ) 
    { 
     xfinal = currentx;
     yfinal =currenty+1;
	 leftturn();
	 orientation= plusy ;
	} 
  else 
    if( yfinal < currenty ) 
	  { 
	  xfinal = currentx;
	  yfinal =currenty-1;
	  rightturn();
	  orientation= minusy ;
	  } 
    else 
	  { 
	  xfinal = interx;
	  yfinal = intery;
	  rightturn (); 
	  rightturn ();
	  orientation= minusx ;
	  } 
     break; 
  } 
case( minusx ): 
  { 
   if ( yfinal < currenty ) 
      { 
	  xfinal = currentx;
	  yfinal =currenty-1;
	  leftturn();
	  orientation= minusy ;
	  } 
    else 
	  if( yfinal > currenty ) 
	  { 
	  xfinal = currentx;
	  yfinal =currenty+1;
	  rightturn();
	  orientation= plusy ;
	  } 
     else 
	   { 
	   xfinal = interx;
	   yfinal = intery;
	   rightturn (); 
	   rightturn ();
	   orientation= plusx ;
	   } 
     break; 
  } 
case( plusy ): 
  { 
   if ( xfinal < currentx ) 
    { 
     yfinal = currenty;
	 xfinal =currentx-1;
	 leftturn();
	 orientation= minusx ;
    } 
   else 
     if ( xfinal > currentx ) 
	 { 
	 yfinal = currenty;
	 xfinal =currentx+1;
	 rightturn();
	 orientation= plusx ;
	 } 
    else 
	 { 
	 xfinal = interx;
	 yfinal = intery;
	 rightturn (); 
	 rightturn ();
	 orientation= minusy ;
	 } 
   break; 
  }  
case( minusy ): 
  { 
   if ( xfinal > currentx ) 
    { 
    yfinal = currenty;
    xfinal =currentx+1;
    leftturn();
    orientation= plusx ;
    } 
   else 
     if ( xfinal < currentx )
	 { 
	 yfinal = currenty;
	 xfinal =currentx-1;
	 rightturn();
	 orientation= minusx ;
	 } 
    else 
	  { 
	  xfinal = interx;
	  yfinal = intery;
	  rightturn (); 
	  rightturn ();
	  orientation= plusy ;
	  } 
   break; 
  } 
} 
if ((temp2x== currentx && temp2y== currenty ) || (temp2x== currentx && temp2y== currenty )) //if its any one of 
//the previous two encountered then goto the middle position and then refind the way 
{ xfinal = interx;yfinal = intery ;} 

if (obstacle) { xfinal = interx;yfinal = intery ;} //if you find another obstacle then move 
//to the middle position - this is again to avoid an L shaped obstacle 

//update the history of the robot 
temp1x=temp2x; 
temp1y=temp2y; 
temp2x= currentx ; 
temp2y= currenty ; 
} 


if(!LS) //if the left sensor is detected then turn left to allign 如果左传感器被识别，那向左拐
{ 
dir=left; 
} 

if(!RS) //if the right sensor is detected then take a right to allign the robot 如果右边的传感器被识别，那向右拐
{ 
dir=right; 
} 

if (LS && RS) // if none then keep moving straight 如果没有,直接往前走
{ 
dir = straight; 

} 


if (!LS && ! RS) //if the sensors become alligned then move straight and call the ai 如果传感器成为alligned，那么直接移动并且叫ai
{ 
dir=straight; 
PORTA.6=1; 
force( forwardvar ); 
ai (); 
} 
move(); 

} 
} 
} 

void move (void) //move the robot depending on the action requested 
{ 
if (dir == stepleft ) 
{ 
stepL =backward( stepL ); 
} 
if (dir == stepright ) 
{ 
stepR =backward( stepR ); 
} 
if(dir == straight) 
{ 
stepR = forward( stepR ); 
stepL = backward( stepL ); 
} 
if(dir == left) 
{ 
stepR = backward( stepR ); 
stepL = backward( stepL ); 
} 
if(dir == right) 
{ 
stepR = forward( stepR ); 
stepL = forward( stepL ); 
} 
if(dir == back) 
{ //for a wheel to go left 
stepR = backward( stepR ); 
stepL = forward( stepL ); 
} 
if (dir == stop) 
{ 
stepR =0; 
stepL =0; 
} 
PORTC = ( stepL <<4)| stepR ; 
} 

char backward(char x) //for a wheel to go left 
{ 
switch(x) 
{ 
case (8+1): 
{ 
x=4+8; 
break; 
} 
case 1+2: 
{ 
x=8+1; 
break; 
} 
case (2+4) : 
{ 
x=1+2; 
break; 
} 
case 4+8 : 
{ 
x=2+4; 
break; 
} 
default:x =8+1; 
} 
return x; 
} 



char forward(char x) //for a wheel to go forward 
{ 
switch(x) 
{ 
case (8+1): 
{ 
x=1+2; 
break; 
} 
case 1+2: 
{ 
x=2+4; 
break; 
} 
case (2+4) : 
{ 
x=4+8; 
break; 
} 
case 4+8 : 
{ 
x=8+1; 
break; 
} 
default: x=8+1; 
} 
return x; 
} 
void sound() 
{ 
PORTA.3=1; 
for ( i =0;i<100;i++); 
PORTA.3=0; 
} 
void ai (void) 
{ 
// char xdiff ; //define varaibles that will be the differece in the final and the current position 
//char ydiff ; 

switch (orientation) //update the value of the current coordiantes because the robot just crossed the line 
{ 
case ( plusx ): 
{ 
currentx ++; 
break; 
} 
case ( minusx ): 

{ 
currentx --; 
break; 
} 
case ( plusy ): 
{ 
currenty ++; 
break; 
} 
case ( minusy ): 
{ 
currenty --; 
break; 
} 
} 

//if there is an obstacle then change the finalx and finaly coordinates to make the thing go to the left block 


switch(orientation) 
{ 


case ( plusx ): 
{ 
if ( currentx > xfinal ) //if you have overpassed the point and the direction is plusx take a u turn 
{ 
rightturn (); 
rightturn (); 
orientation= minusx ; 
} 
if ( currentx == xfinal ) //if you have covered the x coordinate .. 
{ 
if ( yfinal > currenty ) //and you are away from y - take a left 
{ 
leftturn (); 
orientation= plusy ; 
} 
if ( yfinal < currenty ) //otherwise take a right to hone in on the target 
{ 
rightturn (); 
orientation= minusy ; 
} 
} 
break; 
} 


case ( minusx ): //if the orintation is -x 
{ 

if ( currentx < xfinal ) //take a uturn if you are opposite to where you have to go 
{ 
rightturn (); 
rightturn (); 
orientation= plusx ; 
} 
if ( currentx == xfinal ) 
{ 
if ( yfinal > currenty ) //if you have achieved x then take a right if you have to treverse positive y 
{ 
rightturn (); 
orientation= plusy ; 
} 
if ( yfinal < currenty ) //take a left turn if you have to go to a y that is negative with respect to position 
{ 
leftturn (); 
orientation= minusy ; 
} 
} 
break; 
} 


case ( plusy ) : //if the orientation is plusy 
{ 

if ( yfinal < currenty ) //take a uturn if you are going in the opposite direction 
{ 
rightturn (); 
rightturn (); 
orientation= minusy ; 
} 
if ( currenty == yfinal ) //if the y direction has been acheieved then 
{ 
if ( currentx < xfinal ) //if you have to go to x take a right 
{ 
rightturn (); 
orientation= plusx ; 
} 
if ( currentx > xfinal ) //otherwise if you have a negative distance take a left 
{ 
leftturn (); 
orientation= minusx ; 
} 
} 
break; 
} 
case ( minusy ): // if you are going in the negative y direction 
{ 

if ( yfinal > currenty ) //if you are travelling away from the target take a u turn 
{ 
rightturn (); 
rightturn (); 
orientation= plusy ; 
} 
if ( currenty == yfinal ) // otherwise if y has been achieved 
{ 
if ( currentx < xfinal ) 
{ 
leftturn (); //and you are away from x in + - take a left 
orientation= plusx ; 
} 
if ( currentx > xfinal ) 
{ 
rightturn (); //otherwise take a right 
orientation= minusx ; 
} 
} 
break; 
} 
} 


if((orientation!= plusx ) && (orientation!= minusy ) && (orientation!= minusx ) && (orientation!= plusy )) 
while(1) 
{ 
sound(); 
} 


if ( currentx == xfinal && currenty == yfinal ) // if you have achieved the temporary target 
{ 
xfinal = ultimatex ; //change the target to the final destination 
yfinal = ultimatey ; 
} 

if ( currentx == ultimatex && currenty == ultimatey ) // if you have achieved the final destination 
{ 
while(1) //stop and make a sound 
{ 
sound(); 
} 
} 

} 

void rightturn (void) //to turn right 
{ 
dir=right; 
force( rightturnvar ); 
dir=straight; 
} 


void leftturn (void) //to tuen left 
{ 
dir=left; 
force( leftturnvar ); 
dir=straight; 
}