public void doLocalization() {
// drive to location listed in tutorial
// start rotating and clock all 4 gridlines
// do trig to compute (0,0) and 0 degrees
// when done travel to (0,0) and turn to 0 degrees
double blackLineAngles[] = new double[4];
navigator.travelTo(calibrationCoordinates[0], calibrationCoordinates[1]);
navigator.turnTo(Math.PI / 2, ROTATION_SPEED * 2);
for (int index = 0; index < blackLineAngles.length; index++) {
// Capture the angle when we first encounter the black line
while (!blackLineDetected()) navigator.rotateCounterClockWise(ROTATION_SPEED);
Sound.beep();
blackLineAngles[index] = odometer.getTheta();
// turn off of black line so as not to capture the same line twice
navigator.turnTo(odometer.getTheta() + 5 * Math.PI / 180, ROTATION_SPEED);
}
double deltaY = blackLineAngles[2] - blackLineAngles[0];
double deltaX = blackLineAngles[3] - blackLineAngles[1];
odometer.setX(-light_SensorDistanceFromOrigin * Math.cos(deltaY / 2));
odometer.setY(-light_SensorDistanceFromOrigin * Math.cos(deltaX / 2));
odometer.setTheta(odometer.getTheta() + blackLineAngles[0] + Math.toRadians(180) + deltaY / 2);
navigator.travelTo(0, 0);
initiateFinalCalibration();
}
private void initiateFinalCalibration() {
navigator.turnTo(0);
while (odometer.getTheta() <= Math.toRadians(20) && !blackLineDetected())
navigator.rotateCounterClockWise(15);
if (blackLineDetected()) {
Sound.beep();
odometer.setTheta(0);
} else {
while (odometer.getTheta() >= -Math.toRadians(20) && !blackLineDetected())
navigator.rotateClockWise(10);
if (blackLineDetected()) {
Sound.beep();
odometer.setTheta(0);
}
}
navigator.stopMotors();
navigator.turnTo(0);
}