Clean up LinearDigitalFilter class (#782)

* Renamed LinearDigitalFilter to LinearFilter
* Filter base class removed since it wasn't useful
* C++: std::shared_ptr<> replaced with double parameter

wpilibc/src/main/native/cpp/LinearFilter.cpp

@@ -0,0 +1,63 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2015-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/LinearFilter.h"
+
+#include <cassert>
+#include <cmath>
+
+using namespace frc;
+
+LinearFilter::LinearFilter(wpi::ArrayRef<double> ffGains,
+                           wpi::ArrayRef<double> fbGains)
+    : m_inputs(ffGains.size()),
+      m_outputs(fbGains.size()),
+      m_inputGains(ffGains),
+      m_outputGains(fbGains) {}
+
+LinearFilter LinearFilter::SinglePoleIIR(double timeConstant, double period) {
+  double gain = std::exp(-period / timeConstant);
+  return LinearFilter(1.0 - gain, -gain);
+}
+
+LinearFilter LinearFilter::HighPass(double timeConstant, double period) {
+  double gain = std::exp(-period / timeConstant);
+  const double ffGains[] = {gain, -gain};
+  return LinearFilter(ffGains, -gain);
+}
+
+LinearFilter LinearFilter::MovingAverage(int taps) {
+  assert(taps > 0);
+
+  std::vector<double> gains(taps, 1.0 / taps);
+  return LinearFilter(gains, {});
+}
+
+void LinearFilter::Reset() {
+  m_inputs.reset();
+  m_outputs.reset();
+}
+
+double LinearFilter::Calculate(double input) {
+  double retVal = 0.0;
+
+  // Rotate the inputs
+  m_inputs.push_front(input);
+
+  // Calculate the new value
+  for (size_t i = 0; i < m_inputGains.size(); i++) {
+    retVal += m_inputs[i] * m_inputGains[i];
+  }
+  for (size_t i = 0; i < m_outputGains.size(); i++) {
+    retVal -= m_outputs[i] * m_outputGains[i];
+  }
+
+  // Rotate the outputs
+  m_outputs.push_front(retVal);
+
+  return retVal;
+}

wpilibc/src/main/native/cpp/PIDBase.cpp

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -34,12 +34,8 @@ PIDBase::PIDBase(double Kp, double Ki, double Kd, double Kf, PIDSource& source,
   m_D = Kd;
   m_F = Kf;
 
-  // Save original source
-  m_origSource = std::shared_ptr<PIDSource>(&source, NullDeleter<PIDSource>());
-
-  // Create LinearDigitalFilter with original source as its source argument
-  m_filter = LinearDigitalFilter::MovingAverage(m_origSource, 1);
-  m_pidInput = &m_filter;
+  m_pidInput = &source;
+  m_filter = LinearFilter::MovingAverage(1);
 
   m_pidOutput = &output;
 
@@ -200,10 +196,7 @@ void PIDBase::SetPercentTolerance(double percent) {
 
 void PIDBase::SetToleranceBuffer(int bufLength) {
   std::lock_guard<wpi::mutex> lock(m_thisMutex);
-
-  // Create LinearDigitalFilter with original source as its source argument
-  m_filter = LinearDigitalFilter::MovingAverage(m_origSource, bufLength);
-  m_pidInput = &m_filter;
+  m_filter = LinearFilter::MovingAverage(bufLength);
 }
 
 bool PIDBase::OnTarget() const {
@@ -249,7 +242,7 @@ void PIDBase::InitSendable(SendableBuilder& builder) {
 }
 
 void PIDBase::Calculate() {
-  if (m_origSource == nullptr || m_pidOutput == nullptr) return;
+  if (m_pidInput == nullptr || m_pidOutput == nullptr) return;
 
   bool enabled;
   {
@@ -277,7 +270,7 @@ void PIDBase::Calculate() {
     {
       std::lock_guard<wpi::mutex> lock(m_thisMutex);
 
-      input = m_pidInput->PIDGet();
+      input = m_filter.Calculate(m_pidInput->PIDGet());
 
       pidSourceType = m_pidInput->GetPIDSourceType();
       P = m_P;

wpilibc/src/main/native/include/frc/LinearFilter.h

@@ -0,0 +1,140 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2015-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <vector>
+
+#include <wpi/ArrayRef.h>
+
+#include "frc/circular_buffer.h"
+
+namespace frc {
+
+/**
+ * This class implements a linear, digital filter. All types of FIR and IIR
+ * filters are supported. Static factory methods are provided to create commonly
+ * used types of filters.
+ *
+ * Filters are of the form:<br>
+ *  y[n] = (b0 * x[n] + b1 * x[n-1] + … + bP * x[n-P]) -
+ *         (a0 * y[n-1] + a2 * y[n-2] + … + aQ * y[n-Q])
+ *
+ * Where:<br>
+ *  y[n] is the output at time "n"<br>
+ *  x[n] is the input at time "n"<br>
+ *  y[n-1] is the output from the LAST time step ("n-1")<br>
+ *  x[n-1] is the input from the LAST time step ("n-1")<br>
+ *  b0 … bP are the "feedforward" (FIR) gains<br>
+ *  a0 … aQ are the "feedback" (IIR) gains<br>
+ * IMPORTANT! Note the "-" sign in front of the feedback term! This is a common
+ *            convention in signal processing.
+ *
+ * What can linear filters do? Basically, they can filter, or diminish, the
+ * effects of undesirable input frequencies. High frequencies, or rapid changes,
+ * can be indicative of sensor noise or be otherwise undesirable. A "low pass"
+ * filter smooths out the signal, reducing the impact of these high frequency
+ * components.  Likewise, a "high pass" filter gets rid of slow-moving signal
+ * components, letting you detect large changes more easily.
+ *
+ * Example FRC applications of filters:
+ *  - Getting rid of noise from an analog sensor input (note: the roboRIO's FPGA
+ *    can do this faster in hardware)
+ *  - Smoothing out joystick input to prevent the wheels from slipping or the
+ *    robot from tipping
+ *  - Smoothing motor commands so that unnecessary strain isn't put on
+ *    electrical or mechanical components
+ *  - If you use clever gains, you can make a PID controller out of this class!
+ *
+ * For more on filters, we highly recommend the following articles:<br>
+ * https://en.wikipedia.org/wiki/Linear_filter<br>
+ * https://en.wikipedia.org/wiki/Iir_filter<br>
+ * https://en.wikipedia.org/wiki/Fir_filter<br>
+ *
+ * Note 1: Calculate() should be called by the user on a known, regular period.
+ * You can use a Notifier for this or do it "inline" with code in a
+ * periodic function.
+ *
+ * Note 2: For ALL filters, gains are necessarily a function of frequency. If
+ * you make a filter that works well for you at, say, 100Hz, you will most
+ * definitely need to adjust the gains if you then want to run it at 200Hz!
+ * Combining this with Note 1 - the impetus is on YOU as a developer to make
+ * sure Calculate() gets called at the desired, constant frequency!
+ */
+class LinearFilter {
+ public:
+  /**
+   * Create a linear FIR or IIR filter.
+   *
+   * @param ffGains The "feed forward" or FIR gains.
+   * @param fbGains The "feed back" or IIR gains.
+   */
+  LinearFilter(wpi::ArrayRef<double> ffGains, wpi::ArrayRef<double> fbGains);
+
+  LinearFilter(LinearFilter&&) = default;
+  LinearFilter& operator=(LinearFilter&&) = default;
+
+  // Static methods to create commonly used filters
+  /**
+   * Creates a one-pole IIR low-pass filter of the form:<br>
+   *   y[n] = (1 - gain) * x[n] + gain * y[n-1]<br>
+   * where gain = e<sup>-dt / T</sup>, T is the time constant in seconds
+   *
+   * This filter is stable for time constants greater than zero.
+   *
+   * @param timeConstant The discrete-time time constant in seconds.
+   * @param period       The period in seconds between samples taken by the
+   *                     user.
+   */
+  static LinearFilter SinglePoleIIR(double timeConstant, double period);
+
+  /**
+   * Creates a first-order high-pass filter of the form:<br>
+   *   y[n] = gain * x[n] + (-gain) * x[n-1] + gain * y[n-1]<br>
+   * where gain = e<sup>-dt / T</sup>, T is the time constant in seconds
+   *
+   * This filter is stable for time constants greater than zero.
+   *
+   * @param timeConstant The discrete-time time constant in seconds.
+   * @param period       The period in seconds between samples taken by the
+   *                     user.
+   */
+  static LinearFilter HighPass(double timeConstant, double period);
+
+  /**
+   * Creates a K-tap FIR moving average filter of the form:<br>
+   *   y[n] = 1/k * (x[k] + x[k-1] + … + x[0])
+   *
+   * This filter is always stable.
+   *
+   * @param taps The number of samples to average over. Higher = smoother but
+   *             slower
+   */
+  static LinearFilter MovingAverage(int taps);
+
+  /**
+   * Reset the filter state.
+   */
+  void Reset();
+
+  /**
+   * Calculates the next value of the filter.
+   *
+   * @param input Current input value.
+   *
+   * @return The filtered value at this step
+   */
+  double Calculate(double input);
+
+ private:
+  circular_buffer<double> m_inputs{0};
+  circular_buffer<double> m_outputs{0};
+  std::vector<double> m_inputGains;
+  std::vector<double> m_outputGains;
+};
+
+}  // namespace frc

wpilibc/src/main/native/include/frc/PIDBase.h

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -14,11 +14,11 @@
 #include <wpi/mutex.h>
 
 #include "frc/Base.h"
+#include "frc/LinearFilter.h"
 #include "frc/PIDInterface.h"
 #include "frc/PIDOutput.h"
 #include "frc/PIDSource.h"
 #include "frc/Timer.h"
-#include "frc/filters/LinearDigitalFilter.h"
 #include "frc/smartdashboard/SendableBase.h"
 
 namespace frc {
@@ -215,7 +215,7 @@ class PIDBase : public SendableBase, public PIDInterface, public PIDOutput {
    *
    * @return the average error
    */
-  WPI_DEPRECATED("Use a LinearDigitalFilter as the input and GetError().")
+  WPI_DEPRECATED("Use a LinearFilter as the input and GetError().")
   virtual double GetAvgError() const;
 
   /**
@@ -397,8 +397,7 @@ class PIDBase : public SendableBase, public PIDInterface, public PIDOutput {
   double m_error = 0;
   double m_result = 0;
 
-  std::shared_ptr<PIDSource> m_origSource;
-  LinearDigitalFilter m_filter{nullptr, {}, {}};
+  LinearFilter m_filter{{}, {}};
 };
 
 }  // namespace frc

wpilibc/src/main/native/include/frc/PIDController.h

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -19,7 +19,6 @@
 #include "frc/PIDBase.h"
 #include "frc/PIDSource.h"
 #include "frc/Timer.h"
-#include "frc/filters/LinearDigitalFilter.h"
 
 namespace frc {
 

wpilibc/src/main/native/include/frc/filters/LinearDigitalFilter.h

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2015-2019 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -11,6 +11,7 @@
 #include <vector>
 
 #include <wpi/ArrayRef.h>
+#include <wpi/deprecated.h>
 
 #include "frc/circular_buffer.h"
 #include "frc/filters/Filter.h"
@@ -76,6 +77,7 @@ class LinearDigitalFilter : public Filter {
    * @param ffGains The "feed forward" or FIR gains
    * @param fbGains The "feed back" or IIR gains
    */
+  WPI_DEPRECATED("Use LinearFilter class instead.")
   LinearDigitalFilter(PIDSource& source, wpi::ArrayRef<double> ffGains,
                       wpi::ArrayRef<double> fbGains);
 
@@ -86,6 +88,7 @@ class LinearDigitalFilter : public Filter {
    * @param ffGains The "feed forward" or FIR gains
    * @param fbGains The "feed back" or IIR gains
    */
+  WPI_DEPRECATED("Use LinearFilter class instead.")
   LinearDigitalFilter(std::shared_ptr<PIDSource> source,
                       wpi::ArrayRef<double> ffGains,
                       wpi::ArrayRef<double> fbGains);