Wall Following Code

• December
• 18

Documentation:  Here is the archive containing the source code and the documentation below.

It took around a month of reading and experimentation to get
to the point that the robot runs and enough knowledge was gathered to enable
the robot to have decision making capabilities. 

WallFollow Version I:

The initial wall-following algorithm was written in a day
using methods located in the roombacomm library.  The algorithm utilizes the robot’s two bumper
sensors and its wall following sensor.  Note that there is only one wall sensor, which
is located on the right side of the robot.

Maintaining a straight line:

This part of the program is very robust and should not need
any modification.

When the robot is following the wall, it will clip its right
bumper sensor if it veers towards the right and the wall sensor will not detect
the wall if it veers too much to the left.

The algorithm for following the wall is such that robot
would go straight as long as the wall sensor detects the wall. If the right
bump sensor is engaged (veering towards the right), the robot will spin left
until the sensor disengages. If the wall sensor does not detect the wall
(veering towards the left), it will spin right until the wall sensor senses the
wall again.

if (roombacomm.wall())

            {

                  roombacomm.goStraightAt(speed);

                 

                  //turns left
if right bumper is engaged

                  if (roombacomm.bumpRight())

                  {    

                        while(roombacomm.bumpRight())

                        {

                              roombacomm.updateSensors();

                              roombacomm.spinLeftAt(speed);

                        }

                  }

            }

           

            if (!roombacomm.wall())

            {

                  double angle = 0;

                  while (!roombacomm.wall())

                  {

                        roombacomm.updateSensors();

                        roombacomm.spinRightAt(speed);

                        angle += roombacomm.angle();

                        System.out.println(”angle = ” +angle);

                  }

            }

Finding a wall:

This algorithm needs improvement,
especially finding a wall that is not in the path of the robot (case 2). It
works fairly consistently with respect to 90 degree intersections otherwise it
will take a few tries to find the wall.

There are two types of walls that
the robot will try to find: ones that the adjoining wall is in the path of the
robot, and ones that are not. Green square represents the robot and the lines
represent the wall.

  

  
Case 1:               Case 2:

                 

     

If the second wall is in the path of the robot (case
1), it will cause the front bumper to engage, which will trigger both the left
and right wall sensors. The robot will spin 90 degrees left when the front
bumper is engaged and the wall following algorithm continues to work.

If the second wall is not in the path of the robot
(case 2), the wall sensor will be disengaged, causing the robot to continuously
spin right, trying to find the wall that it lost. Since the robot only needs to
turn a few degrees before the wall is found again, it was assumed that robot
was at the edge of the wall, its following if its
angle measurement is going to be greater than a few degrees. 90 degrees was
used as the determining measurement. If the robot spun more than 90 degrees to
the right, it will spin 90 degrees left, go a set distance straight without
reliance of the wall sensor, turn right and go straight a set distance and
hopefully the wall sensor would detect the presence of a wall. This part of the
algorithm was placed into the if(!roombacomm.wall())
statement.

 -65 degrees
was used instead of 90 degrees because the angle returns a negative number when
the robot turns left and the sensor lost its accuracy, and spinning 65 degrees
resulted in the robot spinning 90 degrees in actuality. A while statement was
used for the robot going straight to improve accuracy when it is leaving the
first wall. 225mm was determined through experimentation. More explanation of
the accuracy of roombacomm’s methods will be offered
in the second version of the wall following algorithm.

if (!roombacomm.wall())

            {

                  double angle = 0;

                  while (!roombacomm.wall())

                  {

                        roombacomm.updateSensors();

                        roombacomm.spinRightAt(speed);

                        angle += roombacomm.angle();

                        System.out.println(”angle = ” +angle);

                       

     

                        if (angle <
-65)

                        {

                              roombacomm.spinLeft(90);

                              double distance = 0;

                              while (distance < 225)

                              {

                                    roombacomm.updateSensors();

                                    roombacomm.goStraightAt(100);

                                    distance +=roombacomm.distance();

                              }

                             

                              roombacomm.spinRight(90);

                              roombacomm.goStraight(250);

                              angle = 0;

                              break;

                        }                

                  }    

}

     

The whole wall following code became:

        int speed = 100;

        boolean done = false;

        while(!done)

        {

            roombacomm.updateSensors();

           

            //turns 90
degrees left if front bumper is engaged

            if (roombacomm.bumpLeft()
&& roombacomm.bumpRight())

            {

                  roombacomm.spinLeft1(speed,90);

            }

           

            //goes
straight if wall sensor is engaged

            if (roombacomm.wall())

            {

                  roombacomm.goStraightAt(speed);

                 

                  //turns left
if right bumper is engaged

                  if (roombacomm.bumpRight())

                  {    

                        while(roombacomm.bumpRight())

                        {

                              roombacomm.updateSensors();

                              roombacomm.spinLeftAt(speed);

                        }

                  }

            }

            //turns right
if wall sensor isn’t engaged

            if (!roombacomm.wall())

            {

                  double angle = 0;

                  while (!roombacomm.wall())

                  {

                        roombacomm.updateSensors();

                        roombacomm.spinRightAt(speed);

                        angle += roombacomm.angle();

                        System.out.println(”angle = ” +angle);

                       

                        /*wall finding
algorithm, engages if robot

                          turns right more
than 90 degrees*/

                        if (angle < -65)

                        {

                              roombacomm.spinLeft(90);

                              double distance = 0;

                              while (distance < 225)

                              {

                                    roombacomm.updateSensors();

                                    roombacomm.goStraightAt(100);

                                    distance +=roombacomm.distance();

                              }

                             

                              roombacomm.spinRight(90);

                              roombacomm.goStraight(250);

                              angle = 0;

                              break;

                        }

                  }    

            }

        }

Version (Still WallFollowingV2, since there aren’t
any major modifications)

Roombacomm’s
sensor readings are not accurate. Version 2.0 attempts to correct this problem.
The distance and angle method that roombacomm used
was actually telling telling the robot to go at a
certain speed for a certain time. Methods were modified to use distance and
angle sensors. It was also modified to accept a given speed.

Modified methods within roombacomm:

    public void goStraight1 ( int speed_local, int distance) {

        double distance_counter = 0;

        while (distance_counter < distance)

           

        {

            updateSensors();

            distance_counter += Math.abs(distance());

            goStraightAt(speed_local);

           

        }

    }

public void spinLeft1(int speed_local, int angle) {

      double angle_counter = 0;

      while (angle_counter < angle)

      {

            updateSensors();

            angle_counter+=Math.abs(angle());

            spinLeftAt(speed_local);

      }

    }

public void spinRight1(int speed_local, int angle) {

      double angle_counter
= 0;

      while (angle_counter <
angle)

      {

            updateSensors();

            angle_counter+=Math.abs(angle());

            spinRightAt(speed_local);

      }

    }

Version 3.0 (WallFollowingV2)

Methods were added to roombacomm
to try to improve finding the wall that is not in the path of the robot. If
robot clips the right bumper, it will spin left 30 degrees and if the front
bumper is clipped, it will spin the robot left 90 degrees. An initial wall
finding algorithm was put in place so robot no longer needed to be placed at
the edge of the wall.

public void goStraight2 ( int speed_local, int distance) {

        double distance_counter = 0;

        while (distance_counter < distance)

           

        {

            updateSensors();

            distance_counter += Math.abs(distance());

            goStraightAt(speed_local);

           

            if (bumpLeft() && bumpRight())

            {

                  spinLeft(90);

                  break;

            }

           

            if (bumpRight())

            {    

                 

                  spinLeft1(100,30);

                 

                  break;

            }

        }

Wall Finding Algorithm:

Robot will go straight until it hits the wall. If
the front bumper is engaged, it will spin the robot 90 degrees left. If the
right bumper is engaged, it will spin the robot 30 degrees left. It will break
if the wall sensor is engaged.

        //Wall Finding Algorithm

        while(!done)

        {

            roombacomm.updateSensors();

      

            //Stops
algorithm if wall is found

            if (roombacomm.wall())

                  break;

       

            roombacomm.goStraightAt(100);

       

            //Spins left
90 degrees if wall is found

            if (roombacomm.bumpLeft()
&& roombacomm.bumpRight())

            {

                  roombacomm.spinLeft(90);

                  break;

            }

           

            //spins left
30 degrees if right bumper sensor is engaged

            if (roombacomm.bumpRight())

            {    

                  roombacomm.spinLeft1(100,30);

            }

            done = keyIsPressed();

        }