Page 268 - The Definitive Guide to Building Java Robots

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

CHAPTER 7 ■ NAVIGATION 249

Again, to move successfully in this area, the robot will need the following information:
• A start point (a)
•An end point (b)

• A way to orient ourselves (find a heading) Θ (theta)
• A way to know how far we need to go (c)

From the previous section, the robot moved in the path of vector c with a heading and a
time (or distance), but without a starting point or knowing whether or not it was at its end
point. It moved relative to nothing, which is not that useful in navigation. So, how can the robot
find its start point? There are two ways: I could tell the robot where it starts, or the robot could
figure it out by itself.
To tell the robot where it should start, I added a single static method to the Localization
class. This method takes any two points and returns a DistanceVector. Though the calculations
speak for themselves, I needed to convert the angles from the Cartesian plane to the plane of
compass readings. For Cartesian, the readings (clockwise from the top) are 90, 0, 270, and 180,
with compass readings of 0, 90, 180, and 270. I performed this conversion by comparing the
points with respect to one another and the arc tangent of the two points slope. See Example 7-11.

Example 7-11. Localization.getDistanceVector()
// calculate vector from 2 points.
public static DistanceVector getDistanceVector(Point a, Point b) ➥
throws Exception {

int d;
int dx = a.x - b.x;
int dy = a.y - b.y;
System.out.println(a.toString());
System.out.println(b.toString());
// get distance
double mag = Math.sqrt(dx * dx + dy * dy);
// get angle
if ((dx) == 0) {
d = 90;
} else {
double slope = (double) (dy) / (double) (dx);
d = (int) Math.toDegrees(Math.atan(slope));
}
// adjust angle to coordinate system of N,E,S,W
if (a.y <= b.y) { // if 1st point(Y) higher
if (a.x > b.x) { // if 1st point(X) is more to right
d = 360 - (90 + d);
} else {
d = 90 - d;
}