Page 284 - The Definitive Guide to Building Java Robots

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

CHAPTER 7 ■ NAVIGATION 265

// remaining time for original heading
double remainingTime = remainingVect.magnitude - offsetAdjacent;
int finalAngle = (int) Math.toDegrees(Math.atan(offsetOpposite / ➥
remainingTime));
double finalMagnitude = Math.sqrt(offsetAdjacent * offsetAdjacent + ➥
remainingTime
* remainingTime);
Utils.log("Obstacle prob=" + rightProb + "," + leftProb);
if (rightProb < leftProb) {
// turn right
headingOne = newHeading + 45;
headingTwo = newHeading - finalAngle;
} else {
headingOne = newHeading - 45;
headingTwo = newHeading + finalAngle;
}

MotionVector bypassOne = new DistanceVector(headingOne, offsetTime);
move(bypassOne);
MotionVector bypassTwo = new MotionVector(headingTwo, finalMagnitude);
move(bypassTwo);
}

private boolean isObstacleFwd() throws Exception {
DistanceReadings dist = getNavStamp().getSonarIR();
if (dist.ir.left > 100 || dist.ir.right > 120 || dist.sonar.left < 12
|| dist.sonar.center < 12 || dist.sonar.right < 12) {
return true;
} else {
return false;
}

}
public static void main(String[] args) {
try {
WebSerialClient com = new WebSerialClient("10.10.10.99", "8080", "1");
ObstacleNavigation nav = new ObstacleNavigation(com);
// in seconds
MotionVector[] v = new MotionVector[] { new MotionVector(90, 10) };
nav.move(v);
} catch (Exception e) {
e.printStackTrace();
System.exit(1);
}
}
}