Velocity-based PID with the WPILib PIDController Class

Greetings all!

We’re trying to implement velocity-based PID loop using the supplied WPILib PIDController class. We’ve had limited success, but it’s still not working quite the way we want it to.

We wrote a wrapper class called PIDEncoder which passes GetRate() from the Encoder class to PIDGet()

We then translate our joystick axes to desired wheel speeds, and pass those into our PIDController SetPoints.

The problem is that our wheel speeds never reach our desired SetPoints. When our SetPoint is 10000 counts per second for instance, our wheels only reach about 5000 counts per second.

Right off the bat, many of you will suggest we up our P constant, however increasing it any more results in our wheels oscillating very badly, especially at low speeds.

Can anyone else share some of their experience in getting velocity-based PID to work with the supplied WPILib?

At this point, I’m not sure if I can simply tune the PID constants to get it working the way we want it to, or if we need to tweak the PID Controller class itself - i.e. getting the Proportional response to be non-linear. Should we only be using the I term?

System specs:
-4 x US Digital 256 CPR Encoders (included with ToughBoxes)
-4 x 8" Wheel via 1:1 chain + sprockets to unmodified ToughBox w/ 1 CIM
-1 ToughBox/CIM combo per wheel
-P = 0.00014, I = 0, D = 0

Without seeing your code I can only speculate based on my experience with PID. It sounds to me like you have a math issue and or a coding issue. If the system becomes unstable(oscillates) when you increase p gain, it usually means that the system is approaching the desired target and then overshooting. In your case the target is velocity. My advice is to not add any other gains until you have confirmed that your target velocity and current velocity are correct. The fact that when you increase P gain your system overshoots tells me that this is the case.

Disclaimer: All I know about PID loops I learned through the Wikipedia page and tweaking the several that we had on our robot last year.

Safety note: Always test PIDs on a stand first. Sometimes you can have very unpredictable results, and you don’t want a 60-150lb robot flying off at full speed into equipment or people.

You’ll probably need an I term to help you accumulate more power.

Think of the definition of a PID loop:

Output = P + I + D
P = error * kP
I = sum(past errors) + kI*error
D = not too important in this case.

In this case, your error is 5000 (10000 - 5000) and your kP is 0.00014. Since your kI and kD is zero, your output is 5000 * 0.00014 = 0.7. If a motor power of 0.7 ends up equaling the amount of drag in your drive system, then your robot will not accelerate any further. If you set a (very small) kI, then your I term will grow as long as you’re not hitting your setpoint, which means your motors will speed up until the setpoint is hit.

Of course, if your setpoint isn’t hittable (physical max speed is 8000 ticks/sec, maybe) then you’ll probably have problems slowing them back down, but that’s a tweakable thing.

An I term will also help you with low-speed stuff. Right now, if you set a setpoint of 1000 ticks/sec, that’ll translate to a motor power of 0.14. If a motor power of 0.14 translates to 2000 ticks/sec on the robot, then you’ll have a new output of -0.14, which might translate to -2000 ticks/sec, which will then give a motor power of 0.42 on your next cycle, and you see in this simple example you’ve got oscillation. An I term will allow you to reduce your P term and will at least smoothen the oscillations.

Another tweak might be to increase the frequency of the PID loop, which I believe you can do in its constructor. Faster updates might reduce oscillations, depending on the update rate of the encoders and cRio.


I’ve noticed a few things today:

  1. At low speeds, the P response works “fine” - but once our speed goes higher than around 25% of our max, our P term is no longer enought to get us to our SetPoint.

  2. As I up our P, the low speeds oscillate, but the higher speeds seem to behave okay


We’re going to try and re-tune our PID today. Instead of tuning the P term first, we’ll try and see if we can get an I-only loop somewhat working first.

I’m used to tuning the P response first for positional PID loops, but there’s definitely a very significant I term when it comes to velocity-based. You have to constantly supply power just to maintain a particular speed.

Hopefully tuning for I first will get us closer.

Thanks for the advice!

We had a lot of difficultly last year with tuning our PID loops, so I created a nice web interface for the robot that allowed us to tune it (among other things) on the fly. I think LabVIEW has the same type of functionality, its really useful instead of compiling, reload, test, compile, reload, test…

What kind of speed controllers are you using? This non linear response suggests that you are using Victors. Is this the case? I would still advise you to withhold the I and D gains until you have established that your code is correct. P is very simple to debug. PI is not so easy. PID is even harder yet. If you are using Windriver place your input (throttle), Verror, and PID output variables into the expressions view. Insert a breakpoint on the next executable line of code after the PID machine. Compare the resultant values, since P is effectively P output = (Pk*Verror)*throttle (capped at 1/-1) you should be able to do the math yourself and determine if the values make sense. The math example above is how we do P you should verify that the wpi libs do it the same way. Some add or subtract. Either way the point is that as Verror approaches 0 your P output should decrease proportionally to the error, hence the name proportional gain.

I seems as though you are trying to do a positional PID when you want a velocity PID. For a positional PID, the motor will start out fast and then slow down to 0 when you get to your position. If you do this for speed, however, as the indicated speed goes up, your PID will start to slow down and your speed will drop which will cause the PID to speed up the motors again. With some values, it may stabilize at about 1/2 power.

For a velocity PID, you don’t control the motors directly with the PID. Instead you control the amount you want to change the speed of the motor. This variable is adjusted like a motor control but it is added to the current motor speed. Suppose the current motor power is 0. If the desired speed in 100, for example, the PID may set the delta at 0.1 (depending on the kP value) 0.1 will added to the motor power each time through the PID loop. As the speed of the robot approaches 100 the delta will be reduced untill it reaches 0.0. at this point the power of he robot will be at whatever value is needed to sustain the desired speed.

You are correct. I was giving him an example of positional PID. The math is the same except that the product of K*error is added to the previous throttle. My mistake.

Which in effect would give you an “I” term in your control loop, since I is essentially a scaled accumulation of error?

Thanks for all the responses.

We retuned our PID loop, and started with an I-only control loop, and got it working nicely. There is a bit of integral windup that causes some overshoot when we rotate about our axis, but we can now hit our setpoints. We’ll probably try and tweak by limiting the amount of I-accumulation to prevent large wind-up.

Right now however, it drives pretty nicely, but we may tweak at a later date but doing some similar to what Mike Mahar suggested.

We might implement a simple open-loop joystick -> motor algorithm, then add to that the outputs of a PID loop that calculates a delta to get us to our set points.

After doing some research, it sounds like this type of “feed-forward” implementation is popular in velocity-based control applications.

Does anyone else do it this way?

Yes, basically it is I without an accumulator variable to clear as you would with position servo. You are in fact integrating over time.

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.

Mr. Lim

As other have stated, you need integral control in order to have zero tracking error in steady state in a velocity control scheme. That’s because in order to track a step reference change, you need an integrator in the compensator (which you don’t get with a proportional controller) or in the plant (which you don’t get when dealing with velocity). If you were controlling position, then you would probably be OK with a proportional controller (except if you had a considerable deadzone), because then the integrator would be present in the plant.

It might be easier to think of it in these terms: you need a voltage to keep the motor spinning. If you were to reach the desired setpoint speed with a proportional controller, then the error would be zero, and your control effort K*error would also be zero, so your motor would no longer be at your setpoint. In position control that’s not a problem, because if you’re at the desired setpoint you shouldn’t be applying any voltage for it to move.

If you want to understand this deeper, look up internal model principle in a good book or even Google. I can try to explain it if you can work in the frequency domain.

Regarding the use of the I term alone, it will work, BUT, if your system can be represented by a second order model (and it is very likely that this is a good approximation), then your response will be at best two times slower than in open loop (as in, it will take twice the time to reach the setpoint than if you did not have any control at all). Sure, you will be driving at the precise velocity you want, but you’ll be accelerating slower than your opponents, which is never a good thing in a competitive FIRST field.

The best thing would be to use the full PID controller, but it can be tricky to tune without a reasonably good model. I recommend you stick with PI.

You can even predict what your error will be with a proportional controller. Here’s what you do: in open loop, apply a voltage step to the motor and determine its steady state velocity. Divide this velocity by the amplitude of the step you applied and call this number the plant static gain G(0).
Now close the loop with a proportional controller with gain K. The steady state error (that is, your desired setpoint - let’s call it A - minus the actual velocity you’re reaching) should be equal to A/(1+ K*G(0)).
You should try this simple test to see that there IS logic behind control systems… :wink:

If you are using a Victor, then your results will not be correct, because changing the Victor input from 0.5 to 1.0 does not mean the output voltage doubles. The Jaguars are a lot linear-er in this aspect, and that’s why I strongly recommend them for control loop applications, EVEN if their failure rates were much worse than the Victors - in our experience, they are similar, even though we had been using the Victors for a longer time.

Sorry for the long post, but I hope someone learns something from it!