Page 280 - The Definitive Guide to Building Java Robots

Preston_5564C07.fm Page 261 Monday, September 26, 2005 5:38 AM

CHAPTER 7 ■ NAVIGATION 261

Code Objective

The objective here is to give the robot the ability to navigate around obstacles.

Code Discussion

The ObstacleNavigation class builds on the Localization class from the last chapter. It has a
single variable, offsetDistance, which will always be the diameter of the robot away from the
original path. The constructor passes JSerialPort to the parent class Localization, and then any
references required for the SonarServos, NavStamp, and SpeedDiffDrive classes can be used
from the parents’ accessor methods.
The only public method in this class is move(), which overrides the move() method in the
Navigation class. The first thing I do in this method is log the MotionVector sent to the method.
This was helpful to me while debugging, and it’s also good to have a log for the robot’s movements.
Next it calls changeHeading() like its parent. Second, in the event of a negative magnitude, an
exception is thrown because the sensors are configured for forward detection. If I had more
sensors, I would adjust this for any direction, forward or reverse.
The next variable in the method, totalMs, holds the calculation of the total distance in
milliseconds for the entire move. This is needed so that if the robot encounters an obstacle it
can be calculated where the obstacle was encountered, permitting the bypass vectors to deter-
mine the position of the original coordinates. In Figure 7-10, the totalMs will represent the
length of line c.
The next variable is offsetTime. This calculates the time based on the surface rate and the
current offsetDistance. For the current robot, this is 34 inches divided by 14 inches per second,
which translates to about 2400 milliseconds.
The first calculation that needs to be made is the number of times the sensors can take
readings during the total range of motion. I’ll call these variables sweeps. By knowing the number
of sweeps calculated, the number of interval calls can be made to the isObstacleFwd() methods. If
an obstacle does get in the way of the robot, the interval count is maintained. Using this number,
the remaining time can be calculated in the current MotionVector. The variable used to calculate
the remaining time is remainingMS, which is the count multiplied by the cycle time of the sensors.
The final bit of logic in this method is checking to see if an obstacle was encountered. If it
was, then I pause the robot for a second before moving in reverse for 1 second. This clears
the robot’s path so it can turn and move along the bypass vector path without hitting the
obstacle. Next, I need to increase the total remainingMS by the second it moved in reverse.
Finally, before calling moveBypass(), I check to see if the remainingMS is greater than the
offsetTime. This is because it’s still possible that the robot is close enough to its desired position
that any bypass would not get it any closer to its goal than if it just stayed put. Finally, if no
obstacle is detected, the robot just moves forward for the remaining milliseconds.
The first of the two private methods is ObsicalForward(), which determines if there’s an
obstacle in the way of the robot. Again, you want to adjust the values in this method for your
robot’s configuration. Here I get all the sonar readings from the NavStaqmp, and depending on
the readings, I return a true for obstacle and a false for no obstacle.
The second method, moveBypass(), creates two bypass vectors. These are vectors e and f
in Figure 7-10. To calculate them, first the robot needs to determine the direction of the
obstacle. To calculate the direction, I used a probability approach for each of the readings.