diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java
index 12c4175570..f0420b7426 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java
@@ -181,6 +181,15 @@ public class PIDController implements Sendable, AutoCloseable {
    */
   public void setSetpoint(double setpoint) {
     m_setpoint = setpoint;
+
+    if (m_continuous) {
+      double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
+      m_positionError = MathUtil.inputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
+    } else {
+      m_positionError = m_setpoint - m_measurement;
+    }
+
+    m_velocityError = (m_positionError - m_prevError) / m_period;
   }
 
   /**
@@ -200,18 +209,8 @@ public class PIDController implements Sendable, AutoCloseable {
    * @return Whether the error is within the acceptable bounds.
    */
   public boolean atSetpoint() {
-    double positionError;
-    if (m_continuous) {
-      double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
-      positionError = MathUtil.inputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
-    } else {
-      positionError = m_setpoint - m_measurement;
-    }
-
-    double velocityError = (positionError - m_prevError) / m_period;
-
-    return Math.abs(positionError) < m_positionTolerance
-        && Math.abs(velocityError) < m_velocityTolerance;
+    return Math.abs(m_positionError) < m_positionTolerance
+        && Math.abs(m_velocityError) < m_velocityTolerance;
   }
 
   /**
@@ -303,8 +302,7 @@ public class PIDController implements Sendable, AutoCloseable {
    * @return The next controller output.
    */
   public double calculate(double measurement, double setpoint) {
-    // Set setpoint to provided value
-    setSetpoint(setpoint);
+    m_setpoint = setpoint;
     return calculate(measurement);
   }
 
@@ -322,7 +320,7 @@ public class PIDController implements Sendable, AutoCloseable {
       double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
       m_positionError = MathUtil.inputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
     } else {
-      m_positionError = m_setpoint - measurement;
+      m_positionError = m_setpoint - m_measurement;
     }
 
     m_velocityError = (m_positionError - m_prevError) / m_period;
diff --git a/wpimath/src/main/native/cpp/controller/PIDController.cpp b/wpimath/src/main/native/cpp/controller/PIDController.cpp
index 34af2aa97a..f5d4801fe6 100644
--- a/wpimath/src/main/native/cpp/controller/PIDController.cpp
+++ b/wpimath/src/main/native/cpp/controller/PIDController.cpp
@@ -70,6 +70,16 @@ units::second_t PIDController::GetPeriod() const {
 
 void PIDController::SetSetpoint(double setpoint) {
   m_setpoint = setpoint;
+
+  if (m_continuous) {
+    double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
+    m_positionError =
+        frc::InputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
+  } else {
+    m_positionError = m_setpoint - m_measurement;
+  }
+
+  m_velocityError = (m_positionError - m_prevError) / m_period.value();
 }
 
 double PIDController::GetSetpoint() const {
@@ -77,19 +87,8 @@ double PIDController::GetSetpoint() const {
 }
 
 bool PIDController::AtSetpoint() const {
-  double positionError;
-  if (m_continuous) {
-    double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
-    positionError =
-        frc::InputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
-  } else {
-    positionError = m_setpoint - m_measurement;
-  }
-
-  double velocityError = (positionError - m_prevError) / m_period.value();
-
-  return std::abs(positionError) < m_positionTolerance &&
-         std::abs(velocityError) < m_velocityTolerance;
+  return std::abs(m_positionError) < m_positionTolerance &&
+         std::abs(m_velocityError) < m_velocityTolerance;
 }
 
 void PIDController::EnableContinuousInput(double minimumInput,
@@ -136,7 +135,7 @@ double PIDController::Calculate(double measurement) {
     m_positionError =
         frc::InputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
   } else {
-    m_positionError = m_setpoint - measurement;
+    m_positionError = m_setpoint - m_measurement;
   }
 
   m_velocityError = (m_positionError - m_prevError) / m_period.value();
@@ -151,8 +150,7 @@ double PIDController::Calculate(double measurement) {
 }
 
 double PIDController::Calculate(double measurement, double setpoint) {
-  // Set setpoint to provided value
-  SetSetpoint(setpoint);
+  m_setpoint = setpoint;
   return Calculate(measurement);
 }