Tuned test constants for VelocityPID.

Also added a GetAvgError method to the PIDController
which averages the past n error values for use with
noisy sensor values (namely, for the velocity stuff).

Change-Id: I8a9cf40259dd56ef9093b36ed6891cc18b9131cf

wpilibc/wpilibC++/include/PIDController.h

@@ -16,6 +16,9 @@
 
 #include <memory>
 
+#include <atomic>
+#include <queue>
+
 class PIDOutput;
 
 /**
@@ -53,6 +56,7 @@ class PIDController : public LiveWindowSendable,
   virtual double GetSetpoint() const override;
 
   virtual float GetError() const;
+  virtual float GetAvgError() const;
 
   virtual void SetPIDSourceType(PIDSourceType pidSource);
   virtual PIDSourceType GetPIDSourceType() const;
@@ -60,6 +64,7 @@ class PIDController : public LiveWindowSendable,
   virtual void SetTolerance(float percent);
   virtual void SetAbsoluteTolerance(float absValue);
   virtual void SetPercentTolerance(float percentValue);
+  virtual void SetToleranceBuffer(unsigned buf = 1);
   virtual bool OnTarget() const;
 
   virtual void Enable() override;
@@ -96,13 +101,19 @@ class PIDController : public LiveWindowSendable,
     kPercentTolerance,
     kNoTolerance
   } m_toleranceType = kNoTolerance;
-  float m_tolerance = 0.05;  // the percetage or absolute error that is considered on
-                      // target
+
+  // the percetage or absolute error that is considered on target.
+  float m_tolerance = 0.05;
   float m_setpoint = 0;
   float m_error;
   float m_result = 0;
   float m_period;
 
+  // Length of buffer for averaging for tolerances.
+  std::atomic<unsigned> m_bufLength{1};
+  std::queue<double> m_buf;
+  double m_bufTotal = 0;
+
   mutable priority_mutex m_mutex;
 
   std::unique_ptr<Notifier> m_controlLoop;

wpilibc/wpilibC++Devices/src/PIDController.cpp

@@ -169,6 +169,15 @@ 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();
+      }
     }
   }
 }
@@ -310,6 +319,7 @@ void PIDController::SetOutputRange(float minimumOutput, float maximumOutput) {
 
 /**
  * Set the setpoint for the PIDController
+ * Clears the queue for GetAvgError().
  * @param setpoint the desired setpoint
  */
 void PIDController::SetSetpoint(float setpoint) {
@@ -325,6 +335,9 @@ void PIDController::SetSetpoint(float setpoint) {
     } else {
       m_setpoint = setpoint;
     }
+
+    // Clear m_buf.
+    m_buf = std::queue<double>();
   }
 
   if (m_table != nullptr) {
@@ -368,6 +381,22 @@ 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
+ */
+float PIDController::GetAvgError() const {
+  float avgError = 0;
+  {
+    std::unique_lock<priority_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.
@@ -407,6 +436,25 @@ void PIDController::SetAbsoluteTolerance(float absTolerance) {
   }
 }
 
+/*
+ * Set the number of previous error samples to average for tolerancing. When
+ * determining whether a mechanism is on target, the user may want to use a
+ * rolling average of previous measurements instead of a precise position or
+ * velocity. This is useful for noisy sensors which return a few erroneous
+ * measurements when the mechanism is on target. However, the mechanism will
+ * not register as on target for at least the specified bufLength cycles.
+ * @param bufLength Number of previous cycles to average. Defaults to 1.
+ */
+void PIDController::SetToleranceBuffer(unsigned bufLength) {
+  m_bufLength = bufLength;
+
+  // Cut the buffer down to size if needed.
+  while (m_buf.size() > bufLength) {
+    m_bufTotal -= m_buf.front();
+    m_buf.pop();
+  }
+}
+
 /*
  * Return true if the error is within the percentage of the total input range,
  * determined by SetTolerance. This asssumes that the maximum and minimum input
@@ -417,7 +465,7 @@ void PIDController::SetAbsoluteTolerance(float absTolerance) {
  * time.
  */
 bool PIDController::OnTarget() const {
-  double error = GetError();
+  double error = GetAvgError();
 
   std::unique_lock<priority_mutex> sync(m_mutex);
   switch (m_toleranceType) {

wpilibc/wpilibC++IntegrationTests/src/MotorEncoderTest.cpp

@@ -150,18 +150,19 @@ TEST_P(MotorEncoderTest, VelocityPIDController) {
   Reset();
 
   m_encoder->SetPIDSourceType(PIDSourceType::kRate);
-  PIDController pid(0.002f, 0.0f, 0.0001f, m_encoder, m_speedController);
-  pid.SetAbsoluteTolerance(20.0f);
+  PIDController pid(1e-5, 0.0f, 3e-5, 8e-5, m_encoder, m_speedController);
+  pid.SetAbsoluteTolerance(50.0f);
+  pid.SetToleranceBuffer(10);
   pid.SetOutputRange(-0.3f, 0.3f);
-  pid.SetSetpoint(30);
+  pid.SetSetpoint(2000);
 
   /* 10 seconds should be plenty time to get to the setpoint */
   pid.Enable();
   Wait(10.0);
 
-  RecordProperty("PIDError", pid.GetError());
+  RecordProperty("PIDError", pid.GetAvgError());
 
-  EXPECT_TRUE(pid.OnTarget()) << "PID loop did not converge within 10 seconds.";
+  EXPECT_TRUE(pid.OnTarget()) << "PID loop did not converge within 10 seconds. Goal was: " << 2000 << " Error was: " << pid.GetError();
 
   pid.Disable();
 }

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

@@ -6,6 +6,7 @@
 package edu.wpi.first.wpilibj;
 
 import java.util.TimerTask;
+import java.util.LinkedList;
 
 import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable;
 import edu.wpi.first.wpilibj.tables.ITable;
@@ -39,6 +40,9 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
                                      // integral calc
   private Tolerance m_tolerance; // the tolerance object used to check if on
                                  // target
+  private int m_bufLength = 0;
+  private LinkedList<Double> m_buf;
+  private double m_bufTotal = 0.0;
   private double m_setpoint = 0.0;
   private double m_error = 0.0;
   private double m_result = 0.0;
@@ -79,7 +83,7 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
 
     @Override
     public boolean onTarget() {
-      return (Math.abs(getError()) < percentage / 100 * (m_maximumInput - m_minimumInput));
+      return (Math.abs(getAvgError()) < percentage / 100 * (m_maximumInput - m_minimumInput));
     }
   }
 
@@ -92,7 +96,7 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
 
     @Override
     public boolean onTarget() {
-      return Math.abs(getError()) < value;
+      return Math.abs(getAvgError()) < value;
     }
   }
 
@@ -153,6 +157,8 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
     instances++;
     HLUsageReporting.reportPIDController(instances);
     m_tolerance = new NullTolerance();
+
+    m_buf = new LinkedList<Double>();
   }
 
   /**
@@ -298,6 +304,14 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
         }
         pidOutput = m_pidOutput;
         result = m_result;
+
+        // Update the buffer.
+        m_buf.push(m_error);
+        m_bufTotal += m_error;
+        // Remove old elements when the buffer is full.
+        if (m_buf.size() > m_bufLength) {
+          m_bufTotal -= m_buf.pop();
+        }
       }
 
       pidOutput.pidWrite(result);
@@ -445,6 +459,7 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
 
   /**
    * Set the setpoint for the PIDController
+   * Clears the queue for GetAvgError().
    *$
    * @param setpoint the desired setpoint
    */
@@ -461,6 +476,8 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
       m_setpoint = setpoint;
     }
 
+    m_buf.clear();
+
     if (table != null)
       table.putNumber("setpoint", m_setpoint);
   }
@@ -502,6 +519,21 @@ 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;
+  }
+
   /**
    * Set the percentage error which is considered tolerable for use with
    * OnTarget. (Input of 15.0 = 15 percent)
@@ -549,6 +581,24 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
     m_tolerance = new PercentageTolerance(percentage);
   }
 
+  /**
+   * Set the number of previous error samples to average for tolerancing. When
+   * determining whether a mechanism is on target, the user may want to use a
+   * rolling average of previous measurements instead of a precise position or
+   * velocity. This is useful for noisy sensors which return a few erroneous
+   * measurements when the mechanism is on target. However, the mechanism will
+   * not register as on target for at least the specified bufLength cycles.
+   * @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.pop();
+    }
+  }
+
   /**
    * Return true if the error is within the percentage of the total input range,
    * determined by setTolerance. This assumes that the maximum and minimum input

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

@@ -188,17 +188,19 @@ public class MotorEncoderTest extends AbstractComsSetup {
   @Test
   public void testVelocityPIDController() {
     me.getEncoder().setPIDSourceType(PIDSourceType.kRate);
-    PIDController pid = new PIDController(0.002, 0, 0.0001, me.getEncoder(), me.getMotor());
-    pid.setAbsoluteTolerance(50);
-    pid.setOutputRange(-0.2, 0.2);
-    pid.setSetpoint(30);
+    PIDController pid =
+        new PIDController(1e-5, 0.0, 3e-5, 8e-5, me.getEncoder(), me.getMotor());
+    pid.setAbsoluteTolerance(200);
+    pid.setToleranceBuffer(50);
+    pid.setOutputRange(-0.3, 0.3);
+    pid.setSetpoint(2000);
 
     pid.enable();
     Timer.delay(10.0);
     pid.disable();
 
     assertTrue(
-        "PID loop did not reach setpoint within 10 seconds. The error was: " + pid.getError(),
+        "PID loop did not reach setpoint within 10 seconds. The error was: " + pid.getAvgError(),
         pid.onTarget());
 
     pid.free();