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