GRIP WITH PID and 1 Target Tracking

We have been using GRIP to do vision tracking and we are getting the correct values and everything but the one thing that we don’t know how to do is get the PID working for the tracking. As of now when we try to track the target, our robot just goes back and forth because it constantly overshoots. If someone could help us with this that would be great! Also if anyone knows how to track only 1 target at a time that would be great too.

You can’t really use vision data directly as the input to a PID loop. It’s way too laggy, so you’ll always overshoot.

Instead, get a single value, then use that to calculate a setpoint. For the best accuracy, you can get to that setpoint using a PID loop with a gyro or something.

Sorry if I explained that wrong. We do have a single value and we have a set point and are trying to reach it by using a gyro but we don’t know how to do the PID loop.

Ah, okay.

Are you using a PIDController/PIDSubsystem? If so, it’s really a matter of figuring out the right P, I, and D constants to use, which is just a lot of trial and error.

I’m not sure what that is or how to use it

This should help:

http://wpilib.screenstepslive.com/s/4485/m/13810/l/241879-operating-the-robot-with-feedback-from-sensors-pid-control

So in this example the PIDController constructor only has 1 output source. What happens if I am trying to use 6 motors? (Since it is the drive train)

If you’re doing this in a Command (I’d recommend this), you can change it to a PIDCommand. This will require the methods returnPIDInput() and usePIDOutput() and a new constructor. In returnPIDInput(), you can return the heading from the gyro. In the usePIDOutput, you can write code that runs each side based on the output. Ex:

protected void usePIDOutput(double output) {
	//limit output
	output = Math.min(1, output);
	output = Math.max(-1, output);
	//use output
	driveTrain.setLeft(output);
	driveTrain.setRight(-output);
}

If your PID constants are positive, and your gyro isn’t reversed, the sign of the outputs should be as is (if the bot needs to turn left, the error is negative, so left should drive back, so left output should be negative). It’s easy enough to check for that and reverse though.

As far as tuning goes, start with a P around 0.1. Get it as good as you can just by tuning P, then move on to D. You probably won’t need I. There are lots of articles on this though.

So these are all separate speed controllers? In that case, you could use a lambda to set all of them.


PIDController pid = new PIDController(0.8, 0.01, 0.2, gyro, speed -> {
	frontLeft.set(speed);
	centerLeft.set(speed);
	backLeft.set(speed);
	
	frontRight.set(-speed);
	centerRight.set(-speed);
	backRight.set(-speed);
});


double setpoint = calculateSetpoint();
pid.setSetpoint(setpoint);