Page 275 - The Definitive Guide to Building Java Robots

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

256 CHAPTER 7 ■ NAVIGATION

// 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;
}
} else {
if (a.x < b.x) {
d = 90 - d;
} else {
d = 180 + (90 - d);
}
}
return new DistanceVector(d, mag);
}

// this uses sonarServos, add your own sensors here if needed
public NavPoint getStart() throws Exception {

int[] nesw = getFourCoordinates();
return new NavPoint(NavPoint.START_POINT, nesw[3], nesw[2]);
}

public int[] getFourCoordinates() throws Exception {
// first face north.
changeHeading(0);
sonarServos.lookSide();
Utils.pause(500);
SonarReadings sonarReadings = getNavStamp().getSonar();
int north = sonarReadings.center;
int east = sonarReadings.right - ROBOT_RADIUS;
int west = sonarReadings.left + ROBOT_RADIUS;
sonarServos.lookAft();
Utils.pause(500);
sonarReadings = getNavStamp().getSonar();
// average of two readings
int south = (int) ((sonarReadings.left + sonarReadings.right) / 2.0);
return new int[] {north,east,south,west};
}

// this uses sonarServos, add your own sensors here if needed
public NavPoint getStart2() throws Exception {

int heading = getNavStamp().getCompass();
int north = 0, south = 0, east = 0, west = 0;
int eastPos = 90 - heading;