Added linear digital filters

Linear digital filter class based on code from FRC team 341

Change-Id: I4c5198e36a089e08a6d054bf1bf80392def27e23

wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/TestBench.java

@@ -19,13 +19,17 @@ import edu.wpi.first.wpilibj.CANJaguar;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.DigitalOutput;
 import edu.wpi.first.wpilibj.Jaguar;
+import edu.wpi.first.wpilibj.PIDSource;
 import edu.wpi.first.wpilibj.Relay;
 import edu.wpi.first.wpilibj.Servo;
 import edu.wpi.first.wpilibj.Talon;
 import edu.wpi.first.wpilibj.Victor;
+import edu.wpi.first.wpilibj.filters.LinearDigitalFilter;
 import edu.wpi.first.wpilibj.fixtures.AnalogCrossConnectFixture;
 import edu.wpi.first.wpilibj.fixtures.CANMotorEncoderFixture;
 import edu.wpi.first.wpilibj.fixtures.DIOCrossConnectFixture;
+import edu.wpi.first.wpilibj.fixtures.FilterNoiseFixture;
+import edu.wpi.first.wpilibj.fixtures.FilterOutputFixture;
 import edu.wpi.first.wpilibj.fixtures.MotorEncoderFixture;
 import edu.wpi.first.wpilibj.fixtures.RelayCrossConnectFixture;
 import edu.wpi.first.wpilibj.fixtures.TiltPanCameraFixture;
@@ -79,6 +83,17 @@ public final class TestBench {
   public static final int DIOCrossConnectA2 = 7;
   public static final int DIOCrossConnectA1 = 6;
 
+  // Filter constants
+  public static final double kStdDev = 10.0;
+  public static final double kFilterStep = 0.005;
+  public static final double kFilterTime = 2.0;
+  public static final double kSinglePoleIIRTimeConstant = 0.015915;
+  public static final double kSinglePoleIIRExpectedOutput = -3.2172003;
+  public static final double kHighPassTimeConstant = 0.006631;
+  public static final double kHighPassExpectedOutput = 10.074717;
+  public static final int kMovAvgTaps = 6;
+  public static final double kMovAvgExpectedOutput = -10.191644;
+
   /** The Singleton instance of the Test Bench */
   private static TestBench instance = null;
 
@@ -410,6 +425,86 @@ public final class TestBench {
     return encoderPortPairs;
   }
 
+  /**
+   * Constructs a new set of objects representing a single-pole IIR filter with
+   * a noisy data source
+   *
+   * @return a single-pole IIR filter with a noisy data source
+   */
+  public FilterNoiseFixture<LinearDigitalFilter> getSinglePoleIIRNoiseFixture() {
+    return new FilterNoiseFixture<LinearDigitalFilter>() {
+      @Override
+      protected LinearDigitalFilter giveFilter(PIDSource source) {
+        return LinearDigitalFilter.singlePoleIIR(source,
+                                                 kSinglePoleIIRTimeConstant,
+                                                 kFilterStep);
+      }
+    };
+  }
+
+  /**
+   * Constructs a new set of objects representing a moving average filter with a
+   * noisy data source using a linear digital filter
+   *
+   * @return a moving average filter with a noisy data source
+   */
+  public FilterNoiseFixture<LinearDigitalFilter> getMovAvgNoiseFixture() {
+    return new FilterNoiseFixture<LinearDigitalFilter>() {
+      @Override
+      protected LinearDigitalFilter giveFilter(PIDSource source) {
+        return LinearDigitalFilter.movingAverage(source, kMovAvgTaps);
+      }
+    };
+  }
+
+  /**
+   * Constructs a new set of objects representing a single-pole IIR filter with
+   * a repeatable data source
+   *
+   * @return a single-pole IIR filter with a repeatable data source
+   */
+  public FilterOutputFixture<LinearDigitalFilter> getSinglePoleIIROutputFixture() {
+    return new FilterOutputFixture<LinearDigitalFilter>(kSinglePoleIIRExpectedOutput) {
+      @Override
+      protected LinearDigitalFilter giveFilter(PIDSource source) {
+        return LinearDigitalFilter.singlePoleIIR(source,
+                                                 kSinglePoleIIRTimeConstant,
+                                                 kFilterStep);
+      }
+    };
+  }
+
+  /**
+   * Constructs a new set of objects representing a high-pass filter with a
+   * repeatable data source
+   *
+   * @return a high-pass filter with a repeatable data source
+   */
+  public FilterOutputFixture<LinearDigitalFilter> getHighPassOutputFixture() {
+    return new FilterOutputFixture<LinearDigitalFilter>(kHighPassExpectedOutput) {
+      @Override
+      protected LinearDigitalFilter giveFilter(PIDSource source) {
+        return LinearDigitalFilter.highPass(source, kHighPassTimeConstant,
+                                            kFilterStep);
+      }
+    };
+  }
+
+  /**
+   * Constructs a new set of objects representing a moving average filter with a
+   * repeatable data source using a linear digital filter
+   *
+   * @return a moving average filter with a repeatable data source
+   */
+  public FilterOutputFixture<LinearDigitalFilter> getMovAvgOutputFixture() {
+    return new FilterOutputFixture<LinearDigitalFilter>(kMovAvgExpectedOutput) {
+      @Override
+      protected LinearDigitalFilter giveFilter(PIDSource source) {
+        return LinearDigitalFilter.movingAverage(source, kMovAvgTaps);
+      }
+    };
+  }
+
   /**
    * Gets the singleton of the TestBench. If the TestBench is not already
    * allocated in constructs an new instance of it. Otherwise it returns the