Drive forward to x inches

[TimTheGreat]

Originally Posted by acrilex

Thanks,

I do understand the logic within this, we had a code with this kind of calculations, even slowing down when approaching at less than 5 inches, but the real problem is to actually make it work with PID, as I see many other teams are using this and it works perfectly.

Acrilex

So this is basically PID? Or really just P. You probably don't need I and D for driving a distance. You probably can't get much more accurate than within an inch anyway (from what we have found. But every robot is different).

[GeeTwo]

If you aren't interested in arriving at a spot with precision, you can simply drive forward and turn off the motors when you reach the desired number of clicks traveled.

That is, turn on the drive, then each pass of your loop, check the distance traveled and turn the motors off when you reach the desired count. You WILL overshoot if you do this, but probably not by much. If you figure out how far you coast after stopping the motors, just apply a bit of Kentucky windage and turn off the motors that much early.

[TimTheGreat]

Originally Posted by GeeTwo

If you aren't interested in arriving at a spot with precision

Kinda defeats the purpose of encoders though, doesn't it? Timed drive will do the same.

[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;
      }
   }
}