PIDController class now uses LinearDigitalFilter for filtering velocity instead of internal queue (#38)

wpilibc/src/main/native/cpp/PIDController.cpp

@@ -69,7 +69,13 @@ PIDController::PIDController(double Kp, double Ki, double Kd, double Kf,
   m_D = Kd;
   m_F = Kf;
 
-  m_pidInput = source;
+  // 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_pidOutput = output;
   m_period = period;
 
@@ -140,15 +146,6 @@ void PIDController::Calculate() {
     result = m_result;
 
     pidOutput->PIDWrite(result);
-
-    // Update the buffer.
-    m_buf.push(m_error);
-    m_bufTotal += m_error;
-    // Remove old elements when buffer is full.
-    if (m_buf.size() > m_bufLength) {
-      m_bufTotal -= m_buf.front();
-      m_buf.pop();
-    }
   }
 }
 
@@ -322,8 +319,6 @@ void PIDController::SetOutputRange(double minimumOutput, double maximumOutput) {
 /**
  * Set the setpoint for the PIDController.
  *
- * Clears the queue for GetAvgError().
- *
  * @param setpoint the desired setpoint
  */
 void PIDController::SetSetpoint(double setpoint) {
@@ -340,10 +335,6 @@ void PIDController::SetSetpoint(double setpoint) {
     } else {
       m_setpoint = setpoint;
     }
-
-    // Clear m_buf.
-    m_buf = std::queue<double>();
-    m_bufTotal = 0;
   }
 
   if (m_setpointEntry) m_setpointEntry.SetDouble(m_setpoint);
@@ -397,24 +388,6 @@ PIDSourceType PIDController::GetPIDSourceType() const {
   return m_pidInput->GetPIDSourceType();
 }
 
-/**
- * Returns the current average of the error over the past few iterations.
- *
- * You can specify the number of iterations to average with SetToleranceBuffer()
- * (defaults to 1). This is the same value that is used for OnTarget().
- *
- * @return the average error
- */
-double PIDController::GetAvgError() const {
-  double avgError = 0;
-  {
-    std::lock_guard<wpi::mutex> sync(m_mutex);
-    // Don't divide by zero.
-    if (m_buf.size()) avgError = m_bufTotal / m_buf.size();
-  }
-  return avgError;
-}
-
 /*
  * Set the percentage error which is considered tolerable for use with
  * OnTarget.
@@ -463,13 +436,10 @@ void PIDController::SetPercentTolerance(double percent) {
  */
 void PIDController::SetToleranceBuffer(int bufLength) {
   std::lock_guard<wpi::mutex> sync(m_mutex);
-  m_bufLength = bufLength;
 
-  // Cut the buffer down to size if needed.
-  while (m_buf.size() > static_cast<uint32_t>(bufLength)) {
-    m_bufTotal -= m_buf.front();
-    m_buf.pop();
-  }
+  // Create LinearDigitalFilter with original source as its source argument
+  m_filter = LinearDigitalFilter::MovingAverage(m_origSource, bufLength);
+  m_pidInput = &m_filter;
 }
 
 /*
@@ -484,12 +454,7 @@ void PIDController::SetToleranceBuffer(int bufLength) {
  * This will return false until at least one input value has been computed.
  */
 bool PIDController::OnTarget() const {
-  {
-    std::lock_guard<wpi::mutex> sync(m_mutex);
-    if (m_buf.size() == 0) return false;
-  }
-
-  double error = GetAvgError();
+  double error = GetError();
 
   std::lock_guard<wpi::mutex> sync(m_mutex);
   switch (m_toleranceType) {

wpilibc/src/main/native/include/Filters/Filter.h

@@ -23,7 +23,7 @@ class Filter : public PIDSource {
 
   // PIDSource interface
   void SetPIDSourceType(PIDSourceType pidSource) override;
-  PIDSourceType GetPIDSourceType() const;
+  PIDSourceType GetPIDSourceType() const override;
   double PIDGet() override = 0;
 
   /**

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

@@ -9,13 +9,13 @@
 
 #include <atomic>
 #include <memory>
-#include <queue>
 #include <string>
 
 #include <support/mutex.h>
 
 #include "Base.h"
 #include "Controller.h"
+#include "Filters/LinearDigitalFilter.h"
 #include "LiveWindow/LiveWindow.h"
 #include "Notifier.h"
 #include "PIDInterface.h"
@@ -64,7 +64,6 @@ class PIDController : public LiveWindowSendable, public PIDInterface {
   double GetDeltaSetpoint() const;
 
   virtual double GetError() const;
-  virtual double GetAvgError() const;
 
   virtual void SetPIDSourceType(PIDSourceType pidSource);
   virtual PIDSourceType GetPIDSourceType() const;
@@ -156,11 +155,8 @@ class PIDController : public LiveWindowSendable, public PIDInterface {
   double m_result = 0;
   double m_period;
 
-  // Length of buffer for averaging for tolerances.
-  std::atomic<unsigned> m_bufLength{1};
-
-  std::queue<double> m_buf;
-  double m_bufTotal = 0;
+  std::shared_ptr<PIDSource> m_origSource;
+  LinearDigitalFilter m_filter{nullptr, {}, {}};
 
   mutable wpi::mutex m_mutex;
 

wpilibc/src/main/native/include/PIDSource.h

@@ -20,7 +20,7 @@ enum class PIDSourceType { kDisplacement, kRate };
 class PIDSource {
  public:
   virtual void SetPIDSourceType(PIDSourceType pidSource);
-  PIDSourceType GetPIDSourceType() const;
+  virtual PIDSourceType GetPIDSourceType() const;
   virtual double PIDGet() = 0;
 
  protected: