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:

wpilibcIntegrationTests/src/FRCUserProgram/cpp/PIDToleranceTest.cpp

@@ -58,21 +58,21 @@ TEST_F(PIDToleranceTest, Absolute) {
 
   EXPECT_FALSE(pid->OnTarget())
       << "Error was in tolerance when it should not have been. Error was "
-      << pid->GetAvgError();
+      << pid->GetError();
 
   inp.val = setpoint + tolerance / 2;
   Wait(1.0);
 
   EXPECT_TRUE(pid->OnTarget())
       << "Error was not in tolerance when it should have been. Error was "
-      << pid->GetAvgError();
+      << pid->GetError();
 
   inp.val = setpoint + 10 * tolerance;
   Wait(1.0);
 
   EXPECT_FALSE(pid->OnTarget())
       << "Error was in tolerance when it should not have been. Error was "
-      << pid->GetAvgError();
+      << pid->GetError();
 }
 
 TEST_F(PIDToleranceTest, Percent) {
@@ -84,7 +84,7 @@ TEST_F(PIDToleranceTest, Percent) {
 
   EXPECT_FALSE(pid->OnTarget())
       << "Error was in tolerance when it should not have been. Error was "
-      << pid->GetAvgError();
+      << pid->GetError();
 
   inp.val =
       setpoint + (tolerance) / 200 *
@@ -93,7 +93,7 @@ TEST_F(PIDToleranceTest, Percent) {
 
   EXPECT_TRUE(pid->OnTarget())
       << "Error was not in tolerance when it should have been. Error was "
-      << pid->GetAvgError();
+      << pid->GetError();
 
   inp.val =
       setpoint +
@@ -103,5 +103,5 @@ TEST_F(PIDToleranceTest, Percent) {
 
   EXPECT_FALSE(pid->OnTarget())
       << "Error was in tolerance when it should not have been. Error was "
-      << pid->GetAvgError();
+      << pid->GetError();
 }

wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDController.java

@@ -7,13 +7,12 @@
 
 package edu.wpi.first.wpilibj;
 
-import java.util.ArrayDeque;
-import java.util.Queue;
 import java.util.TimerTask;
 
 import edu.wpi.first.networktables.EntryListenerFlags;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.wpilibj.filters.LinearDigitalFilter;
 import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable;
 import edu.wpi.first.wpilibj.util.BoundaryException;
 
@@ -54,14 +53,15 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
   private double m_totalError = 0.0;
   // the tolerance object used to check if on target
   private Tolerance m_tolerance;
-  private int m_bufLength = 1;
-  private Queue<Double> m_buf;
-  private double m_bufTotal = 0.0;
   private double m_setpoint = 0.0;
   private double m_prevSetpoint = 0.0;
   private double m_error = 0.0;
   private double m_result = 0.0;
   private double m_period = kDefaultPeriod;
+
+  PIDSource m_origSource;
+  LinearDigitalFilter m_filter;
+
   protected PIDSource m_pidInput;
   protected PIDOutput m_pidOutput;
   java.util.Timer m_controlLoop;
@@ -96,8 +96,7 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
 
     @Override
     public boolean onTarget() {
-      return isAvgErrorValid() && Math.abs(getAvgError()) < m_percentage / 100 * (m_maximumInput
-          - m_minimumInput);
+      return Math.abs(getError()) < m_percentage / 100 * (m_maximumInput - m_minimumInput);
     }
   }
 
@@ -110,7 +109,7 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
 
     @Override
     public boolean onTarget() {
-      return isAvgErrorValid() && Math.abs(getAvgError()) < m_value;
+      return Math.abs(getError()) < m_value;
     }
   }
 
@@ -157,7 +156,13 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
     m_D = Kd;
     m_F = Kf;
 
-    m_pidInput = source;
+    // Save original source
+    m_origSource = source;
+
+    // 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;
 
@@ -166,8 +171,6 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
     instances++;
     HLUsageReporting.reportPIDController(instances);
     m_tolerance = new NullTolerance();
-
-    m_buf = new ArrayDeque<Double>(m_bufLength + 1);
   }
 
   /**
@@ -282,14 +285,6 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
 
         pidOutput = m_pidOutput;
         result = m_result;
-
-        // Update the buffer.
-        m_buf.add(m_error);
-        m_bufTotal += m_error;
-        // Remove old elements when the buffer is full.
-        if (m_buf.size() > m_bufLength) {
-          m_bufTotal -= m_buf.remove();
-        }
       }
 
       pidOutput.pidWrite(result);
@@ -471,7 +466,7 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
   }
 
   /**
-   * Set the setpoint for the PIDController Clears the queue for GetAvgError().
+   * Set the setpoint for the PIDController.
    *
    * @param setpoint the desired setpoint
    */
@@ -488,9 +483,6 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
       m_setpoint = setpoint;
     }
 
-    m_buf.clear();
-    m_bufTotal = 0;
-
     if (m_setpointEntry != null) {
       m_setpointEntry.setDouble(m_setpoint);
     }
@@ -541,32 +533,6 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
     return m_pidInput.getPIDSourceType();
   }
 
-  /**
-   * Returns the current difference of the error over the past few iterations. You can specify the
-   * number of iterations to average with setToleranceBuffer() (defaults to 1). getAvgError() is
-   * used for the onTarget() function.
-   *
-   * @return the current average of the error
-   */
-  public synchronized double getAvgError() {
-    double avgError = 0;
-    // Don't divide by zero.
-    if (m_buf.size() != 0) {
-      avgError = m_bufTotal / m_buf.size();
-    }
-    return avgError;
-  }
-
-  /**
-   * Returns whether or not any values have been collected. If no values have been collected,
-   * getAvgError is 0, which is invalid.
-   *
-   * @return True if {@link #getAvgError()} is currently valid.
-   */
-  private synchronized boolean isAvgErrorValid() {
-    return m_buf.size() != 0;
-  }
-
   /**
    * Set the PID tolerance using a Tolerance object. Tolerance can be specified as a percentage of
    * the range or as an absolute value. The Tolerance object encapsulates those options in an
@@ -609,12 +575,8 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
    * @param bufLength Number of previous cycles to average.
    */
   public synchronized void setToleranceBuffer(int bufLength) {
-    m_bufLength = bufLength;
-
-    // Cut the existing buffer down to size if needed.
-    while (m_buf.size() > bufLength) {
-      m_bufTotal -= m_buf.remove();
-    }
+    m_filter = LinearDigitalFilter.movingAverage(m_origSource, bufLength);
+    m_pidInput = m_filter;
   }
 
   /**

wpilibj/src/test/java/edu/wpi/first/wpilibj/PIDToleranceTest.java

@@ -77,15 +77,15 @@ public class PIDToleranceTest {
     m_pid.enable();
     Timer.delay(1);
     assertFalse("Error was in tolerance when it should not have been. Error was "
-        + m_pid.getAvgError(), m_pid.onTarget());
+        + m_pid.getError(), m_pid.onTarget());
     m_inp.m_val = m_setPoint + m_tolerance / 2;
     Timer.delay(1.0);
     assertTrue("Error was not in tolerance when it should have been. Error was "
-        + m_pid.getAvgError(), m_pid.onTarget());
+        + m_pid.getError(), m_pid.onTarget());
     m_inp.m_val = m_setPoint + 10 * m_tolerance;
     Timer.delay(1.0);
     assertFalse("Error was in tolerance when it should not have been. Error was "
-        + m_pid.getAvgError(), m_pid.onTarget());
+        + m_pid.getError(), m_pid.onTarget());
   }
 
   @Test
@@ -94,16 +94,16 @@ public class PIDToleranceTest {
     m_pid.setSetpoint(m_setPoint);
     m_pid.enable();
     assertFalse("Error was in tolerance when it should not have been. Error was "
-        + m_pid.getAvgError(), m_pid.onTarget());
+        + m_pid.getError(), m_pid.onTarget());
     //half of percent tolerance away from setPoint
     m_inp.m_val = m_setPoint + m_tolerance / 200 * m_range;
     Timer.delay(1.0);
     assertTrue("Error was not in tolerance when it should have been. Error was "
-        + m_pid.getAvgError(), m_pid.onTarget());
+        + m_pid.getError(), m_pid.onTarget());
     //double percent tolerance away from setPoint
     m_inp.m_val = m_setPoint + m_tolerance / 50 * m_range;
     Timer.delay(1.0);
     assertFalse("Error was in tolerance when it should not have been. Error was "
-        + m_pid.getAvgError(), m_pid.onTarget());
+        + m_pid.getError(), m_pid.onTarget());
   }
 }

wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java

@@ -185,8 +185,8 @@ public class MotorEncoderTest extends AbstractComsSetup {
     pid.disable();
 
     assertTrue(
-        "PID loop did not reach setpoint within 10 seconds. The average error was: " + pid
-            .getAvgError() + "The current error was" + pid.getError(), pid.onTarget());
+        "PID loop did not reach setpoint within 10 seconds. The current error was" + pid
+            .getError(), pid.onTarget());
 
     pid.free();
   }
@@ -206,7 +206,7 @@ public class MotorEncoderTest extends AbstractComsSetup {
     pid.disable();
 
     assertTrue(
-        "PID loop did not reach setpoint within 10 seconds. The error was: " + pid.getAvgError(),
+        "PID loop did not reach setpoint within 10 seconds. The error was: " + pid.getError(),
         pid.onTarget());
 
     pid.free();