Drive forward to x inches

[acrilex]

Hello,

I have a problem. We are trying to achieve a function to drive to a specific distance, for exemple 10 inches. What I would like to be able to do is call driveToDistance(leftDistance, rightDistance).

We are using encoders, and they are well calibrated. For exemple, if we manually push the robot during 10 feets, RobotMap.driveTrainEncoderLeft.getDistance(); really return 120 inches for the left encoder.

How could we make a function so the robot goes forward to 10 feets using information returned by the encoders? I'm trying to understand PID, but a code exemple would help greatly.

For your information, in our code,

Left encoder is RobotMap.driveTrainEncoderLeft ;
Right encoder is RobotMap.driveTrainEncodeRight ;

Left motor is RobotMap.driveTrainLeftController ;
Right motor is RobotMap.driveTrainRightController ;

RobotDrive is RobotMap.driveTrainRobotDrive ;
(for tankDrive and arcadeDrive)

Thanks,
Acrilex

[TimTheGreat]

So this is written in python but you can make it work for java.

def encoder_drive(self, target_position, max_speed):
		target_offset = target_position - self.return_drive_encoder_position() #Figure out how far off you are
		
		if abs(target_offset)> 1000: #If you're less than 1000 'ticks' away (maybe change to 1 inch for you) then drive forward more
			self.y = target_offset * self.drive_constant.value # speed = offset * low constant (decreases speed as you approach target)
			self.y = max(min(max_speed, self.y), -max_speed) #limit that speed to some speed x)
			return False #you aren't at the position yet
		return True # if you're within an inch, you're there

later on in the code you want to tell robotdrive to drive with the y variable. Take a look at 1418's 2015 code here

[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

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

[mikets]

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

If you want to do precision autonomous driving, you may want to look at our library class TrcPidDrive.
https://github.com/trc492/Frc2016Fir...cPidDrive.java
This class contains several overloads of the method setTarget. The mecanum version of it takes the parameters xTarget, yTarget and turnTarget among other things. It uses three PID controllers, one controlling the X direction using the encoder, one on Y direction also using the encoder and one controlling the turn (or keeping it straight) using the gyro.