Wall-following robot

Lab #1 write-up
Michael J. Chan
Ben Hulse
Peter Kasting
2/5/01

Results


Wallfollower #1


The algorithm for the wall follower is a very simple algorithm. The robot starts out, checks all of its sonar readings, and the homes in on the closest wall it can find. Once it finds the wall it rotates to be perpendicular to the wall, and then proceeds to follow that wall, checking only the left 180 degree arc of the sonar pointed towards the wall. Every loop, the robot first checks sonar, then it checks to make sure that the closest sonar reading is in position 21, or the sensor perpendicular to the wall. If the closest sonar reading is not in position 21, the robot rotates so as to get the closest reading into slot 21. If the robot is oriented properly, then it moves forward by setting velocity to 5.0 inches/sec. The robot does not attempt any fine tuned rotations to get the absolute minimum distance to the wall. As such, it is possible for the robot to zig-zag along walls. If the robot is approaching an extremely narrow opening in the wall, it is possible that it may overshoot, find the closest sensor reading in slot 17(straight ahead). As such it would miss a gap in the wall, and orient along the new wall. Example 3 below shows such an instance. Correcting for a narrow gap causes difficulty in navigating the inside 90 degree turns because in order to correct for that problem, one must check only sensors 21-25(side to rear sensors), or to be very careful with how speed is managed. 

#include 
#include 
#include 

#include "Nclient.h"
#define DIST 10
#define TURN_MULT 1

void dumpSensors() {
   for(int i = 0; i < 16; i++) {
      cout << State[17 + i] << " ";
   }
   cout << endl;
}

int main(int argc, char **argv) {
   int min, minIndex; 
   SERV_TCP_PORT = 6666;

   if(connect_robot(1)) {
      cout << "Connected" << endl;
   } else {
      cout << "couldn't find server" << endl;
      exit(0); 
   }
   zr();

   for(int i = 17; i < 33; i++) {
      Smask[i] = 1;
   }
   ct();
   gs();
   bool acquired = false;
   int rv = 0;
   while(1) {
      // Step 1: find closest wall
      if(!acquired) {
         min = State[17];
         minIndex = 17;
         for(int i = 18; i < 33;  i++) {
	   if(State[i]  < min)
   {
   min = State[i]; 
   minIndex = i;  }  }
   } else {
      min =  State[17];
      minIndex = 17;
   for(int i =  18;  i < 25;
      i++)  {
   if(State[i]
   <
   min)
   { min = 
   State[i];
   minIndex = i; } } }
      if(min >
         DIST)  {
         if(minIndex = 
           = 17) {  if(min * 10 >  100) {
	       vm(50, 0, 0);  }
            else
         {
      vm(50 - State[17],
         0,  0);
	 }  }
	 else {  for(int i =   0;
	    i  < minIndex -  17;
	       i++)  {
            vm(min * TURN_MULT, 225,  225); }
         }  } else
	    if (minIndex ! = 21) {
	    if(21 -  minIndex >  0) { vm(min * TURN_MULT,
	       -225, -225); } else {
            vm(min * TURN_MULT, 225, 225); } if(min == State[21])
         {
	 acquired = true;  }
         }
      else { if((State[17]*10)
	 >  50 + (2 * DIST)) {
	    vm(50, 0, 0);
         } else {
	    vm((State[17] * 10) - (2 * DIST), 0, 0);
         }
      }
   }
}



 

Wall follower navigating test world 1:
 


Wall follower navigating test world 2 :
 


Robot navigating sample test world with a narrow gap in a wall plus 90 degree turns:



I tried to come up with a more intelligent way to control the speed, but I could not come up with any value that produced better results than a flat 50. Varying the speed would generally lead to one test case where the robot would crash into a wall. Setting the speed higher than 50 would cause difficulty navigating narrow openings as presented in example 3. If I had followed the walls from a distance greater than 10, it would have been possible to move the robot more quickly, however the robot would have less accurately followed the walls.

Results

Wallfollower #2


The algorithm of this wall-following robot is also fairly simple. First of all, it is purely reactive. It plows ahead until it reaches a wall, so if, for example, the second test-case had been reversed left-to-right, it would have fallen off the map, so perhaps a little tweaking is necessary. It continues ahead until something blocks one or more of its front three sensors. At that point, it rotates left (without moving) until its front is no longer obstructed, and then, it continues blithely ahead. It doesn't move while rotating so it doesn't have to calculate if there is sufficient room to make a curving turn. The left rotation may have gone too far, to which the algorithm compensates by five degree nudges back toward the wall at speed 200. If the robot gets too close to the wall, it nudges away twelve degrees at speed 110 (too much and the robot gets separated from the wall). The amounts are not equal because: 1) it could possibly equalize to a parallel course which it couldn't if the values were equal, and 2) avoid the wall is a higher priority. Should the right wall suddenly fall away, as in a right turn of some angle, the robot begins to instigate a loop to the right. This is done by noting a massive difference between sonar sensor 12 (112.5 degrees right of center) and sonar sensor 13 (due right). The full loop is broken by the continuation of the wall, so the robot continues its standard left-turn algorithm. 
/*
 * survivor.cc
 *
 * This is a skeleton file for controlling the Nomad
 *   simulator in Assignment A on various reactive control strategies.
 *
 * Feel free to use the makefile in this directory also.
 *
 */

#include 
#include 
#include 
#include "KbdInput.hh"

extern "C" {
#include "Nclient.h"
}

/******************************************************************
 *
 * This function gets the shortest sonar reading in a range from
 *
 *   fromSonar to toSonar
 *
 * The index of that minimum reading is returned.
 *
 * Because I think of the sonars as being numbered starting from
 * zero, there is a conversion to the actual location of the 
 * sonar values in the State vector.
 *
 * Note: though input values may be anything bigger than -32, the
 *       index returned is the actual indes into the state vector,
 *       i.e., between 17 and 32 inclusive.
 *
 ******************************************************************/

int
getShortestSonarIndex(int fromSonar, int toSonar)
{
  int realSN;              /* real sonar number in the State vector */
  int minSonar = 1000;     /* higher than any real sonar value      */
  int minSonarIndex;    

    /* swap from and to, if they're not in order */

  if (fromSonar > toSonar) { fromSonar ^= toSonar ^= fromSonar ^= toSonar; }

  for (int i=fromSonar ; i 15) && (front2 > 15) && (front3 > 15))
	{
	    vm(147, -450*dir, -450*dir);
	    gs();
	}
	else break;
    }
}


/****************************
 *
 *  The usual main method
 *
 ****************************/

int
main(int argc, char** argv)
{

  SERV_TCP_PORT = 7390;    /* this is NOT the default tcp port    */

  /*
   * tv is the constant translational velocity of the robot
   */

  int tv = 50;             /* five inches per second default      */

  if (argc > 1)            /* a different velocity can be sent in */
  {
	tv = atoi(argv[1]);
      cout << "Setting translational velocity to " << tv << endl;
  }

  /* 
   * connect to the simulator
   */

  if (connect_robot(1)) 
	printf("Connected to robot\n");
  else {
	printf("Failed to connect to robot\n");
	exit(0);
  }

  /* 
   * zero the robot
   */

  zr();

  /* 
   * turn on appropriate sensors 
   */

  for (int i=17 ; i<=32 ; ++i)  Smask[i] = 1;
  ct();

  gs();                     /* update the robot's state   */

  int rv = 0;               /* start going straight ahead */

  vm(tv,rv,rv);             /* set the initial velocities */

  /*
   * the KbdInput object allows nonblocking input
   */

  KbdInput KI; 
  char c; 

  /*
   * the sense/act loop implementing reactive control
   */

  KI.on();                  /* turn on raw input/output             */
    //  More convenient names
    int sonar1, sonar2, sonar3, sonar4, sonar5, sonar6;
    int sonar12, sonar13, sonar14, sonar15, sonar16;		
    double pi = 3.141592653;		//  Pi : the ratio
    double angle = 0;			//  Target turning angle
    int stop = 0;			//  My flag to cause robot to stop
    int dir = 1;			//  Turn left/right, default:L
    for (int i=1 ; i<42 ; ++i) {Smask[i] = 1;}
    ct();

  while (1) 
  {
    c = KI.nextChar();      /* c is 0 if there's no input ready     */
  
    if ( c == 'q' )         /* hit q in the terminal window to quit */
    {
       break;
    }

    // setup the variables, rename the sensors.  The front 11 are used

    tv = State[38];
    rv = State[39];
    sonar1 = State[17];
    sonar2 = State[18];
    sonar3 = State[19];
    sonar4 = State[20];
    sonar5 = State[21];
    sonar6 = State[22];
    sonar12 = State[28];
    sonar13 = State[29];
    sonar14 = State[30];
    sonar15 = State[31];
    sonar16 = State[32];

    //  If the wall is encountered, turn left
    if ((sonar1 < 20) || (sonar15 < 15) || (sonar3 < 15) || (sonar2 < 15))
    {
	//  If we're just getting too close to the right wall, adjust
	if (sonar1 > 40) { vm (110, 120, 120); }
	//  Else, brake hard for the corner
	else
	{
	    st();
	    int x = sonar3;
	    int y = sonar2;
	    double angle = atan2 ((x*sin(pi/4)-y*sin((3/8)*pi)),
		(x*cos(pi/4)-y*cos((3/8)*pi)));
	    angle = angle * (180/pi);
	    {
		pr (0, 450, 450);
	    }
	}
    }
    //  No wall, keep going
    else
    {
	//  The wall suddenly fell away, right corner?
	if ((sonar13 >= (sonar12 * 3)) && (sonar12 < 20))
	{
	    Uturn(1);
	}
	//  Stick to the wall, don't fall off
	else if (((sonar12 - sonar13*tan(pi/8)) > 4) && (sonar12 < 30))
	{
	    vm(200, -50, -50);
	}
	//  We lost the wall.  Emergency correction
	else if (((sonar12 - sonar13*tan(pi/8)) > 2) && (sonar12 < 50))
	{
	    vm(200, -450, -450);
	}
	//  Everything's fine.  Full speed ahead
	else vm(200,0,0);
    }
    gs();                                     //  update state

  }



  KI.off();                 /* reset terminal settings              */

  disconnect_robot(1);      /* gracefully disconnect from the robot */

}

Basically a closed maze, no complications




An open-ended maze with uneven joints, no complications




A jagged, closed maze, slight loss of wall following on the mid-right side
as the wall falls away gradually, and the robot encounters the next wall
in the middle of the turn.


The robot performed well in the areas of straight corridors and left turns. The performance is a little troublesome at the beginning, as it was noted earlier that it requires that the wall to follow be in its initial path, and also in right turns. The right turns were done in the interests of speed, and as such, they give the edge a moderate berth. They loop around a bit because if it were slower, it might be more accurate though sometimes collisions occurred. If faster, it sometimes lost track of the wall by flying too far off-course. The robot is fast, but it can miss corridors of a certain size.