Speed PID Function

[Ether]

Originally Posted by John_1102

My mentor sent me this.

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.

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:

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