Speed PID Function

[khatamidk]

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()

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