@@ -0,0 +1,1417 @@
+package com.edinarobotics.utils.sensors;%0A%0A/**%0A * takes a series of values and smooths them using FIR filtering%0A * %0A */%0Apublic class FIRFilter %7B%0A    private double%5B%5D tapWeights;%0A    private double%5B%5D values;%0A    /**%0A     * constructs a FIR filter that has weights given by tapWeights%5B%5D%0A     * @param tapWeights is an array of the weights to be assigned to each tap%0A     */%0A    public FIRFilter(double%5B%5D tapWeights)%0A    %7B%0A        this.tapWeights = tapWeights;%0A        values = new double%5Bthis.tapWeights.length%5D;%0A    %7D%0A    /**%0A     * Performs FIR smoothing on values%5B%5D%0A     * @param currentVal is the newest value%0A     * @return a double representing the FIR smoothed value%0A     */%0A    public double filter(double currentVal)%0A    %7B%0A        //Shift values over 1 index, then add currentVal to values%0A        shiftArray();%0A        values%5B0%5D = currentVal;%0A        //Filtering%0A        double FIRValue = 0.0;%0A        for(int index = 0; index%3Cvalues.length&&index%3CtapWeights.length; index++)%0A        %7B%0A            FIRValue +=values%5Bindex%5D*tapWeights%5Bindex%5D;%0A        %7D%0A        return FIRValue;%0A        %0A    %7D %0A    private void shiftArray()%0A    %7B%0A        double%5B%5D tempArray = new double%5Bvalues.length%5D;%0A        for(int index = 1; index %3C tempArray.length; index++)%0A        %7B%0A            %0A           tempArray%5Bindex%5D = values%5Bindex-1%5D;%0A        %7D%0A        tempArray%5B0%5D = 0.0;%0A        %0A        values = tempArray;%0A    %7D%0A    %0A    %0A%7D%0A

Add FIRFilter to smooth values from ultrasonic sensor.