From 776cb915bce0bff8eaae1979ac0dfaf44ad2b0dd Mon Sep 17 00:00:00 2001
From: Thad House <ThadHouse@users.noreply.github.com>
Date: Sat, 13 Aug 2016 23:49:31 -0700
Subject: [PATCH] Revert "PIDController queue now stores inputs instead of
 errors (#138)" (#205)

This reverts commit 7501ae65a1c7f8e31fd19589979793b99527348d.
---
 wpilibc/athena/src/PIDController.cpp             | 16 +++++++++-------
 .../edu/wpi/first/wpilibj/PIDController.java     | 15 +++++++--------
 2 files changed, 16 insertions(+), 15 deletions(-)

diff --git a/wpilibc/athena/src/PIDController.cpp b/wpilibc/athena/src/PIDController.cpp
index 0047c50c66..dcb6240959 100644
--- a/wpilibc/athena/src/PIDController.cpp
+++ b/wpilibc/athena/src/PIDController.cpp
@@ -148,8 +148,8 @@ void PIDController::Calculate() {
     pidOutput->PIDWrite(result);
 
     // Update the buffer.
-    m_buf.push(input);
-    m_bufTotal += input;
+    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();
@@ -332,6 +332,8 @@ 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) {
@@ -348,6 +350,10 @@ void PIDController::SetSetpoint(float setpoint) {
     } else {
       m_setpoint = setpoint;
     }
+
+    // Clear m_buf.
+    m_buf = std::queue<double>();
+    m_bufTotal = 0;
   }
 
   if (m_table != nullptr) {
@@ -416,7 +422,7 @@ float PIDController::GetAvgError() const {
   {
     std::lock_guard<priority_recursive_mutex> sync(m_mutex);
     // Don't divide by zero.
-    if (m_buf.size()) avgError = m_setpoint - m_bufTotal / m_buf.size();
+    if (m_buf.size()) avgError = m_bufTotal / m_buf.size();
   }
   return avgError;
 }
@@ -530,10 +536,6 @@ void PIDController::Disable() {
     std::lock_guard<priority_recursive_mutex> sync(m_mutex);
     m_pidOutput->PIDWrite(0);
     m_enabled = false;
-
-    // Clear buffer
-    m_buf = std::queue<double>();
-    m_bufTotal = 0;
   }
 
   if (m_table != nullptr) {
diff --git a/wpilibj/src/shared/java/edu/wpi/first/wpilibj/PIDController.java b/wpilibj/src/shared/java/edu/wpi/first/wpilibj/PIDController.java
index 15029581dd..3e9d0fa619 100644
--- a/wpilibj/src/shared/java/edu/wpi/first/wpilibj/PIDController.java
+++ b/wpilibj/src/shared/java/edu/wpi/first/wpilibj/PIDController.java
@@ -311,8 +311,8 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
         result = m_result;
 
         // Update the buffer.
-        m_buf.add(input);
-        m_bufTotal += input;
+        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();
@@ -488,7 +488,7 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
   }
 
   /**
-   * Set the setpoint for the PIDController.
+   * Set the setpoint for the PIDController Clears the queue for GetAvgError().
    *
    * @param setpoint the desired setpoint
    */
@@ -505,6 +505,9 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
       m_setpoint = setpoint;
     }
 
+    m_buf.clear();
+    m_bufTotal = 0;
+
     if (m_table != null) {
       m_table.putNumber("setpoint", m_setpoint);
     }
@@ -566,7 +569,7 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
     double avgError = 0;
     // Don't divide by zero.
     if (m_buf.size() != 0) {
-      avgError = m_setpoint - m_bufTotal / m_buf.size();
+      avgError = m_bufTotal / m_buf.size();
     }
     return avgError;
   }
@@ -673,10 +676,6 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
     m_pidOutput.pidWrite(0);
     m_enabled = false;
 
-    // Clear buffer
-    m_buf.clear();
-    m_bufTotal = 0;
-
     if (m_table != null) {
       m_table.putBoolean("enabled", false);
     }