Fix onTarget() so that it returns false until there are any values re…

…trieved

The onTarget() method uses the average error and there might not be a valid value
until the PIDController runs once. This is causing commands to bail out early in
simulation.

Change-Id: I7bac9ecb847cbe4f378385b1a6998bba10147554

wpilibc/Athena/src/PIDController.cpp

@@ -486,8 +486,10 @@ void PIDController::SetToleranceBuffer(unsigned bufLength) {
  * setpoint.
  * Ideally it should be based on being within the tolerance for some period of
  * time.
+ * This will return false until at least one input value has been computed.
  */
 bool PIDController::OnTarget() const {
+  if (m_buf.size() == 0) return false;
   double error = GetAvgError();
 
   std::lock_guard<priority_recursive_mutex> sync(m_mutex);

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

@@ -86,7 +86,7 @@ public boolean onTarget() {
 
     @Override
     public boolean onTarget() {
-      return (Math.abs(getAvgError()) < percentage / 100 * (m_maximumInput - m_minimumInput));
+      return isAvgErrorValid() && (Math.abs(getAvgError()) < percentage / 100 * (m_maximumInput - m_minimumInput));
     }
   }
 
@@ -99,7 +99,7 @@ public boolean onTarget() {
 
     @Override
     public boolean onTarget() {
-      return Math.abs(getAvgError()) < value;
+      return isAvgErrorValid() && Math.abs(getAvgError()) < value;
     }
   }
 
@@ -576,6 +576,16 @@ public synchronized double getAvgError() {
   }
 
   /**
+   * 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 percentage error which is considered tolerable for use with
    * OnTarget. (Input of 15.0 = 15 percent)
    *$