Example #1
0
  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();
  }
Example #2
0
  private void initiateFinalCalibration() {
    navigator.turnTo(0);
    while (odometer.getTheta() <= Math.toRadians(20) && !blackLineDetected())
      navigator.rotateCounterClockWise(15);

    if (blackLineDetected()) {
      Sound.beep();
      odometer.setTheta(0);
    } else {
      while (odometer.getTheta() >= -Math.toRadians(20) && !blackLineDetected())
        navigator.rotateClockWise(10);

      if (blackLineDetected()) {
        Sound.beep();
        odometer.setTheta(0);
      }
    }
    navigator.stopMotors();
    navigator.turnTo(0);
  }