Page 262 - The Definitive Guide to Building Java Robots
P. 262
Preston_5564C07.fm Page 243 Monday, September 26, 2005 5:38 AM
CHAPTER 7 ■ NAVIGATION 243
// in case it overshoots direction twice
if (toggle) {
// reset
toggle = false;
// reduce turn time by 250ms
turnSize = turnSize - 250;
}
// turn for a second left
if (relHeading < 180 && relHeading > 15) {
if (lastTurn == 'R') {
toggle = true;
}
drive.pivotLeft(turnSize);
// record what turn
lastTurn = 'L';
// turn for a second right
} else if (relHeading >= 180 && relHeading < 345) {
// records toggle
if (lastTurn == 'L') {
toggle = true;
}
drive.pivotRight(turnSize);
lastTurn = 'R';
} else if (relHeading >= 345) {
drive.pivotRight(250);
} else if (relHeading <= 15) {
drive.pivotLeft(250);
}
}
// set back to default speed
drive.setSpeed(DEFAULT_SPEED);
}
// adjust for angle measured to absolute angle
public static int getRealAngle(int theta) {
int phi = 0;
double ratio = 0.0;
// if in 1st quadrant
if (theta > 0 && theta < 90) {
// 1. get % of the total range
// 2. get range
// 3. multiply range by percentage, add it to current north reading.
phi = (int) ((theta / 90.0) * (REL_EAST - REL_NORTH)) + REL_NORTH;
}