Page 281 - The Definitive Guide to Building Java Robots

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

262 CHAPTER 7 ■ NAVIGATION

The first measurements are the infrared detectors. If the left side is greater than the right, I
increase the left’s probability of having an obstacle closer (higher means closer for the infrared
sensors). If the sonar reading on the left is smaller, I increase the left’s probability of an obstacle
because there is more room on the right.
In the next set of calculations, I need to determine the final length of vector f. This is the
second vector in the bypass path. Since I know the length of the first vector, I will have to calculate
the second vector so that the robot will move to its original target point. The calculation first
looks at the distance the robot has traveled parallel to its original heading. This is the COS of
the angle multiplied by the offsetDistance. Now I can calculate the remaining distance in the
direction of the original heading by subtracting the remaining time from this value.
Finally, the robot has a new distance it needs to travel such that the angle that is computed
by the right triangle of one side is the remaining distance on the original path, and the second
side is the distance traveled from the original path. I can calculate the angle by taking the arc
tangent of these two ratios, and then I can calculate the distance by squaring the sides, adding
them, and then taking the square root.
Now the robot will recursively call move() with two new headings. Because of this recur-
sion, it’s theoretically possible for the robot to move in a circle until it avoids the obstacle(s).
This is better handled with mapping (which will be discussed in the next section), so an exception
is thrown if more than one bypass is required. See Example 7-15.

Example 7-15. ObstacleNavigation.java
package com.scottpreston.javarobot.chapter7;

import com.scottpreston.javarobot.chapter2.JSerialPort;
import com.scottpreston.javarobot.chapter2.Utils;
import com.scottpreston.javarobot.chapter2.WebSerialClient;

public class ObstacleNavigation extends Localization {

private double offsetDistance = 0;
private boolean inBypass = false;

public ObstacleNavigation(JSerialPort serialPort) throws Exception {
super(serialPort);
offsetDistance = Math.sin(Math.toRadians(45)) * ROBOT_RADIUS * 2;

}

public void move(MotionVector vect) throws Exception {
Utils.log("MV=" + vect.toString());
if (vect.magnitude < 0) {
throw new Exception("Only avoids obstacles in forward direction");
}
changeHeading(vect.heading);

97022d2480fe4a63cfdfa123a6e70098