diff --git a/wpilibc/athena/src/PIDController.cpp b/wpilibc/athena/src/PIDController.cpp
index 20412df1d4..24b3e25fc6 100644
--- a/wpilibc/athena/src/PIDController.cpp
+++ b/wpilibc/athena/src/PIDController.cpp
@@ -110,16 +110,7 @@ void PIDController::Calculate() {
     float result;
     PIDOutput* pidOutput;
 
-    m_error = m_setpoint - input;
-    if (m_continuous) {
-      if (std::fabs(m_error) > (m_maximumInput - m_minimumInput) / 2) {
-        if (m_error > 0) {
-          m_error = m_error - m_maximumInput + m_minimumInput;
-        } else {
-          m_error = m_error + m_maximumInput - m_minimumInput;
-        }
-      }
-    }
+    m_error = GetContinuousError(m_setpoint - input);
 
     if (m_pidInput->GetPIDSourceType() == PIDSourceType::kRate) {
       if (m_P != 0) {
@@ -403,12 +394,11 @@ double PIDController::GetDeltaSetpoint() const {
  * @return the current error
  */
 float PIDController::GetError() const {
-  double pidInput;
+  double setpoint = GetSetpoint();
   {
     std::lock_guard<priority_recursive_mutex> sync(m_mutex);
-    pidInput = m_pidInput->PIDGet();
+    return GetContinuousError(setpoint - m_pidInput->PIDGet());
   }
-  return GetSetpoint() - pidInput;
 }
 
 /**
@@ -598,6 +588,27 @@ void PIDController::InitTable(std::shared_ptr<ITable> subtable) {
   }
 }
 
+/**
+ * Wraps error around for continuous inputs. The original error is returned if
+ * continuous mode is disabled. This is an unsynchronized function.
+ *
+ * @param error The current error of the PID controller.
+ * @return Error for continuous inputs.
+ */
+double PIDController::GetContinuousError(double error) const {
+  if (m_continuous) {
+    if (std::fabs(error) > (m_maximumInput - m_minimumInput) / 2) {
+      if (error > 0) {
+        return error - (m_maximumInput - m_minimumInput);
+      } else {
+        return error + (m_maximumInput - m_minimumInput);
+      }
+    }
+  }
+
+  return error;
+}
+
 std::shared_ptr<ITable> PIDController::GetTable() const { return m_table; }
 
 void PIDController::ValueChanged(ITable* source, llvm::StringRef key,
diff --git a/wpilibc/shared/include/PIDController.h b/wpilibc/shared/include/PIDController.h
index bd54009d24..2add6d2b3d 100644
--- a/wpilibc/shared/include/PIDController.h
+++ b/wpilibc/shared/include/PIDController.h
@@ -88,6 +88,7 @@ class PIDController : public LiveWindowSendable,
   std::shared_ptr<ITable> m_table;
   virtual void Calculate();
   virtual double CalculateFeedForward();
+  double GetContinuousError(double error) const;
 
  private:
   // factor for "proportional" control
diff --git a/wpilibc/sim/src/PIDController.cpp b/wpilibc/sim/src/PIDController.cpp
index 59764a6267..f8596ced9b 100644
--- a/wpilibc/sim/src/PIDController.cpp
+++ b/wpilibc/sim/src/PIDController.cpp
@@ -126,9 +126,9 @@ void PIDController::Calculate() {
       if (m_continuous) {
         if (std::fabs(m_error) > (m_maximumInput - m_minimumInput) / 2) {
           if (m_error > 0) {
-            m_error = m_error - m_maximumInput + m_minimumInput;
+            m_error = m_error - (m_maximumInput - m_minimumInput);
           } else {
-            m_error = m_error + m_maximumInput - m_minimumInput;
+            m_error = m_error + (m_maximumInput - m_minimumInput);
           }
         }
       }
@@ -403,12 +403,11 @@ double PIDController::GetDeltaSetpoint() const {
  * @return the current error
  */
 float PIDController::GetError() const {
-  double pidInput;
+  double setpoint = GetSetpoint();
   {
-    std::lock_guard<priority_recursive_mutex> lock(m_mutex);
-    pidInput = m_pidInput->PIDGet();
+    std::lock_guard<priority_recursive_mutex> sync(m_mutex);
+    return GetContinuousError(setpoint - m_pidInput->PIDGet());
   }
-  return GetSetpoint() - pidInput;
 }
 
 /**
@@ -596,6 +595,27 @@ void PIDController::InitTable(std::shared_ptr<ITable> table) {
   }
 }
 
+/**
+ * Wraps error around for continuous inputs. The original error is returned if
+ * continuous mode is disabled. This is an unsynchronized function.
+ *
+ * @param error The current error of the PID controller.
+ * @return Error for continuous inputs.
+ */
+double PIDController::GetContinuousError(double error) const {
+  if (m_continuous) {
+    if (std::fabs(error) > (m_maximumInput - m_minimumInput) / 2) {
+      if (error > 0) {
+        return error - (m_maximumInput - m_minimumInput);
+      } else {
+        return error + (m_maximumInput - m_minimumInput);
+      }
+    }
+  }
+
+  return error;
+}
+
 std::shared_ptr<ITable> PIDController::GetTable() const { return m_table; }
 
 void PIDController::ValueChanged(ITable* source, llvm::StringRef key,
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 b8ed44ef4a..3e9d0fa619 100644
--- a/wpilibj/src/shared/java/edu/wpi/first/wpilibj/PIDController.java
+++ b/wpilibj/src/shared/java/edu/wpi/first/wpilibj/PIDController.java
@@ -265,16 +265,7 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
         input = pidInput.pidGet();
       }
       synchronized (this) {
-        m_error = m_setpoint - input;
-        if (m_continuous) {
-          if (Math.abs(m_error) > (m_maximumInput - m_minimumInput) / 2) {
-            if (m_error > 0) {
-              m_error = m_error - m_maximumInput + m_minimumInput;
-            } else {
-              m_error = m_error + m_maximumInput - m_minimumInput;
-            }
-          }
-        }
+        m_error = getContinuousError(m_setpoint - input);
 
         if (m_pidInput.getPIDSourceType().equals(PIDSourceType.kRate)) {
           if (m_P != 0) {
@@ -546,8 +537,7 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
    * @return the current error
    */
   public synchronized double getError() {
-    // return m_error;
-    return getSetpoint() - m_pidInput.pidGet();
+    return getContinuousError(getSetpoint() - m_pidInput.pidGet());
   }
 
   /**
@@ -765,6 +755,26 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
     }
   }
 
+  /**
+   * Wraps error around for continuous inputs. The original error is returned if continuous mode is
+   * disabled. This is an unsynchronized function.
+   *
+   * @param error The current error of the PID controller.
+   * @return Error for continuous inputs.
+   */
+  protected double getContinuousError(double error) {
+    if (m_continuous) {
+      if (Math.abs(error) > (m_maximumInput - m_minimumInput) / 2) {
+        if (error > 0) {
+          return error - (m_maximumInput - m_minimumInput);
+        } else {
+          return error + (m_maximumInput - m_minimumInput);
+        }
+      }
+    }
+
+    return error;
+  }
 
   @Override
   public ITable getTable() {