diff --git a/wpilibc/athena/src/PIDController.cpp b/wpilibc/athena/src/PIDController.cpp
index dcb6240959..0047c50c66 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(m_error);
-    m_bufTotal += m_error;
+    m_buf.push(input);
+    m_bufTotal += input;
     // Remove old elements when buffer is full.
     if (m_buf.size() > m_bufLength) {
       m_bufTotal -= m_buf.front();
@@ -332,8 +332,6 @@ 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) {
@@ -350,10 +348,6 @@ void PIDController::SetSetpoint(float setpoint) {
     } else {
       m_setpoint = setpoint;
     }
-
-    // Clear m_buf.
-    m_buf = std::queue<double>();
-    m_bufTotal = 0;
   }
 
   if (m_table != nullptr) {
@@ -422,7 +416,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_bufTotal / m_buf.size();
+    if (m_buf.size()) avgError = m_setpoint - m_bufTotal / m_buf.size();
   }
   return avgError;
 }
@@ -536,6 +530,10 @@ 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 3e9d0fa619..15029581dd 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(m_error);
-        m_bufTotal += m_error;
+        m_buf.add(input);
+        m_bufTotal += input;
         // 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 Clears the queue for GetAvgError().
+   * Set the setpoint for the PIDController.
    *
    * @param setpoint the desired setpoint
    */
@@ -505,9 +505,6 @@ 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);
     }
@@ -569,7 +566,7 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
     double avgError = 0;
     // Don't divide by zero.
     if (m_buf.size() != 0) {
-      avgError = m_bufTotal / m_buf.size();
+      avgError = m_setpoint - m_bufTotal / m_buf.size();
     }
     return avgError;
   }
@@ -676,6 +673,10 @@ 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);
     }