// 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
}
}
}
}
// run method (required for Thread)
public void run() {
long updateStart, updateEnd;
double distL, distR, deltaD, deltaT, dX, dY;
while (true) {
updateStart = System.currentTimeMillis();
// put (some of) your odometer code here
nowTachoL = leftMotor.getTachoCount(); // get tacho counts
nowTachoR = rightMotor.getTachoCount();
distL = Math.PI * WR * (nowTachoL - lastTachoL) / 180; // compute
// wheel
distR = Math.PI * WR * (nowTachoR - lastTachoR) / 180; // displacements
lastTachoL = nowTachoL; // save tacho counts for next iteration
lastTachoR = nowTachoR;
deltaD = 0.5 * (distL + distR); // compute vehicle displacement
deltaT = (distL - distR) / WB; // compute change in heading
synchronized (lock) {
// don't use the variables x, y, or theta anywhere but here!
theta += deltaT;
if (theta >= Math.PI * 2) {
theta = theta - Math.PI * 2;
}
dX = deltaD * Math.sin(theta); // compute X component of
// displacement
dY = deltaD * Math.cos(theta); // compute Y component of
// displacement
x = x + dX;
y = y + dY;
thetaDeg = theta * 180 / Math.PI;
}
// 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
}
}
}
}
