diff --git a/wpilibc/src/main/native/cpp/controller/PIDController.cpp b/wpilibc/src/main/native/cpp/controller/PIDController.cpp
index 7b9b62317d..0f61ea8074 100644
--- a/wpilibc/src/main/native/cpp/controller/PIDController.cpp
+++ b/wpilibc/src/main/native/cpp/controller/PIDController.cpp
@@ -54,8 +54,18 @@ void PIDController::SetSetpoint(double setpoint) { m_setpoint = setpoint; }
 double PIDController::GetSetpoint() const { return m_setpoint; }
 
 bool PIDController::AtSetpoint() const {
-  return std::abs(m_positionError) < m_positionTolerance &&
-         std::abs(m_velocityError) < m_velocityTolerance;
+  double positionError;
+  if (m_continuous) {
+    positionError = frc::GetModulusError<double>(
+        m_setpoint, m_measurement, m_minimumInput, m_maximumInput);
+  } else {
+    positionError = m_setpoint - m_measurement;
+  }
+
+  double velocityError = (positionError - m_prevError) / m_period.to<double>();
+
+  return std::abs(positionError) < m_positionTolerance &&
+         std::abs(velocityError) < m_velocityTolerance;
 }
 
 void PIDController::EnableContinuousInput(double minimumInput,
@@ -86,6 +96,7 @@ double PIDController::GetPositionError() const { return m_positionError; }
 double PIDController::GetVelocityError() const { return m_velocityError; }
 
 double PIDController::Calculate(double measurement) {
+  m_measurement = measurement;
   m_prevError = m_positionError;
 
   if (m_continuous) {
diff --git a/wpilibc/src/main/native/include/frc/controller/PIDController.h b/wpilibc/src/main/native/include/frc/controller/PIDController.h
index 5f97c1e80e..03441c1c5a 100644
--- a/wpilibc/src/main/native/include/frc/controller/PIDController.h
+++ b/wpilibc/src/main/native/include/frc/controller/PIDController.h
@@ -238,6 +238,7 @@ class PIDController : public frc::Sendable,
   double m_velocityTolerance = std::numeric_limits<double>::infinity();
 
   double m_setpoint = 0;
+  double m_measurement = 0;
 };
 
 }  // namespace frc2
diff --git a/wpilibc/src/test/native/cpp/controller/PIDToleranceTest.cpp b/wpilibc/src/test/native/cpp/controller/PIDToleranceTest.cpp
index 3251098901..efb32a5fe5 100644
--- a/wpilibc/src/test/native/cpp/controller/PIDToleranceTest.cpp
+++ b/wpilibc/src/test/native/cpp/controller/PIDToleranceTest.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2014-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -23,9 +23,17 @@ TEST(PIDToleranceTest, AbsoluteTolerance) {
   frc2::PIDController controller{0.5, 0.0, 0.0};
   controller.EnableContinuousInput(-kRange / 2, kRange / 2);
 
+  EXPECT_TRUE(controller.AtSetpoint())
+      << "Error was not in tolerance when it should have been. Error was "
+      << controller.GetPositionError();
+
   controller.SetTolerance(kTolerance);
   controller.SetSetpoint(kSetpoint);
 
+  EXPECT_FALSE(controller.AtSetpoint())
+      << "Error was in tolerance when it should not have been. Error was "
+      << controller.GetPositionError();
+
   controller.Calculate(0.0);
 
   EXPECT_FALSE(controller.AtSetpoint())
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java
index 526e91efad..ea9dbce7f1 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java
@@ -62,6 +62,7 @@ public class PIDController implements Sendable, AutoCloseable {
   private double m_velocityTolerance = Double.POSITIVE_INFINITY;
 
   private double m_setpoint;
+  private double m_measurement;
 
   /**
    * Allocates a PIDController with the given constants for Kp, Ki, and Kd and a default period of
@@ -211,8 +212,18 @@ public class PIDController implements Sendable, AutoCloseable {
    * @return Whether the error is within the acceptable bounds.
    */
   public boolean atSetpoint() {
-    return Math.abs(m_positionError) < m_positionTolerance
-        && Math.abs(m_velocityError) < m_velocityTolerance;
+    double positionError;
+    if (m_continuous) {
+      positionError = ControllerUtil.getModulusError(m_setpoint, m_measurement, m_minimumInput,
+          m_maximumInput);
+    } else {
+      positionError = m_setpoint - m_measurement;
+    }
+
+    double velocityError = (positionError - m_prevError) / m_period;
+
+    return Math.abs(positionError) < m_positionTolerance
+        && Math.abs(velocityError) < m_velocityTolerance;
   }
 
   /**
@@ -313,6 +324,7 @@ public class PIDController implements Sendable, AutoCloseable {
    * @param measurement The current measurement of the process variable.
    */
   public double calculate(double measurement) {
+    m_measurement = measurement;
     m_prevError = m_positionError;
 
     if (m_continuous) {
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/PIDToleranceTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/PIDToleranceTest.java
index b2d43c634c..c486cbd97d 100644
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/PIDToleranceTest.java
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/PIDToleranceTest.java
@@ -31,9 +31,17 @@ class PIDToleranceTest {
     try (var controller = new PIDController(0.05, 0.0, 0.0)) {
       controller.enableContinuousInput(-kRange / 2, kRange / 2);
 
+      assertTrue(controller.atSetpoint(),
+          "Error was not in tolerance when it should have been. Error was "
+          + controller.getPositionError());
+
       controller.setTolerance(kTolerance);
       controller.setSetpoint(kSetpoint);
 
+      assertFalse(controller.atSetpoint(),
+          "Error was in tolerance when it should not have been. Error was "
+          + controller.getPositionError());
+
       controller.calculate(0.0);
 
       assertFalse(controller.atSetpoint(),