Page 276 - The Definitive Guide to Building Java Robots

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

CHAPTER 7 ■ NAVIGATION 257

int southPos = 180 - heading;
int westPos = 270 - heading;
int northPos = 360 - heading;
SonarReadings sonarReadings = null;

int bestReadings[] = null; // order x,y
if (heading >= 0 && heading < 91) { //1st quad
sonarServos.moveLeft(westPos);
sonarServos.moveRight(southPos);
Utils.pause(500);
sonarReadings = getNavStamp().getSonar();
west = sonarReadings.left;
south = sonarReadings.right;
bestReadings = new int[] { REL_WEST, REL_SOUTH };
} else if (heading > 90 && heading < 181) {
sonarServos.moveLeft(northPos);
sonarServos.moveRight(westPos);
Utils.pause(500);
sonarReadings = getNavStamp().getSonar();
north = sonarReadings.left;
west = sonarReadings.right;
bestReadings = new int[] { REL_WEST, REL_NORTH };
} else if (heading > 180 && heading < 271) {
sonarServos.moveLeft(eastPos);
sonarServos.moveRight(northPos);
Utils.pause(500);
sonarReadings = getNavStamp().getSonar();
east = sonarReadings.left;
north = sonarReadings.right;
bestReadings = new int[] { REL_EAST, REL_NORTH };
} else if (heading > 270 && heading < 360) {
sonarServos.moveLeft(southPos);
sonarServos.moveRight(eastPos);
Utils.pause(500);
sonarReadings = getNavStamp().getSonar();
south = sonarReadings.left;
east = sonarReadings.right;
bestReadings = new int[] { REL_EAST, REL_SOUTH };
}

NavPoint navPoint = new NavPoint(NavPoint.START_POINT, 0, 0);
int xOffset = 0;
int yOffset = 0;
if (bestReadings[0] == REL_EAST) {
xOffset = (int)(ROBOT_RADIUS * Math.cos(Math.toRadians(eastPos)));
navPoint.x = 100 - east;