@Override
  public void loop() {
    /* reset the navX-Model device yaw angle so that whatever direction */
    /* it is currently pointing will be zero degrees.                   */
    if (first_iteration) {
      first_iteration = false;
      navx_device.zeroYaw();
      yawPIDResult = new navXPIDController.PIDResult();
    }

    /* Wait for new Yaw PID output values, then update the motors
      with the new PID value with each new output value.
    */

    /* Drive straight forward at 1/2 of full drive speed */
    double drive_speed = 0.5;

    if (yawPIDController.isNewUpdateAvailable(yawPIDResult)) {
      if (yawPIDResult.isOnTarget()) {
        leftMotor.setPower(drive_speed);
        rightMotor.setPower(drive_speed);
      } else {
        double output = yawPIDResult.getOutput();
        leftMotor.setPower(drive_speed + output);
        rightMotor.setPower(drive_speed - output);
      }
    } else {
      /* No sensor update has been received since the last time  */
      /* the loop() function was invoked.  Therefore, there's no */
      /* need to update the motors at this time.                 */
    }
  }
  public void DriveOnHeadingReverse(
      navXPIDController.PIDResult yawPIDResult, float heading, float distanceInches) {

    // calculate encoder counts for distance
    float wheelDiameter = 4; // inches
    float wheelCirc = wheelDiameter * (float) 3.14159;
    float encoderTicksPerRotation = 1120;
    float ticksPerInch = encoderTicksPerRotation / wheelCirc;
    int ticksToTravel = (int) (distanceInches * ticksPerInch);

    // check motor position
    int startEncCount = robot.leftfrontMotor.getCurrentPosition();
    int DEVICE_TIMEOUT_MS = 500;
    /* Drive straight forward at 1/2 of full drive speed */
    double drive_speed = 0.5;
    yawPIDController.setPID(0.05, YAW_PID_I, YAW_PID_D);
    yawPIDController.setSetpoint(heading);
    DecimalFormat df = new DecimalFormat("#.##");
    try {
      while ((robot.leftfrontMotor.getCurrentPosition() - startEncCount) > -ticksToTravel
          && !Thread.currentThread().isInterrupted()) {
        if (yawPIDController.waitForNewUpdate(yawPIDResult, DEVICE_TIMEOUT_MS)) {
          if (yawPIDResult.isOnTarget()) {
            robot.Drive(-drive_speed, -drive_speed);
            telemetry.addData("PIDOutput", df.format(drive_speed) + ", " + df.format(drive_speed));
          } else {
            double output = yawPIDResult.getOutput();
            if (output < -0.5) {
              output = -0.5;
            }
            if (output > 0.5) {
              output = 0.5;
            }
            robot.Drive(-drive_speed + output, -drive_speed - output);

            telemetry.addData(
                "PIDOutput",
                df.format(limit(-drive_speed - output))
                    + ", "
                    + df.format(limit(-drive_speed + output)));
          }
          telemetry.addData("Yaw", df.format(navx_device.getYaw()));
          telemetry.addData("Output", df.format(yawPIDResult.getOutput()));
          telemetry.addData("RevEnc:", robot.leftfrontMotor.getCurrentPosition());
          telemetry.addData("EncStart:", startEncCount);
        } else {
          /* A timeout occurred */
          Log.w("navXDriveStraightOp", "Yaw PID waitForNewUpdate() TIMEOUT.");
        }
      }
    } catch (InterruptedException ex) {
      Thread.currentThread().interrupt();
    }
  }
  public void TurnToHeading(
      navXPIDController.PIDResult yawPIDResult, double heading, double maxTimeSeconds) {
    try {
      long starttime = System.currentTimeMillis();

      TARGET_ANGLE_DEGREES = heading;
      int DEVICE_TIMEOUT_MS = 500;
      yawPIDController.setPID(0.02, YAW_PID_I, YAW_PID_D);
      yawPIDController.setSetpoint(TARGET_ANGLE_DEGREES);
      DecimalFormat df = new DecimalFormat("#.##");

      while (!Thread.currentThread().isInterrupted()
          && (System.currentTimeMillis() - starttime) < (long) (maxTimeSeconds * 1000)) {
        if (yawPIDController.waitForNewUpdate(yawPIDResult, DEVICE_TIMEOUT_MS)) {
          if (yawPIDResult.isOnTarget()) {
            float zero = 0;
            robot.FloatMotors();
            telemetry.addData("PIDOutput", df.format(0.00));
            telemetry.update();
          } else {
            double output = yawPIDResult.getOutput();
            if (output > 0 && output < 0.25) {
              output = 0.25;
            }
            if (output < 0 && output > -0.25) {
              output = -0.25;
            }
            robot.Drive(output, -output);
            telemetry.addData("PIDOutput", df.format(output) + ", " + df.format(-output));
          }
        } else {
          /* A timeout occurred */
          Log.w("navXRotateOp", "Yaw PID waitForNewUpdate() TIMEOUT.");
          telemetry.addData("timeout", "timeout occured");
        }
        telemetry.addData("Yaw aw", df.format(navx_device.getYaw()));
        telemetry.update();
      }
    } catch (InterruptedException ex) {
      Thread.currentThread().interrupt();
    }
  }
 @Override
 public void loop() {
   if (!calibration_complete) {
     /* navX-Micro Calibration completes automatically ~15 seconds after it is
     powered on, as long as the device is still.  To handle the case where the
     navX-Micro has not been able to calibrate successfully, hold off using
     the navX-Micro Yaw value until calibration is complete.
      */
     calibration_complete = !navx_device.isCalibrating();
     if (calibration_complete) {
       navx_device.zeroYaw();
     } else {
       telemetry.addData("navX-Micro", "Startup Calibration in Progress");
     }
   } else {
     /* Wait for new Yaw PID output values, then update the motors
       with the new PID value with each new output value.
     */
     if (yawPIDController.isNewUpdateAvailable(yawPIDResult)) {
       if (yawPIDResult.isOnTarget()) {
         leftMotor.setPowerFloat();
         rightMotor.setPowerFloat();
         telemetry.addData("Motor Output", df.format(0.00));
       } else {
         double output = yawPIDResult.getOutput();
         leftMotor.setPower(output);
         rightMotor.setPower(-output);
         telemetry.addData("Motor Output", df.format(output) + ", " + df.format(-output));
       }
     } else {
       /* No sensor update has been received since the last time  */
       /* the loop() function was invoked.  Therefore, there's no */
       /* need to update the motors at this time.                 */
     }
     telemetry.addData("Yaw", df.format(navx_device.getYaw()));
   }
 }