[Pault]

You will want to use WPILib's built in PIDController class for this.

I am assuming you are doing command based programming. If not, the same lines of code still apply, just in a different structure.

One thing to keep in mind is that PIDController will run its calculations in a separate thread. That means that you will need to call driveToDistance() once, and then continuously be checking if the PID is done yet with a different method. This fits in nicely with the command based structure, where you can put driveToDistance() in the initialize method, and do the check in isFinished().

public class Drivetrain extends Subsytem {

   public PIDController distancePIDLeft;
   public PIDController distancePIDRight;

   public Drivetrain() {
      distancePIDLeft = new PIDController(kP, kI, kD, RobotMap.driveTrainEncoderLeft, RobotMap.driveTrainLeftController, 20);
      distancePIDLeft.setAbsoluteTolerance(2); // Sets the tolerance, in the same units as your encoders, for how close to the target the PID needs to get to be considered finished
      distancePIDRight = new PIDController(kP, kI, kD, RobotMap.driveTrainEncoderRight, RobotMap.driveTrainRightController, 20);
      distancePIDRight.setAbsoluteTolerance(2); // Sets the tolerance, in the same units as your encoders, for how close to the target the PID needs to get to be considered finished
   }

   //enables the distance PIDs and sets them to the given distances
   public void driveToDistance(leftDistance, rightDistance) {
      distancePIDLeft.enable();
      distancePIDLeft.setSetpoint(leftDistance);
      distancePIDRight.enable();
      distancePIDRight.setSetpoint(rightDistance);
   }

   //if the both distance PIDs have completed, returns true and disables the PIDs
   public boolean atDistanceTarget() {
      if(distancePIDLeft.onTarget() && distancePIDRight.onTarget()) {
         distancePIDLeft.disable();
         distancePIDRight.disable();
         return true;
      } else {
         return false;
      }
   }
}