// Heading Correction!!
public void correctHeading() {
tachoLeftDelta = Math.abs(tachoLeft2 - tachoLeft1);
tachoRightDelta = Math.abs(tachoRight2 - tachoRight1);
distanceErrorL = getDistanceFromTacho(tachoLeftDelta, leftRadius);
distanceErrorR = getDistanceFromTacho(tachoRightDelta, rightRadius);
distanceError = (distanceErrorL + distanceErrorR) / 2;
thetaError = Math.toDegrees(Math.atan((distanceError / lightLToLightR)));
timeDelta = time1 - time2;
odometer.setTheta(getHeading() + (timeDelta / Math.abs(timeDelta)) * thetaError);
}
/**
* Localizes the robot using the ultrasonic sensor. Clocks the angle at which the sensor detects
* each wall and calculates 0 degrees
*/
public void doLocalization() {
angleA = 0;
angleB = 0;
int UCdistance, expmtDistance;
expmtDistance = 38;
int count = 0;
boolean facingToWall = true;
// rotate the robot until it sees no wall
nav.rotate(165, -165);
facingToWall = true;
while (facingToWall) {
UCdistance = getFilteredData();
if (UCdistance > expmtDistance) {
count++;
}
if (UCdistance > expmtDistance && count >= 3) {
facingToWall = false;
count = 0;
}
}
sleep(3000);
// keep rotating until the robot sees a wall, then latch the angle
while (!facingToWall) {
UCdistance = getFilteredData();
if (UCdistance < expmtDistance) {
count++;
}
if (UCdistance < expmtDistance && count >= 3) {
facingToWall = true;
angleA = odo.getTheta();
Sound.beep();
nav.stopMotor();
count = 0;
}
}
// switch direction and wait until it sees no wall
nav.rotate(-165, 165);
while (facingToWall) {
UCdistance = getFilteredData();
if (UCdistance > expmtDistance) {
count++;
}
if (UCdistance > expmtDistance && count >= 3) {
facingToWall = false;
count = 0;
}
}
// keep rotating until the robot sees a wall, then latch the angle
sleep(3000);
while (!facingToWall) {
UCdistance = getFilteredData();
if (UCdistance < expmtDistance) {
count++;
}
if (UCdistance < expmtDistance && count >= 3) {
facingToWall = true;
angleB = odo.getTheta();
Sound.beep();
nav.stopMotor();
}
}
// angleA is clockwise from angleB, so assume the average of the
// angles to the right of angleB is 45 degrees past 'north'
// we slightly correct the angle '45' to '44' based on the error we measured
angle = 45 - (angleA - angleB) / 2;
// update the odometer position (example to follow:)
odo.setTheta(angle);
nav.turnTo(0);
}
