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

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;
   }
 
   /**