Velocity-based PID with the WPILib PIDController Class

[oddjob]

I think you have got it mostly all figured out. Here's my take.

If you have an absolute encoder on your drive wheel, then it is measuring how far the wheel has rotated, which is a measure of the position of the robot (assuming no wheel spin). So your setpoint for the PID controller needs to be a position value. You probably want a velocity setpoint rather than position though. When the joystick is pushed forwards, you want it to go fast, or ease it back to reduce speed. You typically don’t push the joystick all the way forwards and want the robot to move an equivalent distance, which is what a position setpoint is doing.

A velocity setpoint can be used by differentiating the encoder values to calculate output velocity, then use the velocity error as the PID input. Or, integrate the joystick value to calculate a position. These have pro’s and con’s, read below.

Let’s assume we are using a position setpoint with Kp = 0.5, Ki = 0, Kd = 0. Also assume the motor is perfect with linear response and no friction and unity gain. At time 0:

Position setpoint = 10, Output position = 0.
Kd * (10-0) = 5
next Output position = 5

Increment time:
Position setpoint = 10, Output position = 5.
Kd * (10-5) = 2.5
next Output position = 5+2.5 = 7.5

Increment time:
Position setpoint = 10, Output position = 7.5.
Kd * (10-7.5) = 1.25
next Output position = 7.5+1.25 = 8.75

Increment time:
Position setpoint = 10, Output position = 8.75.
Kd * (10-8.75) = 0.625
next Output position = 8.75+0.625 = 9.375

etc.. With a perfect motor, the robot will eventually move to the desired position, 10. Note that the P term trends towards zero. Also, the I and D terms can be zero and this system can work. With friction losses, you’ll see a “droop” in the Output position and never quite get to 10. The droop can be reduced by increasing the I term, but not so much that you get overshoot. The D term can improve the dynamic performance i.e. how it responds to sudden changes in setpoint.

What if we use a velocity error PID instead of position error? Without going through the math step by step, when the motor velocity equals the setpoint velocity, the P term will be zero. But the motor should not be stationary, so the I term must accumulate and be equal to the velocity. Now the P term is at zero and the motor is rotating due to the I term. This is very different than the positional PID where the I term can be zero at the setpoint.

My recommendation is to use the positional PID because then the I term is used only to reduce droop and not to stay at the setpoint. Even then though, be careful. What if you shove the joystick all the way forwards and the robot is jammed against a wall? Integrate the joystick “velocity” value to get the setpoint position. The position value is increasing all the time, and the robot is not moving. Now the robot spins and frees itself, where is it going to go? The setpoint position value is some huge value so the robot is going to try and go there, which is probably not what you want. So what you really need is a position setpoint PID with the velocity controlled by joystick. The first and most obvious thing to do is to limit the range of the integrated joystick value because there is really not much purpose in letting it ramp up to a huge value. Thinking of this in terms of the robot, you don’t want to allow the position setpoint to become larger than the distance the robot can move in the sampling time increment. Now you’ll probably have a fairly well controlled robot.

Tuning the PID is very tricky. Running the robot with the wheels off the ground is nearly useless for tuning, because the drive system performs differently driving only the mass of each wheel rather than a 150lb robot (120 lb robot + bumpers + battery). If you have a rolling road where you can adjust the mass of the rollers, then you can set up something that closely represents the robot dynamics. Failing that, trial and error isn’t a bad option, it just takes time. I suggest you write some code to adjust the P, I and D parameters up/down in real time from e.g. joystick buttons and run the robot and tweak until satisfied. It’s not optimum, but it’ll get you close enough. With this years game, if you plan on going over bumps, there might be times when the robot drive wheel is in the air. The PID tuning that was carefully tweaked for running the robot on the ground is now way off. Try it and see what happens and if it’s a problem that needs a solution or not. If it’s a huge problem, then maybe have a driver operated control to turn off PID when going over the bump, or be super creative and use a tilt switch read by the code and do it all automatically.