diff --git a/wpilibc/src/main/native/include/frc/controller/ControllerUtil.h b/wpilibc/src/main/native/include/frc/controller/ControllerUtil.h
index 32dced6fb9..7603fde41c 100644
--- a/wpilibc/src/main/native/include/frc/controller/ControllerUtil.h
+++ b/wpilibc/src/main/native/include/frc/controller/ControllerUtil.h
@@ -18,12 +18,6 @@ namespace frc {
  * Returns modulus of error where error is the difference between the reference
  * and a measurement.
  *
- * This implements modular subtraction defined as:
- *
- * e = (r mod m - x mod m) mod m
- *
- * with an offset in the modulus range for minimum input.
- *
  * @param reference Reference input of a controller.
  * @param measurement The current measurement.
  * @param minimumInput The minimum value expected from the input.
@@ -31,28 +25,18 @@ namespace frc {
  */
 template <typename T>
 T GetModulusError(T reference, T measurement, T minimumInput, T maximumInput) {
+  T error = reference - measurement;
   T modulus = maximumInput - minimumInput;
 
-  if constexpr (std::is_same_v<T, double>) {
-    T error = std::fmod(reference, modulus) - std::fmod(measurement, modulus);
+  // Wrap error above maximum input
+  int numMax = (error + maximumInput) / modulus;
+  error -= numMax * modulus;
 
-    // Moduli on the difference arguments establish a precondition for the
-    // following modulus.
-    return std::fmod(error - minimumInput, modulus) + minimumInput;
-  } else if constexpr (std::is_same_v<T, int>) {
-    T error = reference % modulus - measurement % modulus;
+  // Wrap error below minimum input
+  int numMin = (error + minimumInput) / modulus;
+  error -= numMin * modulus;
 
-    // Moduli on the difference arguments establish a precondition for the
-    // following modulus.
-    return (error - minimumInput) % modulus + minimumInput;
-  } else {
-    T error = units::math::fmod(reference, modulus) -
-              units::math::fmod(measurement, modulus);
-
-    // Moduli on the difference arguments establish a precondition for the
-    // following modulus.
-    return units::math::fmod(error - minimumInput, modulus) + minimumInput;
-  }
+  return error;
 }
 
 }  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h b/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h
index 3f97d18bd6..fd246cd87a 100644
--- a/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h
+++ b/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h
@@ -196,6 +196,8 @@ class ProfiledPIDController
   void EnableContinuousInput(Distance_t minimumInput, Distance_t maximumInput) {
     m_controller.EnableContinuousInput(minimumInput.template to<double>(),
                                        maximumInput.template to<double>());
+    m_minimumInput = minimumInput;
+    m_maximumInput = maximumInput;
   }
 
   /**
@@ -254,8 +256,10 @@ class ProfiledPIDController
   double Calculate(Distance_t measurement) {
     if (m_controller.IsContinuousInputEnabled()) {
       // Get error which is smallest distance between goal and measurement
-      auto error = frc::GetModulusError<Distance_t>(
+      auto goalMinDistance = frc::GetModulusError<Distance_t>(
           m_goal.position, measurement, m_minimumInput, m_maximumInput);
+      auto setpointMinDistance = frc::GetModulusError<Distance_t>(
+          m_setpoint.position, measurement, m_minimumInput, m_maximumInput);
 
       // Recompute the profile goal with the smallest error, thus giving the
       // shortest path. The goal may be outside the input range after this
@@ -263,7 +267,8 @@ class ProfiledPIDController
       // report an error of zero. In other words, the setpoint only needs to be
       // offset from the measurement by the input range modulus; they don't need
       // to be equal.
-      m_goal.position = Distance_t{error} + measurement;
+      m_goal.position = goalMinDistance + measurement;
+      m_setpoint.position = setpointMinDistance + measurement;
     }
 
     frc::TrapezoidProfile<Distance> profile{m_constraints, m_goal, m_setpoint};
diff --git a/wpilibc/src/test/native/cpp/controller/ControllerUtilTest.cpp b/wpilibc/src/test/native/cpp/controller/ControllerUtilTest.cpp
index 9e34af8f6b..00259d1ad7 100644
--- a/wpilibc/src/test/native/cpp/controller/ControllerUtilTest.cpp
+++ b/wpilibc/src/test/native/cpp/controller/ControllerUtilTest.cpp
@@ -17,6 +17,11 @@ TEST(ControllerUtilTest, GetModulusError) {
                   frc::GetModulusError(170.0 + 360.0, -170.0, -180.0, 180.0));
   EXPECT_FLOAT_EQ(-20.0,
                   frc::GetModulusError(170.0, -170.0 + 360.0, -180.0, 180.0));
+  EXPECT_FLOAT_EQ(20.0, frc::GetModulusError(-170.0, 170.0, -180.0, 180.0));
+  EXPECT_FLOAT_EQ(20.0,
+                  frc::GetModulusError(-170.0 + 360.0, 170.0, -180.0, 180.0));
+  EXPECT_FLOAT_EQ(20.0,
+                  frc::GetModulusError(-170.0, 170.0 + 360.0, -180.0, 180.0));
 
   // Test range starting at zero
   EXPECT_FLOAT_EQ(-20.0, frc::GetModulusError(170.0, 190.0, 0.0, 360.0));
diff --git a/wpilibc/src/test/native/cpp/controller/PIDInputOutputTest.cpp b/wpilibc/src/test/native/cpp/controller/PIDInputOutputTest.cpp
index c68cb370e2..3580308e7b 100644
--- a/wpilibc/src/test/native/cpp/controller/PIDInputOutputTest.cpp
+++ b/wpilibc/src/test/native/cpp/controller/PIDInputOutputTest.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.                                                               */
@@ -21,7 +21,7 @@ TEST_F(PIDInputOutputTest, ContinuousInputTest) {
   controller->SetP(1);
   controller->EnableContinuousInput(-180, 180);
 
-  EXPECT_TRUE(controller->Calculate(-179, 179) < 0);
+  EXPECT_LT(controller->Calculate(-179, 179), 0);
 }
 
 TEST_F(PIDInputOutputTest, ProportionalGainOutputTest) {
diff --git a/wpilibc/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp b/wpilibc/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp
new file mode 100644
index 0000000000..6f9c36d022
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp
@@ -0,0 +1,111 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <units/angle.h>
+#include <units/angular_acceleration.h>
+#include <units/angular_velocity.h>
+#include <wpi/math>
+
+#include "frc/controller/ProfiledPIDController.h"
+#include "gtest/gtest.h"
+
+class ProfiledPIDInputOutputTest : public testing::Test {
+ protected:
+  frc::ProfiledPIDController<units::degrees>* controller;
+
+  void SetUp() override {
+    controller = new frc::ProfiledPIDController<units::degrees>(
+        0, 0, 0, {360_deg_per_s, 180_deg_per_s_sq});
+  }
+
+  void TearDown() override { delete controller; }
+};
+
+TEST_F(ProfiledPIDInputOutputTest, ContinuousInputTest) {
+  controller->SetP(1);
+  controller->EnableContinuousInput(-180_deg, 180_deg);
+
+  controller->Reset(-179_deg);
+  EXPECT_LT(controller->Calculate(-179_deg, 179_deg), 0);
+}
+
+TEST_F(ProfiledPIDInputOutputTest, ContinuousInputTest1) {
+  controller->SetP(1);
+  controller->EnableContinuousInput(-180_deg, 180_deg);
+
+  static constexpr units::degree_t kSetpoint{-179.0};
+  static constexpr units::degree_t kMeasurement{-179.0};
+  static constexpr units::degree_t kGoal{179.0};
+
+  controller->Reset(kSetpoint);
+  EXPECT_LT(controller->Calculate(kMeasurement, kGoal), 0.0);
+
+  // Error must be less than half the input range at all times
+  EXPECT_LT(units::math::abs(controller->GetSetpoint().position - kMeasurement),
+            180_deg);
+}
+
+TEST_F(ProfiledPIDInputOutputTest, ContinuousInputTest2) {
+  controller->SetP(1);
+  controller->EnableContinuousInput(-units::radian_t{wpi::math::pi},
+                                    units::radian_t{wpi::math::pi});
+
+  static constexpr units::radian_t kSetpoint{-3.4826633343199735};
+  static constexpr units::radian_t kMeasurement{-3.1352207333939606};
+  static constexpr units::radian_t kGoal{-3.534162788601621};
+
+  controller->Reset(kSetpoint);
+  EXPECT_LT(controller->Calculate(kMeasurement, kGoal), 0.0);
+
+  // Error must be less than half the input range at all times
+  EXPECT_LT(units::math::abs(controller->GetSetpoint().position - kMeasurement),
+            units::radian_t{wpi::math::pi});
+}
+
+TEST_F(ProfiledPIDInputOutputTest, ContinuousInputTest3) {
+  controller->SetP(1);
+  controller->EnableContinuousInput(-units::radian_t{wpi::math::pi},
+                                    units::radian_t{wpi::math::pi});
+
+  static constexpr units::radian_t kSetpoint{-3.5176604690006377};
+  static constexpr units::radian_t kMeasurement{3.1191729343822456};
+  static constexpr units::radian_t kGoal{2.709680418117445};
+
+  controller->Reset(kSetpoint);
+  EXPECT_LT(controller->Calculate(kMeasurement, kGoal), 0.0);
+
+  // Error must be less than half the input range at all times
+  EXPECT_LT(units::math::abs(controller->GetSetpoint().position - kMeasurement),
+            units::radian_t{wpi::math::pi});
+}
+
+TEST_F(ProfiledPIDInputOutputTest, ProportionalGainOutputTest) {
+  controller->SetP(4);
+
+  EXPECT_DOUBLE_EQ(-0.1, controller->Calculate(0.025_deg, 0_deg));
+}
+
+TEST_F(ProfiledPIDInputOutputTest, IntegralGainOutputTest) {
+  controller->SetI(4);
+
+  double out = 0;
+
+  for (int i = 0; i < 5; i++) {
+    out = controller->Calculate(0.025_deg, 0_deg);
+  }
+
+  EXPECT_DOUBLE_EQ(-0.5 * controller->GetPeriod().to<double>(), out);
+}
+
+TEST_F(ProfiledPIDInputOutputTest, DerivativeGainOutputTest) {
+  controller->SetD(4);
+
+  controller->Calculate(0_deg, 0_deg);
+
+  EXPECT_DOUBLE_EQ(-10_ms / controller->GetPeriod(),
+                   controller->Calculate(0.0025_deg, 0_deg));
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ControllerUtil.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ControllerUtil.java
index f1e8716e4b..0896ac6d40 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ControllerUtil.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ControllerUtil.java
@@ -12,12 +12,6 @@ public final class ControllerUtil {
    * Returns modulus of error where error is the difference between the reference
    * and a measurement.
    *
-   * <p>This implements modular subtraction defined as:
-   *
-   * <p>e = (r mod m - x mod m) mod m
-   *
-   * <p>with an offset in the modulus range for minimum input.
-   *
    * @param reference Reference input of a controller.
    * @param measurement The current measurement.
    * @param minimumInput The minimum value expected from the input.
@@ -25,12 +19,18 @@ public final class ControllerUtil {
    */
   public static double getModulusError(double reference, double measurement, double minimumInput,
       double maximumInput) {
+    double error = reference - measurement;
     double modulus = maximumInput - minimumInput;
-    double error = reference % modulus - measurement % modulus;
 
-    // Moduli on the difference arguments establish a precondition for the
-    // following modulus.
-    return (error - minimumInput) % modulus + minimumInput;
+    // Wrap error above maximum input
+    int numMax = (int) ((error + maximumInput) / modulus);
+    error -= numMax * modulus;
+
+    // Wrap error below minimum input
+    int numMin = (int) ((error + minimumInput) / modulus);
+    error -= numMin * modulus;
+
+    return error;
   }
 
   private ControllerUtil() {
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ProfiledPIDController.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ProfiledPIDController.java
index 5cbce5dd06..ec7c728233 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ProfiledPIDController.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ProfiledPIDController.java
@@ -218,6 +218,8 @@ public class ProfiledPIDController implements Sendable {
    */
   public void enableContinuousInput(double minimumInput, double maximumInput) {
     m_controller.enableContinuousInput(minimumInput, maximumInput);
+    m_minimumInput = minimumInput;
+    m_maximumInput = maximumInput;
   }
 
   /**
@@ -283,14 +285,17 @@ public class ProfiledPIDController implements Sendable {
   public double calculate(double measurement) {
     if (m_controller.isContinuousInputEnabled()) {
       // Get error which is smallest distance between goal and measurement
-      double error = ControllerUtil.getModulusError(m_goal.position, measurement, m_minimumInput,
-          m_maximumInput);
+      double goalMinDistance = ControllerUtil.getModulusError(m_goal.position, measurement,
+          m_minimumInput, m_maximumInput);
+      double setpointMinDistance = ControllerUtil.getModulusError(m_setpoint.position, measurement,
+          m_minimumInput, m_maximumInput);
 
       // Recompute the profile goal with the smallest error, thus giving the shortest path. The goal
       // may be outside the input range after this operation, but that's OK because the controller
       // will still go there and report an error of zero. In other words, the setpoint only needs to
       // be offset from the measurement by the input range modulus; they don't need to be equal.
-      m_goal.position = error + measurement;
+      m_goal.position = goalMinDistance + measurement;
+      m_setpoint.position = setpointMinDistance + measurement;
     }
 
     var profile = new TrapezoidProfile(m_constraints, m_goal, m_setpoint);
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/ControllerUtilTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/ControllerUtilTest.java
index 58e1223d84..307eccd166 100644
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/ControllerUtilTest.java
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/ControllerUtilTest.java
@@ -18,6 +18,9 @@ class ControllerUtilTest {
     assertEquals(-20.0, ControllerUtil.getModulusError(170.0, -170.0, -180.0, 180.0));
     assertEquals(-20.0, ControllerUtil.getModulusError(170.0 + 360.0, -170.0, -180.0, 180.0));
     assertEquals(-20.0, ControllerUtil.getModulusError(170.0, -170.0 + 360.0, -180.0, 180.0));
+    assertEquals(20.0, ControllerUtil.getModulusError(-170.0, 170.0, -180.0, 180.0));
+    assertEquals(20.0, ControllerUtil.getModulusError(-170.0 + 360.0, 170.0, -180.0, 180.0));
+    assertEquals(20.0, ControllerUtil.getModulusError(-170.0, 170.0 + 360.0, -180.0, 180.0));
 
     // Test range start at zero
     assertEquals(-20.0, ControllerUtil.getModulusError(170.0, 190.0, 0.0, 360.0));
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/ProfiledPIDInputOutputTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/ProfiledPIDInputOutputTest.java
new file mode 100644
index 0000000000..9fca2a4b48
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/ProfiledPIDInputOutputTest.java
@@ -0,0 +1,103 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.controller;
+
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class ProfiledPIDInputOutputTest {
+  private ProfiledPIDController m_controller;
+
+  @BeforeEach
+  void setUp() {
+    m_controller = new ProfiledPIDController(0, 0, 0,
+        new TrapezoidProfile.Constraints(360, 180));
+  }
+
+  @Test
+  void continuousInputTest1() {
+    m_controller.setP(1);
+    m_controller.enableContinuousInput(-180, 180);
+
+    final double kSetpoint = -179.0;
+    final double kMeasurement = -179.0;
+    final double kGoal = 179.0;
+
+    m_controller.reset(kSetpoint);
+    assertTrue(m_controller.calculate(kMeasurement, kGoal) < 0.0);
+
+    // Error must be less than half the input range at all times
+    assertTrue(Math.abs(m_controller.getSetpoint().position - kMeasurement) < 180.0);
+  }
+
+  @Test
+  void continuousInputTest2() {
+    m_controller.setP(1);
+    m_controller.enableContinuousInput(-Math.PI, Math.PI);
+
+    final double kSetpoint = -3.4826633343199735;
+    final double kMeasurement = -3.1352207333939606;
+    final double kGoal = -3.534162788601621;
+
+    m_controller.reset(kSetpoint);
+    assertTrue(m_controller.calculate(kMeasurement, kGoal) < 0.0);
+
+    // Error must be less than half the input range at all times
+    assertTrue(Math.abs(m_controller.getSetpoint().position - kMeasurement) < Math.PI);
+  }
+
+  @Test
+  void continuousInputTest3() {
+    m_controller.setP(1);
+    m_controller.enableContinuousInput(-Math.PI, Math.PI);
+
+    final double kSetpoint = -3.5176604690006377;
+    final double kMeasurement = 3.1191729343822456;
+    final double kGoal = 2.709680418117445;
+
+    m_controller.reset(kSetpoint);
+    assertTrue(m_controller.calculate(kMeasurement, kGoal) < 0.0);
+
+    // Error must be less than half the input range at all times
+    assertTrue(Math.abs(m_controller.getSetpoint().position - kMeasurement) < Math.PI);
+  }
+
+  @Test
+  void proportionalGainOutputTest() {
+    m_controller.setP(4);
+
+    assertEquals(-0.1, m_controller.calculate(0.025, 0), 1e-5);
+  }
+
+  @Test
+  void integralGainOutputTest() {
+    m_controller.setI(4);
+
+    double out = 0;
+
+    for (int i = 0; i < 5; i++) {
+      out = m_controller.calculate(0.025, 0);
+    }
+
+    assertEquals(-0.5 * m_controller.getPeriod(), out, 1e-5);
+  }
+
+  @Test
+  void derivativeGainOutputTest() {
+    m_controller.setD(4);
+
+    m_controller.calculate(0, 0);
+
+    assertEquals(-0.01 / m_controller.getPeriod(), m_controller.calculate(0.0025, 0), 1e-5);
+  }
+}