Velocity-based PID with the WPILib PIDController Class

[Mike Copioli]

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.

[Bongle]

Warnings
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.

Content
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.

[Mr. Lim]

Originally Posted by Mike Copioli

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.

Mike,

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

Bongle,

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!

[virtuald]

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...

http://www.chiefdelphi.com/forums/sh...ad.php?t=80650

[Mike Copioli]

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.

[Mike Mahar]

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.

[Mike Copioli]

Originally Posted by Mike Mahar

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.

[Mr. Lim]

Originally Posted by Mike Copioli

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?

[Mike Copioli]

Originally Posted by Mr. Lim

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

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

[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.