示例#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();
  }
示例#2
0
  // 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
        }
      }
    }
  }