Page 283 - The Definitive Guide to Building Java Robots

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

264 CHAPTER 7 ■ NAVIGATION

// since readings in milliseconds
remainingVect.magnitude = remainingVect.magnitude;
DistanceReadings readings = getNavStamp().getSonarIR();
// to move around obstacle to the left or to the right
int newHeading = remainingVect.heading;
double sq2 = (Math.sqrt(2) / 2.0);
double leftProb = 0;
double rightProb = 0;

// ir is more important use this first
// ir high means close, low means far
if (readings.ir.left - 20 > readings.ir.right) {
// since something closer on left, then turn right
leftProb = leftProb + 0.15;
// if so close turning will cause hit
if (readings.ir.left > 100)
leftProb = leftProb + 0.1;
} else {
rightProb = rightProb + 0.15;
// if so close not turning will cause hit
if (readings.ir.right > 120)
rightProb = rightProb + 0.1;
}
// checking sonar if left < right more room to right so turn right by
// increasing prob.
if (readings.sonar.left < readings.sonar.right) {
leftProb = leftProb + 0.1;
// if close
if (readings.sonar.left < 24)
leftProb = leftProb + 0.1;
// if so close not turning will cause hit
if (readings.sonar.left < 12)
leftProb = leftProb + 0.1;
} else {
rightProb = rightProb + 0.1;
if (readings.sonar.right < 24)
rightProb = rightProb + 0.1;
if (readings.sonar.right < 12)
rightProb = rightProb + 0.1;
}
int headingOne = 0;
int headingTwo = 0;
// int offset distance
double offsetAdjacent = Math.cos(Math.toRadians(45)) * offsetDistance;
double offsetOpposite = Math.sin(Math.toRadians(45)) * offsetDistance;