Page 282 - The Definitive Guide to Building Java Robots

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

CHAPTER 7 ■ NAVIGATION 263

// get total time in MS for motion (vector length)
int totalMS = (int) Math.abs(vect.magnitude) * 1000;
int offsetTime = (int) getSurfaceRate(offsetDistance) * 1000;
// this will be minimum bypass distance
// get number of sonar scans for range of motion
int sweeps = (int) (totalMS / NavStamp.PING_CYCLE_TIME);
// this will start motion
getSonarServos().lookFore();
Utils.pause(2000); // time to move sonar
getDrive().forward();
int count = 0;
boolean Obstacle = false;
while (count < sweeps) {
// moves until it hits something or is done.
if (isObstacleFwd()) {
Utils.log("***fwd Obstacle***");
getDrive().stop();
Obstacle = true;
break;
}
count++;
}
getDrive().stop();
// get remaining time in vector
int remainingMS = totalMS - (count * NavStamp.PING_CYCLE_TIME);
if (Obstacle) {
if (inBypass) {
throw new Exception("Already in bypass find another route.");
}
Utils.pause(1000); // so not rough change of direction
moveRaw(RAW_REV, 1000);
remainingMS = remainingMS + 1000;
// since both an Obstacle and it can be bypassed
if (remainingMS > offsetTime) {
inBypass = true;
moveBypass(new MotionVector(vect.heading, remainingMS), offsetTime);
inBypass = false;
}
} else {
// since can't detect this distance anyway
getDrive().forward(remainingMS);
}
}

private void moveBypass(MotionVector remainingVect, int offsetTime) ➥
throws Exception {