Chief Delphi - Speed PID Function

Page 3 of 4

Show 100 post(s) from this thread on one page

Chief Delphi (http://www.chiefdelphi.com/forums/index.php)

- Programming (http://www.chiefdelphi.com/forums/forumdisplay.php?f=51)

- - Speed PID Function (http://www.chiefdelphi.com/forums/showthread.php?t=101244)

Ether

01-02-2012 14:17

Re: Speed PID Function

Quote:

Originally Posted by John_1102 (Post 1117895)

My mentor explained to me this way.
Your first 9 values are going to be very skewed so wait until you have at least 9 values in your array before you start averaging. Now counter is a pointer variable it ONLY points to a position in the array. I add all the values and get an average every time after 9 samples. I haven't tested changing speeds yet. Only constants.

Your code as written doesn't do what you described above. That was the point of the two responses (post#28 & post#29) to your post#27.

Show your code to your mentor and ask him.

JohnFogarty

01-02-2012 14:24

I just saw what you meant you are right. My bad.

JohnFogarty

01-02-2012 19:22

My mentor sent me this.

Quote:

That should work just fine. I was trying to reduce the number of arithmetic calculations, which would be necessary if the array/table was larger than it is. If you take the sum and subtract the oldest value and add the newest value it take a lot less time. Can you imagine adding up 100 or more entries each time?

In the final example:
What you want is a circular buffer (ring buffer) that you populate each cycle and use each cycle to get a new speed, like this:
Code:
public void getSpeed(){
samples[counter++] = Shooter_En.getRaw();
Shooter_En.reset();
if(counter>9) counter=0;
sum=0;
for(int i = 0; i <= 9; i++) sum += samples[i];
Rate = sum / (10* 0.48);
I have to assume that the first encoder value is way out of line and will skew the results since you have not reset the encoder until you read it the first time. As a matter of fact, until you have 11 samples the Rate is not really valid since you have some amount of samples that are zero since the array has not been filled yet. Would you consider this? Once you have 10 samples, a valid flag says to compute the Rate and could tell the user that it is now valid. See This;

public void getSpeed(){
samples[counter++] = Shooter_En.getRaw();
Shooter_En.reset();
if (counter >9){ counter = 0; ValidFlag = true;}

If (ValidFlag)
{
sum = 0;
for(int i = 0; i < counter; i++) sum += samples[i];
Rate = sum / (10* 0.48);
}

I first questioned the last line: Rate = sum / (10* 0.48); I thought is should be: Rate = (sum /10) / 0.48; Then realized it is the same thing. You should make it Rate = sum/4.80; to eliminate the multiplication.

Dennis

Ether

01-02-2012 20:34

Re: Speed PID Function

Quote:

Originally Posted by John_1102 (Post 1118091)

My mentor sent me this.

Quote:

I have to assume that the first encoder value is way out of line and will skew the results since you have not reset the encoder until you read it the first time. As a matter of fact, until you have 11 samples the Rate is not really valid since you have some amount of samples that are zero since the array has not been filled yet. Would you consider this? Once you have 10 samples, a valid flag says to compute the Rate and could tell the user that it is now valid.

Code:

public void getSpeed(){

samples[counter++] = Shooter_En.getRaw();

Shooter_En.reset();

if (counter >9){ counter = 0; ValidFlag = true;}



If (ValidFlag)

{

sum = 0;

for(int i = 0; i < counter; i++) sum += samples[i];

Rate = sum / (10* 0.48);

}

The added code suppresses updating the rate calculation while the buffer is initially filling. If you are sampling at 20ms it will take only 1/5 of a second to fill the ring buffer at startup. Whether or not that is a problem depends on what this code is being used for. For a shooter flywheel speed signal my guess is you won't be able to tell the difference.

Another option is to use an IIR filter instead of FIR. It's simpler and it's tunable:

Code:

public void getSpeed(){ 

  filtered_count = a*filtered_count + (1-a)*Shooter_En.getRaw();

  Shooter_En.reset();

  Rate =  filtered_count /0.48;

}

... set a=0 and you've got no filtering. As you increase a from zero to 1, the filtering becomes stronger.

Quote:

I first questioned the last line: Rate = sum / (10* 0.48); I thought is should be: Rate = (sum /10) / 0.48; Then realized it is the same thing. You should make it Rate = sum/4.80; to eliminate the multiplication.

Changing 10*0.48 to 4.80 does not eliminate a runtime multiplication. The multiplication is done at compile time. Leaving it 10*0.48 makes it clearer where the numbers came from.

JohnFogarty

02-02-2012 21:41

Re: Speed PID Function

Am I stupid.
Or is the CAN Jaguar functions built to do all of this for me.

Ether

02-02-2012 23:03

Re: Speed PID Function

Quote:

Originally Posted by John_1102 (Post 1118870)

is the CAN Jaguar functions built to do all of this for me.

If you use CAN to communicate with the Jag, then you can plug the encoder directly in to the Jag and access the Jag's built-in PID (built-in to the Jag's firmware) to control speed. There are several threads on CD discussing the pros and cons of doing this.

I don't know what kind of noise filtering, if any, the Jag does on the encoder signal.

Others on this forum have studied the Jag firmware source code (freely available) and may be able to answer.

vamfun

09-02-2012 17:44

Re: Speed PID Function

Quote:

Originally Posted by Ether (Post 1118130)

Another option is to use an IIR filter instead of FIR. It's simpler and it's tunable:

Code:

public void getSpeed(){ 

  filtered_count = a*filtered_count + (1-a)*Shooter_En.getRaw();

  Shooter_En.reset();

  Rate =  filtered_count /0.48;

}

... set a=0 and you've got no filtering. As you increase a from zero to 1, the filtering becomes stronger.

I agree with you here, but if the FIR version is desired, they should implement it in the recursive form. It then approaches the timing of the IIR filter. The ring is still required but rather than summing over the whole ring each time the output is updated with the difference between the fresh sample and the oldest sample.

avg(n) = avg(n) + (sample(n) -sample(n-L))/L where L is the length of the average.

Essentially before you overwrite the oldest sample, you calculate the difference and then overwrite and compute the new average.

Ether

09-02-2012 17:55

Re: Speed PID Function

Quote:

Originally Posted by vamfun (Post 1123274)

The ring is still required but rather than summing over the whole ring each time the output is updated with the difference between the fresh sample and the oldest sample.

Yup.

Although the bigger question here is, do they really need a filter at all? If they're reading the encoder delta counts every execution cycle and dividing by the cycle period, there's already a whole lotta averagin' goin' on (except at very low speeds).

vamfun

09-02-2012 18:15

Re: Speed PID Function

Quote:

Originally Posted by Ether (Post 1123281)

Yup:)
For our team, we plan to bypass getRate() and derive encoder rate by (encoder – encoder_last)/T where T is the loop update time. Our typical wheel speed is 2000 rpm or 33.3 rps . The encoder has 250cnts per rev so there are about 8333 cnts/sec. With a 20 ms loop, there are 167 cnts per cycle. If 1 pulse is missed, it is less than .6% error. The code wheel slit phase errors are being averaged over half of the wheel so much of the high frequency phase variations that would affect getRate() are averaged out.

khatamidk

11-02-2012 12:01

Re: Speed PID Function

Our team was also getting wild fluctuations with velocity, with an error range of upwards of 300 RPM and spikes that extend well past 1000 RPM beyond our setpoint. This was unacceptable for our PID control loop, so our mentor suggested we implement a moving average/median filter to smooth out the graph and get rid of those nasty rate spikes. We also changed the encoder rate to k1X instead of k4X (default).

We are working with the command-subsystem in Java, so this should might help those that are working with PIDSubsystems (which our Shooter class is). We only changed the returnPIDInput() method.

The result was an incredibly smooth graph, fluctuating within an acceptable range that can be tuned with the P constant (and maybe D depending on how close to the value we get).

Here is a picture of repeated tests.

In the above picture, there were 5 tests separated by the deep drops to zero (First 4 were set to 1000RPM, the last set to 2000RPM0. You can see how much smoother the fluctuation of RPM is (the fluctuation that still persists is a result of a much-needed PID constant tuning). As stated earlier, we went from a 300-1000 RPM spike/fluctuation to one less than 100 RPM. We also played around with the sampling size, and we chose 20. It did not affect cRIO CPU usage very much at all. 100 sampling size provided no advantage from what we saw compared to 20.

Here is our implementation of the returnPIDInput()

Code:

private double[] samplingValues = new double[HW.SAMPLING_SIZE];



    //MOVING MEDIAN/AVERAGE FILTER BASED ON A SAMPLING SIZE OF "N" VALUES (ex. 20)

    protected double returnPIDInput() {

        

        //LOOP THROUGH AND SHIFT ALL VALUES TO THE RIGHT BY ONE, REMOVING THE LAST VALUE

        for(int i = samplingValues.length-1; i > 0; i--){

            samplingValues[i] = samplingValues[i-1];

        }

        //PUSH IN A NEW VALUE BASED ON RATE (we are using RPS*60 = RPM)

        samplingValues[0] = shooter_encoder.getRate()*60;

        

        //TEMP ARRAY FOR PERFORMING MEDIAN OPERATION

        double[] median = Arrays.copy(samplingValues, 0, HW.SAMPLING_SIZE, 0, HW.SAMPLING_SIZE);

        Arrays.sort(median);

        

        //RETURNS THE MIDDLE OF THE SORTED MEDIAN ARRAY

        return median[median.length/2];

    }

Hope this helps,
David K.
Team Spectrum
FRC#3847

Ether

11-02-2012 13:38

Re: Speed PID Function

Quote:

Originally Posted by khatamidk (Post 1124240)

...

David,

If I am reading this correctly, you are doing the following:

1) Using GetRate() to grab the "instantaneous" (consecutive-count) rate from the FPGA, and then

2) shifting all the values in an array to make room for this new datum (and dropping the oldest datum from the arrary, and then

3) sorting the whole array, and then

4) picking the median value from the middle of the array.

Have you tried any of the suggestions in earlier posts in this thread, such as:

a) Instead of using GetRate(), read the encoder counts instead, and divide by the sampling time (as explained in post39), or

b) Use a simple IIR filter (as shown in post37), or

c) Use an efficient FIR moving-window average filter (post37)?

vamfun

22-02-2012 15:42

Re: Speed PID Function

Here is a note on precalculating PID gains without a simulation. The note has the full derivation.Excerpt:
Note: Precomputing PID gains for a velocity shooter using pole/zero placement

I wanted to post some formula’s for deriving P and I gains for a velocity PID control loop such as a shooter used in the 2012 Rebound Rumble game.

Assumptions:
a) motor L/R time constant is small relative to motor control frequencies.
b) The motor first order time constant tau_m is known.
c) The input speeds are normalized to the maximum motor speed, w_max. w_max = 12v/ke*60/2pi rpm which is approx w_free of the motor.
d) The desired motor response time constant, tau_d, is smaller than the motor tau_m by a factor of r.

Then the integral gain KI is given by

KI = 1/tau_d

and the proportional gain KP is given by

KP = r = tau_m/tau_d ;

If the integral computation of the PID loop doesn’t contain a dt factor (i.e it is just an error accumulation) as is typical of the WPILIB PIDcontroller then the KI must be multiplied by the PID update period.

KI_WPILIB = T/tau_d where T is the PID period.

If you are feeding back RPM then the gains must be divided by w_max.

Example:

The 599 shooter wheel has an open loop time constant tau_m = .68 sec

We want to have a tau_d = .33 sec which means the error should be around 1% of the target by 1 second (three time constants). During autonomous this would be the delay before we can shoot.

KI = 1/tau_d = 3.

KP = tau_m/tau_d = 3*.68 = 2.04 ;

For WPILIB we want KI_WPILIB = T*KI = .05*3 = .15 ; KP remains the same.

The shooter has a w_max = 5614 rpm so if the feedbacks are not normalized then KP and KI must be divided by w_max. or

KP = 2.04/5614 = 3.63 e-4 ; KI_WPILIB = .15/5614 = 2.49e-5 ;

Thats it. The main factor limiting the value of KP is the magnitude of the noise on the RPM feedback signal. Typically after filtering this can get down to 1% of the RPM. KP multiplies the noise so to keep the noise below 5% then the max KP would be 5.

Tom Line

22-02-2012 16:03

Re: Speed PID Function

I wanted to sum this information up for some of the team members (since a couple of the kids on my team didn't understand a lot of things in this post).

First - the getrate implementation (at least in labview) oscillates because it has a very short time period. To obtain a more stable rate, you canput the calculation function in periodic tasks and run it at its own loop speed. This function should be:

(current encoder count - past encoder count) / loop time = count rate

To smooth this rate further, you can average the samples over several loops, or you can extend the time (calculate the count over 2 or three loops and the time over 2 or three loops). Both may give you somewhat delayed results, depending on your loop rate.

Next, a speed PID varies mainly in that you need to send the setpoint from the last speed loop into the new loop and modify THAT value. This prevents wheel reversal. The derivation from Jared on team 341 can be found here:

http://www.chiefdelphi.com/forums/sh...7&postcount=13

It shows the derivation and implementation of a velocity PID loop.

Ether

22-02-2012 17:15

Re: Speed PID Function

Quote:

Originally Posted by vamfun (Post 1132752)

b) The motor first order time constant tau_m is known

Can you address this in a bit more detail?

Are there some fairly simple tests that a team could do to obtain this number?

Or do you recommend calculating, and if so it might be helpful to do a blog entry on that or post some example calculations here.

Ether

22-02-2012 17:36

Re: Speed PID Function

Quote:

Originally Posted by Tom Line (Post 1132769)

...or you could set the encoder to 1x, if you haven't done so already

Quote:

This function should be:

(current encoder count - past encoder count) / loop time = count rate

To smooth this rate further, you can average the samples over several loops, or you can extend the time (calculate the count over 2 or three loops and the time over 2 or three loops). Both may give you somewhat delayed results, depending on your loop rate.

I think taking readings every execution cycle and filtering them would be a better solution than taking readings only every n^th cycle. Can anyone comment on that?

Quote:

a speed PID varies mainly in that you need to send the setpoint from the last speed loop into the new loop and modify THAT value...The derivation from Jared on team 341 can be found here:

http://www.chiefdelphi.com/forums/sh...7&postcount=13

I believe what Jared showed was that you need to accumulate the output of the PID.

All times are GMT -5. The time now is 08:53.

Page 3 of 4

Show 100 post(s) from this thread on one page