PID for velocity control

[SuperBK]

We want to implement a PID for velocity control of the left and right motors. We understand how a position PID works, but for a velocity control, when the motor reaches the desired velociy, the error will be zero, therefore, it will only have the I and D terms to keep it driving. Is that enough? In this case would the integral sum never be cleared?

About resolution: Right now we get around 1200 counts per second at full speed from the gear tooth sensors. If we wait for 100ms that will give us a speed range of 0-120. Should the PID calculation only run at the same rate? If the PID loop runs ever 26 ms, it will aleady have run three times before we have a speed update. Whats a good balance of PID loop time to data update time?

Is the 26.2ms main loop stable enough to run the PID in or should it have its own timer?

Thanks,
Brian

[Alan Anderson]

There's a perfect answer to what you want. It's called "velocity PID", and it works by computing the required change in output instead of the output itself.

I spent quite some time a couple of weeks ago trying to help the TechnoKats software group implement a velocity PID routine for our robot's drive motors. I might have been doing it wrong, or I might not have been patient enough when trying to tune the PID constants, or I might just have been fighting easyC too hard. In the end, however, we discarded the idea of controlling motor speed, and instead implemented a positional PID control for the robot itself.

To get the effect of closed-loop speed control, we just move the desired robot position at the right rate, and the robot follows along smoothly. There's a similar positional PID for direction, and turning the robot at a given rate works by changing the desired direction at that rate. Feedback inputs to both PID controllers are from quadrature encoders on the drive wheel shaft. The sum of the encoder counts is used as an odometer, and the difference tells us the robot's present heading relative to when it started.

[Uberbots]

Interesting solution, alan...

with our robot we just implemented the pid loop so that the motors corrected for a desired speed, and it worked fine (well that is until the mech. team decided to take electronics into their own hands and killed the wiring...). I dont think we did anything differently from a position correction, besides differentiating position before using it as the feedback.

[Jon236]

So we're not the only team whose Build Team can't quite understand "What part of DON'T cut the wires don't you understand?"

[JBotAlan]

Not to threadjack TOO much, but I am curious: do I need a really high resolution for D to work properly? I just have a resolution of degrees right now, because I am scaling the input before it hits the PID. It's just way too crazy to try to tune...I have a hack in place that zeroes the I term on my steering motor (guess what we're doing for a drivetrain... ) when we're inside a deadband...but I know that's not the right way to do it.

Does D fix my overshooting problems?

JBot

[Bomberofdoom]

Originally Posted by Jon236

So we're not the only team whose Build Team can't quite understand "What part of DON'T cut the wires don't you understand?"

We're one of the teams that we need to tell our PROGRAMMING TEAM "What part of DON'T cut the PWM wires don't you understand?".

[Mr. Lim]

Originally Posted by SuperBK

We want to implement a PID for velocity control of the left and right motors. We understand how a position PID works, but for a velocity control, when the motor reaches the desired velociy, the error will be zero, therefore, it will only have the I and D terms to keep it driving. Is that enough? In this case would the integral sum never be cleared?

About resolution: Right now we get around 1200 counts per second at full speed from the gear tooth sensors. If we wait for 100ms that will give us a speed range of 0-120. Should the PID calculation only run at the same rate? If the PID loop runs ever 26 ms, it will aleady have run three times before we have a speed update. Whats a good balance of PID loop time to data update time?

Is the 26.2ms main loop stable enough to run the PID in or should it have its own timer?

Thanks,
Brian

Brian, I'm referencing Kevin Watson's Navigation code that can be found at: http://frc.kevin.org/frc/2005/. It includes both speed and velocity PID control routines. We've used these in the past with a good amount of success. They can provide a good reference for what you're doing.

The code samples the change in encoder pulses each 26ms cycle, and that's your PID input. The PID and speed updates are both done in the 26 ms cycle, and it appeared to be stable enough, as the main loop is timer driven as well. We used this with Grayhill encoders pulsing at under 2000pps at full speed, and got good velocity control.

Regarding "I," when we hit our desired velocity, the system overshot until negative error, and eventually our "I" term decayed. Under straight driving conditions, it wasn't really that noticeable, because the system reacts fast enough that a large I never really accumulates. This WAS really noticeable when we turned our robot about its axis. The system didn't react as fast because there is a lot more resistance turning on axis, than driving in a straight line, so "I" had the opportunity to wind up a lot more. This resulted in a lot of overshoot... uncontrollable overshoot.

Do you really need an "I" term at all in velocity control? Maybe , because your system's characteristics change depending on whether you're going straight, or turning, or whether you're being pushed. "I" is a pretty good way of handling all those difference situations, except for the overshoot it causes.

You'll find that a lot of teams do talk about interesting ways to selectively use "I" in velocity control, but I'm definitely not the right person to be commenting about that!

-Shawn...

[wireties]

We measure the period between the pulses from our quadrature encoders (setting up timer 2 as a rolling counter). Then you get more bandwidth on the feedback than you can possibly use. Of course you have to put together a nifty way to do 1/x using integer math to derive the velocity.

The I term is supposed to help the wheel match the target velocity over long periods of time. Its probably not necessary for this application, P and D are more critical. If you do use the I term, it is common to have an I limit so you don't accumulate error (as described in last post). And the I term is typically quite small compared to P and D.

HTH

[psy_wombats]

So, looking at Kevin Watson's velocity PID code...

Chiefly I'm looking at the differences between the normal position-based code and the velocity, as we currently have a working normal PID. Looks like it comes down to this line:

motor_info[motor].vel = ((vel_last * 9) + (motor_info[motor].pos - pos_last) * HZ) / 10;

Now, I understand that acceleration is being calculated rather than velocity like the position PID, but, two questions to Kevin or whoever might be knowledgeable:
1 - Where exactly is the 9/10 bit coming from? Is some form of scaling the old value? If not, how was it derived?
2 -What is the purpose of the hertz? I know what it is, just not what it does.

Sorry, I know I could just replicate that line, but I'm apprehensive about adopting code I have no idea what does. Any help is appreciated.

[EricS-Team180]

Originally Posted by psy_wombats

So, looking at Kevin Watson's velocity PID code...

Looks like it comes down to this line...

motor_info[motor].vel = ((vel_last * 9) + (motor_info[motor].pos - pos_last) * HZ) / 10;

1 - Where exactly is the 9/10 bit coming from? Is some form of scaling the old value? If not, how was it derived?
2 -What is the purpose of the hertz? I know what it is, just not what it does.

Looks to me like 90% of the last pass speed + 10% of the current speed (current position - last pass position for 1 pass in the loop)

...where you take that change in position (for 1 pass) times the passes/second (HZ or hertz) to change it to units to match vel_last

That's a low pass filter on speed, whose purpose is to reduce noise in the signal.

[Mr. Lim]

As above, the 9/10 section is a less expensive way of "taking the average of the last 10 samples"...

Nice in case a GTS or encoder vibrates across a transition, which results in a few phantom pulses. It won't cause a huge reaction in your system.

The HZ is a scale factor for the 26ms cycle period. The parameter for setting velocity is in pulses per second, so muliplying by HZ will convert from pulses per cycle to pulses per second.

[AustinSchuh]

One way to help solve the problem is to implement Feed Forward. The basic idea behind Feed Foward is to utilize any extra information you know to make the controller more accurate and responsive.

For driving, the best feed forward information to utilize is your desired drive velocity. If you want to drive at 1/2 of full speed, it takes a certain power level to be applied to each wheel. That number won't change too much, or at least more than your PID controller can't deal with. You can implement that term by adding in a power value to the PID controller that you look up in a lookup table using the goal velocity.

You can have as much fun as you want imagining all sorts of feed forward ideas to implement that will help model what power you need to give the motors before the PID controller deals with the last little bit. I have always wanted to figure out how to get the controller to add in extra power when it knows it wants to accelerate, but I haven't figured out how to model that yet to implement it.

[Jared Russell]

A couple things to keep in mind here:

The standard POSITION PID loop might look something like this:

output = Kp*e_pos + Ki*e_pos_sum + Kd*e_pos_delta;

Where output is the output.
e_pos is the error in position = desired_pos - actual_pos
e_pos_sum is the sum of the position errors = e_pos_sum + e_pos
e_pos_delta is the derivative of the error = e_pos - e_pos_last

(Yes, I know there are other ways to write the PID equation - I usually use the recursive discrete form so I don't need to keep track of an error sum, but I digress...).

This loop, if well tuned, should provide pretty good position control. But what about VELOCITY? Speed is the first derivative of position - so we could differentiate the position loop with respect to time to obtain a velocity controller.

I will use the D() operator to represent taking the time derivative.

D(output) = Kp*D(e_pos) + Ki*D(e_pos_sum) + Kd*D(e_pos_delta);

So far so good. What's the derivative of the output? Well, that's the change in output over time, so D(output) = output - output_last.

Whats the derivative of e_pos? Remember that e_pos itself is (desired_pos - actual_pos). It's derivative would simply replace "pos" with "vel".

e_vel = D(e_pos) = desired_vel - actual_vel.
e_vel_sum = D(e_pos_sum) = e_vel_sum + e_vel.
e_vel_delta = D(e_pos_delta) = e_vel - e_vel_last.

Putting it all together, you get:

output - output_last = Kp*e_vel + Ki*e_vel_sum + Kd*e_vel_delta;

Let's rearrange...

output = output_last + Kp*e_vel + Ki*e_vel_sum + Kd*e_vel_delta;

or...

output += Kp*e_vel + Ki*e_vel_sum + Kd*e_vel_delta;
(assuming output is global or static and persists between loop iterations)

So that's how you'd make a velocity controller. Note that the constants will be different than from the positional case (obviously).

One other thing - most of the time, people only care about closed loop velocity control in a robot in order to GO STRAIGHT. Having two separate PID controllers (one for each side) is one way to do this. But what if I bump the robot off course? The two sides will independently stabilize near the setpoint after the disturbance, but the entire robot may be pointed in a new direction.

Without giving everything away, what if your error term was not (desired - actual), but rather (left_actual - right_actual)? With some changes to the above equation, and a nonzero Ki, the robot will correct its course as it drives, effectively slowing down one motor when the other gets slowed down because of the environment.

[SuperBK]

Thanks for the great replies. It validates that we should keep working on it and that it can work. So far it kinda works, but we have to turn down the proportional gain so far that it actually ramps to the desired velocity because if its highter it will oscillate around the desired velocity. Then since the proportional gain is so low, it can't make small changes to reach the desired speed. Adding in some integral makes it go crazy - it needs to be scaled way back and limited.

One problem we have is that there is a lag from the time you tell the motor to go and the time it starts moving. I see it with prints. You can see it in normal mode where you just assign the pwm value from the joystick. By the time the motor actually starts moving, it has already ran the PID a few times and cranked up the drive too high. Therefore the wheels starts spinning faster than they should. The same thing happens when holding a wheel and you let go and it spins faster for a short time. Each time through the velocity loop it adds to the drive.

Also, we are using gear tooth sensors, not encoders. We cannot tell the direction, so we assume the speed is in the direction we are pointing the joystick. At first we were using the value of the previouls drive, but if it crosses 127, then it will think the speed is going the wrong way. The GTS give us 1200 counts per seconds at full speed. We time it for 50ms and that gives us a relative "speed" of 0-60. Then we try to scale that 0-60 so it looks like the 0-255that comes from the joystick. The joystick minus the speed is our error.

I'm thinking of trying some "feed forward" that starts the wheel off moving at a value close to where it should be, then let the PID home it in. It seems like we need to allow time for the system to react before adjusting it again.

Thanks,
Brian