Page 261 - The Definitive Guide to Building Java Robots
P. 261
Preston_5564C07.fm Page 242 Monday, September 26, 2005 5:38 AM
242 CHAPTER 7 ■ NAVIGATION
// instance variables
public int surface = SURFACE_CEMENT;
private JMotion drive;
private NavStamp navStamp;
public Navigation(JSerialPort serialPort) throws Exception {
// drive with default speed
drive = new SpeedDiffDrive(serialPort);
drive.setSpeed(DEFAULT_SPEED);
// stamp for sensors
navStamp = new NavStamp(serialPort);
}
// change heading
public void changeHeading(int newHeading) throws Exception {
// this will calculate a real angle from a relative measure of
// the coord axis.
newHeading = getRealAngle(newHeading);
int accuracy = 2; // degrees
// autoadjust speed depending on the surface
if (surface == SURFACE_CEMENT) {
// slow so don't overshoot 15 degrees at 1sec intervals
drive.setSpeed(12);
} else {
// moves slower on carpet
drive.setSpeed(20);
}
// used to record lats turn
int lastTurn = 0;
boolean toggle = false;
int turnSize = 1000;
while (true) {
// get compass
int currentHeading = navStamp.getCompass();
// get relative heading from compass to where you want to go
int relHeading = currentHeading - newHeading;
// adjust for negative
if (relHeading < 0) {
relHeading = 360 + relHeading;
}
// if within bounds, stop
if (relHeading <= accuracy || relHeading >= 360 - accuracy) {
drive.stop();
break;
}