Arduino Wavefront Algorithm

First Up... If you want to find out what Wavefront Mapping is all about head over to the Society of Robots Tutorial. It has a great wtite up and its where I first learnt about it.

Basic's done, So lets talk about Map size limits.....

The Arduino 2560 & 1280 both have 8k of SRAM (where the map is stored)

I'm using an array of Int's, each Int is 2 Bytes, so max size would be the square root of 8k / 2.

8192 / 2 = 4096

Sqaure Root of 4096 = 64

REMEMBER, You also need this memory to store other variables, Hence I stick to a map size of 50 x 50.

You could make the map array using bytes to increase the size upto 90ish, but I decided on Int so I could store more information (eg. room numbers, each room has a map, all maps are stored in EEPROM or on a SD Card and gets loaded into SRAM as needed. This is my end goal).

Now, the number values I have choosing to use are as follow's

0 = Clear Space

1-10 = Object Probability (every time an Object is scanned in that space it's probability is increased by 1)

11 = The Robot

12-239 is for Path Solving

240 = The Goal

> 240 are kept free just in case they are needed.

You'll notice all the values will fit in a byte.

Finally some Code!

You need to define the Map X & Y and the Map array size:

int mapX = 50;       //Maps X size
int mapY = 50;      //Maps Y size
int MAP[50][50];   //The Map Array

Now the Wave fill will only work it you have a goal, 240 and a Robot, 11.

The following Function fills in the map counting down, radiating out from the goal.

void waveFill()
{
  int minVal;
  int reset_minVal = 13;  //Lowest value to go to. 1-10 = Used for Objects, 11 = Robot, 240 = goal
  for (int i = 0; i < 239; i++)   //Scroll through find solution "i" times, IF IT LEAVES WHITE WITHOUT SOLVING INCREASE THIS
  {
    for (int x = 0; x < mapX; x++)   //Scroll down the MAP Array
    {
      for (int y = 0; y < mapY; y++)   //Scroll across the MAP Array
     {     
       //if Location is a clear or the Goal
       if (MAP[x][y] == 0 || MAP[x][y] == 11) //THEN FIND THE HIGHEST VALUE AROUND CURRENT COORDINATE
       {
         minVal = reset_minVal;  //Clear minVal data from last round
         //Right   ***************************************************************
         if (x < mapX - 1)//not out of boundary
           if (MAP[x + 1][y] > minVal && MAP[x + 1][y] < 241) 
           {   //IF TO THE RIGHTS VALUE IS GREATER THAN minVal & IS IN RANGE
             minVal = MAP[x + 1][y];
           }
         //Left   ****************************************************************
         if (x > 0)
           if (MAP[x - 1][y] > minVal && MAP[x - 1][y] < 241) 
           {   //IF TO THE LEFTS VALUE IS GREATER THAN minVal & IS IN RANGE
             minVal = MAP[x - 1][y];
           }
         //Down  *****************************************************************
         if (y < mapY - 1)
           if (MAP[x][y + 1] > minVal && MAP[x][y + 1] < 241) 
           {   //IF BELOWS VALUE IS GREATER THAN minVal & IS IN RANGE
             minVal = MAP[x][y + 1];
           }
         //Up  *******************************************************************
         if (y > 0)
           if (MAP[x][y - 1] > minVal && MAP[x][y - 1] < 241) 
           {   //IF ABOVES VALUE IS GREATER THAN minVal & IS IN RANGE
             minVal = MAP[x][y - 1];
           }

         //If Value > reset_minVal && Location is a Robot. SOLVED & EXIT
         if (minVal > reset_minVal && MAP[x][y] == 11) 
         {
           return;
         }
         else if (minVal != reset_minVal)  //If Value doesn't equal the reset value change current coordinate with the min value found - 1
         {
           MAP[x][y] = minVal - 1;
         }
       }
     }
   }
 }
            //0 = Clear, 1-10 = Object, 11 = Robot, 240 = Goal, Above 240 Reserved for Upgrades
}

Next is the Clear Map function, it removes the mapping numbers (Including the Goal) only.

void clearMap()
{
  for (int x = 0; x < mapX; x++)   //Scroll down the MAP Array
  {
    for (int y = 0; y < mapY; y++)   //Scroll across the MAP Array
    {
      if (MAP[x][y] > 11 && MAP[x][y] < 241)  //If the map Coordinate is between 11 & 240 reset to Zero
      {
        MAP[x][y] = 0;
      }
    }
  }
}

I also use a printMap function to test it.

void printMap()
{
  for (int x = 0; x < mapX; x++)   //Scroll down the MAP Array
  {
    for (int y = 0; y < mapY; y++)   //Scroll across the MAP Array
    {
      Serial.print(MAP[x][y]);
      Serial.print(",");
    }
    Serial.println();
  }
}

Now if you'd like to test it you can preload a map by defining the Array, An Example is below. NOTE: I'm using 10 for the walls, and The goal and robot is also defined.

int mapX = 10;
int mapY = 10;
int MAP[10][10] = 
  {
    {10,10,10,10,10,10,10,10,10,10},
    {10, 0, 0, 0,10, 0, 0, 0, 0,10},
    {10, 0,11, 0,10, 0,240, 0, 0,10},    
    {10, 0, 0, 0,10, 0, 0, 0, 0,10},
    {10, 0, 0, 0,10,10,10, 0, 0,10},
    {10, 0,10, 0,10, 0, 0, 0, 0,10},
    {10, 0,10, 0,10, 0, 0, 0, 0,10},
    {10, 0,10,10,10, 0,10,10,10,10},
    {10, 0, 0, 0, 0, 0, 0, 0, 0,10},
    {10,10,10,10,10,10,10,10,10,10}
  };

Next up is populating the map from sensor readings and moving the Robot.

You'll need to know the robots coordinates and the direction it's facing, so...

int robotDir, robotX, robotY;

For the robotDir, I've used the following, 1 = North, 2 = East, 3 = South, 4 = West.

Now this would be ALOT simpler if the robot was only range finding straight ahead, But where would the fun be in that?

My Normal setup in an Sharp IR (mounted vertically) on top of a servo, Using this and some Trig Maths we can scan 180 degrees and add everything in range to the map. This also increases the object probablity a fair bit as quite a few IR reads will affect one grid coordinate.

So the Maths involved is simply a Sin function(haha), Sin(Angle) = Opposite / Hypotenuse. Since we know the Angle(servo angle) and the Hypotenuse(Distance Measured) the equation can be derived to Sin(Angle) * Hypotenuse = Opposite. Using this we can work out the Opposite & Adjacent lengths (X and Y), Adding or Subtracting these to/from the robots location depending on the direction gives us the objects location relative to the robot. Just to make it that bit more fun, Arduino (c) uses Radians and not Degrees, to convert Degrees to Radians with as little floating point maths(takes longer) we times the degrees by 71 then divide the total by 4068, (Degrees * 71) / 4068.

The last bit to mention before the code is the Map Resolution, I tend to work my IR Readings into cm's. Then within the mapObject function I can change the resolution easily. eg, each of my coordinates is a 10cm by 10cm square, by sending the servoAngle (0-180) and the cm's measured I can then simply divide by 10 in the first line of code to calculate the resolution.

I also check for the simple angles first 0,90 & 180 as this avoids all the maths.

The Code:

void mapObject(int servoAngle, int distanceMeasured)
{             //Servo Angle, in Degrees 
              //Distance Sensor Value, in cm's
  distanceMeasured = distanceMeasured / 10     //Change the number to change the Map Resolution
  if (servoAngle == 0)  //Checks for 0degrees first, skips the other calc's if not needed
  {  
    switch(robotDir) //adds object to map
    {
      case 1:   //NORTH
        addObject(robotX - distanceMeasured, robotY);
        return;
      case 2:   //EAST
        addObject(robotX, robotY - distanceMeasured);
        return;
      case 3:   //SOUTH
        addObject(robotX + distanceMeasured, robotY);
        return;
      case 4:   //WEST
        addObject(robotX, robotY + distanceMeasured);
        return;
      //No default as it's always one of these
      //return is used instead of break to exit the whole function   
    }
  } 
  else if (servoAngle == 90)  //Checks for 90degrees first, skips the other calc's if not needed
  {  
    switch(robotDir) //adds object to map
    {
      case 1:   //NORTH
        addObject(robotX, robotY - distanceMeasured);
        return;
      case 2:   //EAST
        addObject(robotX + distanceMeasured, robotY);
        return;
      case 3:   //SOUTH
        addObject(robotX, robotY + distanceMeasured);
        return;
      case 4:   //WEST
        addObject(robotX - distanceMeasured, robotY);
        return;
      //No default as it's always one of these
      //return is used instead of break to exit the whole function   
    }
  } 
  else if (servoAngle == 180)  //Checks for 180degrees first, skips the other calc's if not needed
  {  
    switch(robotDir) //adds object to map
    {
      case 1:   //NORTH
        addObject(robotX + distanceMeasured, robotY);
        return;
      case 2:   //EAST
        addObject(robotX, robotY + distanceMeasured);
        return;
      case 3:   //SOUTH
        addObject(robotX - distanceMeasured, robotY);
        return;
      case 4:   //WEST
        addObject(robotX, robotY - distanceMeasured);
        return;
      //No default as it's always one of these
      //return is used instead of break to exit the whole function   
    }
  } //THE ABOVE MEANS IT WILL SKIP THE FLOAT POINT MATH IF IT CAN
  else
  {        
    if (servoAngle < 90)  //If Angle is under 90degress it subtracts the Sin0 result from the robots Location
    {                                //(to the Left of whatever direction the robot is facing)
      servoAngle = 90 - servoAngle;     
      switch (robotDir)
      {
        case 1:  //NORTH
          addObject(robotX - (sin((float)(servoAngle * 71) / 4068) * distanceMeasured), robotY - (sin((float)((90 - servoAngle) * 71) / 4068) * distanceMeasured));
          return;
        case 2:  //EAST
          addObject(robotX + (sin((float)((90 - servoAngle) * 71) / 4068) * distanceMeasured), robotY - (sin((float)(servoAngle * 71) / 4068) * distanceMeasured));
          return;
        case 3:   //SOUTH
          addObject(robotX - (sin((float)(servoAngle * 71) / 4068) * distanceMeasured), robotY + (sin((float)((90 - servoAngle) * 71) / 4068) * distanceMeasured));
          return;
        case 4:  //WEST
          addObject(robotX - (sin((float)((90 - servoAngle) * 71) / 4068) * distanceMeasured), robotY + (sin((float)(servoAngle * 71) / 4068) * distanceMeasured));
          return;
      }  //no default as it'll always be one of these values
    }
    else  //If Angle is over 90degress it Adds the Sin0 result to the robots location
    {                    //(to the Right of whatever direction the robot is facing)
      servoAngle = servoAngle - 90;
      switch (robotDir)
      {
        case 1:  //NORTH
          addObject(robotX + (sin((float)(servoAngle * 71) / 4068) * distanceMeasured), robotY - (sin((float)((90 - servoAngle) * 71) / 4068) * distanceMeasured));
          return;
        case 2:  //EAST
          addObject(robotX + (sin((float)((90 - servoAngle) * 71) / 4068) * distanceMeasured), robotY + (sin((float)(servoAngle * 71) / 4068) * distanceMeasured));
          return;
        case 3:   //SOUTH
          addObject(robotX + (sin((float)(servoAngle * 71) / 4068) * distanceMeasured), robotY + (sin((float)((90 - servoAngle) * 71) / 4068) * distanceMeasured)); 
          return;
        case 4:  //WEST
          addObject(robotX - (sin((float)((90 - servoAngle) * 71) / 4068) * distanceMeasured), robotY - (sin((float)(servoAngle * 71) / 4068) * distanceMeasured));
          return;
      }  //no default as it'll always be one of these values
    }
  }
}

The addObject function is...

void addObject(int x, int y)  //Increase the probablity of that square being occupied
{
 /* Serial.print(x);   //FOR TESTING
  Serial.print(", ");
  Serial.println(y); */
  
  if(x <= mapX && y <= mapY) //ERROR CHECK IS X & Y  INSIDE MAP BOUNDRY
  {
    if(MAP[y][x] < 10)
    {
      MAP[y][x] = MAP[y][x] + 1;
    }  
  }
/*  else
  {
    //UPGRADE: IF OTHER SIDE OF MAP IS FREE MEMCPY THE WHOLE MAP ACROSS THE REQUIRED SPACE THAN ADD OBJECT
  }  */
  
  //NEED TO FIND A WAY TO DECREASE OBJECT PROBABILITY AND CLEAR COORDINATES CLOSER TO THE ROBOT
  //EASY TODO IF READING IS AT A RIGHT ANGLE AHEAD
}

....To Be Continued....