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();
}
// run method (required for Thread)
public void run() {
long updateStart, updateEnd;
// reset the tacho count, and set the initial tacho counts to the last_tacho_X variables
leftMotor.resetTachoCount();
rightMotor.resetTachoCount();
last_tacho_L = leftMotor.getTachoCount();
last_tacho_R = rightMotor.getTachoCount();
while (true) {
updateStart = System.currentTimeMillis();
int current_tacho_L = leftMotor.getTachoCount();
int current_tacho_R = rightMotor.getTachoCount();
// calculating the distance each wheel has travelled
double distanceL = Math.PI * wheelRadius * (current_tacho_L - last_tacho_L) / 180;
double distanceR = Math.PI * wheelRadius * (current_tacho_R - last_tacho_R) / 180;
// updating the last tacho counts
last_tacho_L = current_tacho_L;
last_tacho_R = current_tacho_R;
// calculating the change in angle and change in absolute displacement
double deltaD = 0.5 * (distanceL + distanceR);
double deltaT = (distanceR - distanceL) / axleLength;
synchronized (lock) {
// updating the locations of the variables
theta += deltaT;
// maintain the bounds of theta
if (theta < 0) theta += 2 * Math.PI;
if (theta > 2 * Math.PI) theta -= 2 * Math.PI;
setDistanceTravelled(getDistanceTravelled() + deltaD);
setX(x + deltaD * Math.cos(theta));
setY(y + deltaD * Math.sin(theta));
}
// this ensures that the odometer only runs once every period
updateEnd = System.currentTimeMillis();
if (updateEnd - updateStart < ODOMETER_PERIOD) {
try {
Thread.sleep(ODOMETER_PERIOD - (updateEnd - updateStart));
} catch (InterruptedException e) {
// there is nothing to be done here because it is not
// expected that the odometer will be interrupted by
// another thread
}
}
}
}