// 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
}
}
}
}
