From ea9512977c433a119e56a4c8ea3b44b6d869cf7f Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Sun, 7 Jul 2019 15:37:13 -0700
Subject: [PATCH] Add replacement PIDController class (#1300)

Originally, PIDController used PIDSource with its "PIDSourceType" to
determine whether a class should return position or velocity to the
controller. However, the supported languages have changed a lot over 10
years and now support lambdas. Instead of using PIDSource and PIDOutput,
users can pass in doubles to the Calculate() function synchronously.
This makes the controller much more flexible for team's needs as they no
longer have to make a separate PIDSource-inheriting class just to
provide a custom input.

The built-in feedforward was removed. Since PIDController is synchronous
now, they can add their own feedforward on top of what Calculate()
returns.

To facilitate running the controller asynchronously, there is a
PIDControllerRunner class that handles that. By separating the loop from
the control law, PIDController can now be composed with others and be
used to control a drivetrain (a multiple input, multiple output system
that requires summing the results from two controllers) much easier.
Also, motion profiling can be used to set the reference over time.

All the classes related to the old PIDController are now deprecated. The
new classes are in an experimental namespace to avoid name conflicts.

While this is a large change, I think it is a necessary one for growth.
The old PIDController design was created in a time when languages only
supported OOP, and we have more tools at our disposal now to solve
problems. This more versatile implementation can be used in more places
like as a replacement for Pathfinder's "EncoderFollower" class.

There has been hesitation to add lambda support to WPILib for a while
now out of concerns for requiring teams to learn more features of C++ or
Java. In my opinion, this change makes PIDController easier to use, not
harder. The concept of a function is a building block of OOP and should
be learned before classes. The ability to store functions as first-class
objects and invoke them just like variables is rather natural.

Note that PID constants for the new controller will be different from
the old one. The original controller didn't take the discretization
period into account. To fix this, teams should just have to divide their
Ki gain by 0.05 and multiply their Kd gain by 0.05 where 0.05 is the
original default period.
---
 .../native/cpp/controller/PIDController.cpp   | 265 ++++++++
 .../cpp/controller/PIDControllerRunner.cpp    |  72 +++
 .../src/main/native/include/frc/Controller.h  |   5 +-
 .../main/native/include/frc/PIDController.h   |   4 +
 .../include/frc/controller/PIDController.h    | 307 ++++++++++
 .../frc/controller/PIDControllerRunner.h      |  73 +++
 .../test/native/cpp/PIDInputOutputTest.cpp    |  70 +++
 .../src/test}/native/cpp/PIDToleranceTest.cpp |  74 +--
 .../src/main/native/cpp/MotorEncoderTest.cpp  |  50 +-
 .../edu/wpi/first/wpilibj/Controller.java     |   5 +-
 .../edu/wpi/first/wpilibj/PIDController.java  |   5 +-
 .../java/edu/wpi/first/wpilibj/PIDOutput.java |   5 +-
 .../java/edu/wpi/first/wpilibj/PIDSource.java |   5 +-
 .../edu/wpi/first/wpilibj/PIDSourceType.java  |   3 +-
 .../wpilibj/controller/PIDController.java     | 570 ++++++++++++++++++
 .../controller/PIDControllerRunner.java       | 129 ++++
 .../wpi/first/wpilibj/PIDInputOutputTest.java |  87 +++
 .../wpi/first/wpilibj/PIDToleranceTest.java   |  95 ++-
 .../wpi/first/wpilibj/MotorEncoderTest.java   |  46 +-
 .../java/edu/wpi/first/wpilibj/PIDTest.java   |  64 +-
 20 files changed, 1773 insertions(+), 161 deletions(-)
 create mode 100644 wpilibc/src/main/native/cpp/controller/PIDController.cpp
 create mode 100644 wpilibc/src/main/native/cpp/controller/PIDControllerRunner.cpp
 create mode 100644 wpilibc/src/main/native/include/frc/controller/PIDController.h
 create mode 100644 wpilibc/src/main/native/include/frc/controller/PIDControllerRunner.h
 create mode 100644 wpilibc/src/test/native/cpp/PIDInputOutputTest.cpp
 rename {wpilibcIntegrationTests/src/main => wpilibc/src/test}/native/cpp/PIDToleranceTest.cpp (53%)
 create mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java
 create mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDControllerRunner.java
 create mode 100644 wpilibj/src/test/java/edu/wpi/first/wpilibj/PIDInputOutputTest.java

diff --git a/wpilibc/src/main/native/cpp/controller/PIDController.cpp b/wpilibc/src/main/native/cpp/controller/PIDController.cpp
new file mode 100644
index 0000000000..8bbbe96d47
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/controller/PIDController.cpp
@@ -0,0 +1,265 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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 "frc/controller/PIDController.h"
+
+#include <algorithm>
+#include <cmath>
+
+#include <hal/HAL.h>
+
+#include "frc/smartdashboard/SendableBuilder.h"
+
+using namespace frc2;
+
+PIDController::PIDController(double Kp, double Ki, double Kd, double period)
+    : frc::SendableBase(false), m_Kp(Kp), m_Ki(Ki), m_Kd(Kd), m_period(period) {
+  static int instances = 0;
+  instances++;
+  HAL_Report(HALUsageReporting::kResourceType_PIDController, instances);
+  SetName("PIDController", instances);
+}
+
+PIDController::PIDController(PIDController&& rhs)
+    : SendableBase(std::move(rhs)),
+      m_Kp(std::move(rhs.m_Kp)),
+      m_Ki(std::move(rhs.m_Ki)),
+      m_Kd(std::move(rhs.m_Kd)),
+      m_period(std::move(rhs.m_period)),
+      m_maximumOutput(std::move(rhs.m_maximumOutput)),
+      m_minimumOutput(std::move(rhs.m_minimumOutput)),
+      m_maximumInput(std::move(rhs.m_maximumInput)),
+      m_minimumInput(std::move(rhs.m_minimumInput)),
+      m_inputRange(std::move(rhs.m_inputRange)),
+      m_continuous(std::move(rhs.m_continuous)),
+      m_currError(std::move(rhs.m_currError)),
+      m_prevError(std::move(rhs.m_prevError)),
+      m_totalError(std::move(rhs.m_totalError)),
+      m_toleranceType(std::move(rhs.m_toleranceType)),
+      m_tolerance(std::move(rhs.m_tolerance)),
+      m_deltaTolerance(std::move(rhs.m_deltaTolerance)),
+      m_setpoint(std::move(rhs.m_setpoint)),
+      m_output(std::move(rhs.m_output)) {}
+
+PIDController& PIDController::operator=(PIDController&& rhs) {
+  std::scoped_lock lock(m_thisMutex, rhs.m_thisMutex);
+
+  SendableBase::operator=(std::move(rhs));
+
+  m_Kp = std::move(rhs.m_Kp);
+  m_Ki = std::move(rhs.m_Ki);
+  m_Kd = std::move(rhs.m_Kd);
+  m_period = std::move(rhs.m_period);
+  m_maximumOutput = std::move(rhs.m_maximumOutput);
+  m_minimumOutput = std::move(rhs.m_minimumOutput);
+  m_maximumInput = std::move(rhs.m_maximumInput);
+  m_minimumInput = std::move(rhs.m_minimumInput);
+  m_inputRange = std::move(rhs.m_inputRange);
+  m_continuous = std::move(rhs.m_continuous);
+  m_currError = std::move(rhs.m_currError);
+  m_prevError = std::move(rhs.m_prevError);
+  m_totalError = std::move(rhs.m_totalError);
+  m_toleranceType = std::move(rhs.m_toleranceType);
+  m_tolerance = std::move(rhs.m_tolerance);
+  m_deltaTolerance = std::move(rhs.m_deltaTolerance);
+  m_setpoint = std::move(rhs.m_setpoint);
+  m_output = std::move(rhs.m_output);
+
+  return *this;
+}
+
+void PIDController::SetP(double Kp) {
+  std::lock_guard<wpi::mutex> lock(m_thisMutex);
+  m_Kp = Kp;
+}
+
+void PIDController::SetI(double Ki) {
+  std::lock_guard<wpi::mutex> lock(m_thisMutex);
+  m_Ki = Ki;
+}
+
+void PIDController::SetD(double Kd) {
+  std::lock_guard<wpi::mutex> lock(m_thisMutex);
+  m_Kd = Kd;
+}
+
+double PIDController::GetP() const {
+  std::lock_guard<wpi::mutex> lock(m_thisMutex);
+  return m_Kp;
+}
+
+double PIDController::GetI() const {
+  std::lock_guard<wpi::mutex> lock(m_thisMutex);
+  return m_Ki;
+}
+
+double PIDController::GetD() const {
+  std::lock_guard<wpi::mutex> lock(m_thisMutex);
+  return m_Kd;
+}
+
+double PIDController::GetPeriod() const { return m_period; }
+
+double PIDController::GetOutput() const {
+  std::lock_guard<wpi::mutex> lock(m_thisMutex);
+  return m_output;
+}
+
+void PIDController::SetSetpoint(double setpoint) {
+  std::lock_guard<wpi::mutex> lock(m_thisMutex);
+
+  if (m_maximumInput > m_minimumInput) {
+    m_setpoint = std::clamp(setpoint, m_minimumInput, m_maximumInput);
+  } else {
+    m_setpoint = setpoint;
+  }
+}
+
+double PIDController::GetSetpoint() const {
+  std::lock_guard<wpi::mutex> lock(m_thisMutex);
+  return m_setpoint;
+}
+
+bool PIDController::AtSetpoint(double tolerance, double deltaTolerance,
+                               Tolerance toleranceType) const {
+  double deltaError = GetDeltaError();
+
+  std::lock_guard<wpi::mutex> lock(m_thisMutex);
+  if (toleranceType == Tolerance::kPercent) {
+    return std::abs(m_currError) < tolerance / 100 * m_inputRange &&
+           std::abs(deltaError) < deltaTolerance / 100 * m_inputRange;
+  } else {
+    return std::abs(m_currError) < tolerance &&
+           std::abs(deltaError) < deltaTolerance;
+  }
+}
+
+bool PIDController::AtSetpoint() const {
+  return AtSetpoint(m_tolerance, m_deltaTolerance, m_toleranceType);
+}
+
+void PIDController::SetContinuous(bool continuous) {
+  std::lock_guard<wpi::mutex> lock(m_thisMutex);
+  m_continuous = continuous;
+}
+
+void PIDController::SetInputRange(double minimumInput, double maximumInput) {
+  std::lock_guard<wpi::mutex> lock(m_thisMutex);
+  m_minimumInput = minimumInput;
+  m_maximumInput = maximumInput;
+  m_inputRange = maximumInput - minimumInput;
+
+  // Clamp setpoint to new input range
+  if (m_maximumInput > m_minimumInput) {
+    m_setpoint = std::clamp(m_setpoint, m_minimumInput, m_maximumInput);
+  }
+}
+
+void PIDController::SetOutputRange(double minimumOutput, double maximumOutput) {
+  std::lock_guard<wpi::mutex> lock(m_thisMutex);
+  m_minimumOutput = minimumOutput;
+  m_maximumOutput = maximumOutput;
+}
+
+void PIDController::SetAbsoluteTolerance(double tolerance,
+                                         double deltaTolerance) {
+  std::lock_guard<wpi::mutex> lock(m_thisMutex);
+  m_toleranceType = Tolerance::kAbsolute;
+  m_tolerance = tolerance;
+  m_deltaTolerance = deltaTolerance;
+}
+
+void PIDController::SetPercentTolerance(double tolerance,
+                                        double deltaTolerance) {
+  std::lock_guard<wpi::mutex> lock(m_thisMutex);
+  m_toleranceType = Tolerance::kPercent;
+  m_tolerance = tolerance;
+  m_deltaTolerance = deltaTolerance;
+}
+
+double PIDController::GetError() const {
+  std::lock_guard<wpi::mutex> lock(m_thisMutex);
+  return GetContinuousError(m_currError);
+}
+
+/**
+ * Returns the change in error per second of the PIDController.
+ *
+ * @return The change in error per second.
+ */
+double PIDController::GetDeltaError() const {
+  std::lock_guard<wpi::mutex> lock(m_thisMutex);
+  return (m_currError - m_prevError) / GetPeriod();
+}
+
+double PIDController::Calculate(double measurement) {
+  std::lock_guard<wpi::mutex> lock(m_thisMutex);
+  return CalculateUnsafe(measurement);
+}
+
+double PIDController::Calculate(double measurement, double setpoint) {
+  std::lock_guard<wpi::mutex> lock(m_thisMutex);
+
+  // Set setpoint to provided value
+  if (m_maximumInput > m_minimumInput) {
+    m_setpoint = std::clamp(setpoint, m_minimumInput, m_maximumInput);
+  } else {
+    m_setpoint = setpoint;
+  }
+
+  return CalculateUnsafe(measurement);
+}
+
+void PIDController::Reset() {
+  std::lock_guard<wpi::mutex> lock(m_thisMutex);
+  m_prevError = 0;
+  m_totalError = 0;
+  m_output = 0;
+}
+
+void PIDController::InitSendable(frc::SendableBuilder& builder) {
+  builder.SetSmartDashboardType("PIDController");
+  builder.AddDoubleProperty("p", [this] { return GetP(); },
+                            [this](double value) { SetP(value); });
+  builder.AddDoubleProperty("i", [this] { return GetI(); },
+                            [this](double value) { SetI(value); });
+  builder.AddDoubleProperty("d", [this] { return GetD(); },
+                            [this](double value) { SetD(value); });
+  builder.AddDoubleProperty("setpoint", [this] { return GetSetpoint(); },
+                            [this](double value) { SetSetpoint(value); });
+}
+
+double PIDController::GetContinuousError(double error) const {
+  if (m_continuous && m_inputRange > 0) {
+    error = std::fmod(error, m_inputRange);
+    if (std::abs(error) > m_inputRange / 2) {
+      if (error > 0) {
+        return error - m_inputRange;
+      } else {
+        return error + m_inputRange;
+      }
+    }
+  }
+
+  return error;
+}
+
+double PIDController::CalculateUnsafe(double measurement) {
+  m_prevError = m_currError;
+  m_currError = GetContinuousError(m_setpoint - measurement);
+
+  if (m_Ki != 0) {
+    m_totalError = std::clamp(m_totalError + m_currError * GetPeriod(),
+                              m_minimumOutput / m_Ki, m_maximumOutput / m_Ki);
+  }
+
+  m_output = std::clamp(m_Kp * m_currError + m_Ki * m_totalError +
+                            m_Kd * (m_currError - m_prevError) / GetPeriod(),
+                        m_minimumOutput, m_maximumOutput);
+
+  return m_output;
+}
diff --git a/wpilibc/src/main/native/cpp/controller/PIDControllerRunner.cpp b/wpilibc/src/main/native/cpp/controller/PIDControllerRunner.cpp
new file mode 100644
index 0000000000..5f7f78a9a4
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/controller/PIDControllerRunner.cpp
@@ -0,0 +1,72 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 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 "frc/controller/PIDControllerRunner.h"
+
+#include "frc/controller/PIDController.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+
+using namespace frc;
+
+PIDControllerRunner::PIDControllerRunner(
+    frc2::PIDController& controller,
+    std::function<double(void)> measurementSource,
+    std::function<void(double)> controllerOutput)
+    : m_controller(controller),
+      m_measurementSource(measurementSource),
+      m_controllerOutput(controllerOutput) {
+  m_notifier.StartPeriodic(m_controller.GetPeriod());
+}
+
+PIDControllerRunner::~PIDControllerRunner() { Disable(); }
+
+void PIDControllerRunner::Enable() {
+  std::lock_guard<wpi::mutex> lock(m_thisMutex);
+  m_enabled = true;
+}
+
+void PIDControllerRunner::Disable() {
+  // Ensures m_enabled modification and m_controllerOutput() call occur
+  // atomically
+  std::lock_guard<wpi::mutex> outputLock(m_outputMutex);
+  {
+    std::lock_guard<wpi::mutex> mainLock(m_thisMutex);
+    m_enabled = false;
+  }
+
+  m_controllerOutput(0.0);
+}
+
+bool PIDControllerRunner::IsEnabled() const {
+  std::lock_guard<wpi::mutex> lock(m_thisMutex);
+  return m_enabled;
+}
+
+void PIDControllerRunner::Run() {
+  // Ensures m_enabled check and m_controllerOutput() call occur atomically
+  std::lock_guard<wpi::mutex> outputLock(m_outputMutex);
+  std::unique_lock<wpi::mutex> mainLock(m_thisMutex);
+  if (m_enabled) {
+    // Don't block other PIDControllerRunner operations on output
+    mainLock.unlock();
+
+    m_controllerOutput(m_controller.Calculate(m_measurementSource()));
+  }
+}
+
+void PIDControllerRunner::InitSendable(frc::SendableBuilder& builder) {
+  m_controller.InitSendable(builder);
+  builder.SetSafeState([this]() { Disable(); });
+  builder.AddBooleanProperty("enabled", [this]() { return IsEnabled(); },
+                             [this](bool enabled) {
+                               if (enabled) {
+                                 Enable();
+                               } else {
+                                 Disable();
+                               }
+                             });
+}
diff --git a/wpilibc/src/main/native/include/frc/Controller.h b/wpilibc/src/main/native/include/frc/Controller.h
index b3277230ad..4124046dd0 100644
--- a/wpilibc/src/main/native/include/frc/Controller.h
+++ b/wpilibc/src/main/native/include/frc/Controller.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2019 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.                                                               */
@@ -7,6 +7,8 @@
 
 #pragma once
 
+#include <wpi/deprecated.h>
+
 namespace frc {
 
 /**
@@ -18,6 +20,7 @@ namespace frc {
  */
 class Controller {
  public:
+  WPI_DEPRECATED("None of the 2020 FRC controllers use this.")
   Controller() = default;
   virtual ~Controller() = default;
 
diff --git a/wpilibc/src/main/native/include/frc/PIDController.h b/wpilibc/src/main/native/include/frc/PIDController.h
index 027032d151..e640f45c30 100644
--- a/wpilibc/src/main/native/include/frc/PIDController.h
+++ b/wpilibc/src/main/native/include/frc/PIDController.h
@@ -48,6 +48,7 @@ class PIDController : public PIDBase, public Controller {
    *               particularly affects calculations of the integral and
    *               differental terms. The default is 0.05 (50ms).
    */
+  WPI_DEPRECATED("Use frc2::PIDController class instead.")
   PIDController(double p, double i, double d, PIDSource* source,
                 PIDOutput* output, double period = 0.05);
 
@@ -63,6 +64,7 @@ class PIDController : public PIDBase, public Controller {
    *               particularly affects calculations of the integral and
    *               differental terms. The default is 0.05 (50ms).
    */
+  WPI_DEPRECATED("Use frc2::PIDController class instead.")
   PIDController(double p, double i, double d, double f, PIDSource* source,
                 PIDOutput* output, double period = 0.05);
 
@@ -78,6 +80,7 @@ class PIDController : public PIDBase, public Controller {
    *               particularly affects calculations of the integral and
    *               differental terms. The default is 0.05 (50ms).
    */
+  WPI_DEPRECATED("Use frc2::PIDController class instead.")
   PIDController(double p, double i, double d, PIDSource& source,
                 PIDOutput& output, double period = 0.05);
 
@@ -93,6 +96,7 @@ class PIDController : public PIDBase, public Controller {
    *               particularly affects calculations of the integral and
    *               differental terms. The default is 0.05 (50ms).
    */
+  WPI_DEPRECATED("Use frc2::PIDController class instead.")
   PIDController(double p, double i, double d, double f, PIDSource& source,
                 PIDOutput& output, double period = 0.05);
 
diff --git a/wpilibc/src/main/native/include/frc/controller/PIDController.h b/wpilibc/src/main/native/include/frc/controller/PIDController.h
new file mode 100644
index 0000000000..57c992612c
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/controller/PIDController.h
@@ -0,0 +1,307 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <functional>
+#include <limits>
+
+#include <wpi/mutex.h>
+
+#include "frc/smartdashboard/SendableBase.h"
+
+namespace frc2 {
+
+/**
+ * Implements a PID control loop.
+ */
+class PIDController : public frc::SendableBase {
+ public:
+  enum class Tolerance { kAbsolute, kPercent };
+
+  /**
+   * Allocates a PIDController with the given constants for Kp, Ki, and Kd.
+   *
+   * @param Kp     The proportional coefficient.
+   * @param Ki     The integral coefficient.
+   * @param Kd     The derivative coefficient.
+   * @param period The period between controller updates in seconds. The
+   *               default is 0.02 seconds.
+   */
+  PIDController(double Kp, double Ki, double Kd, double period = 0.02);
+
+  ~PIDController() override = default;
+
+  PIDController(PIDController&& rhs);
+  PIDController& operator=(PIDController&& rhs);
+
+  /**
+   * Sets the PID Controller gain parameters.
+   *
+   * Sets the proportional, integral, and differential coefficients.
+   *
+   * @param Kp Proportional coefficient
+   * @param Ki Integral coefficient
+   * @param Kd Differential coefficient
+   */
+  void SetPID(double Kp, double Ki, double Kd);
+
+  /**
+   * Sets the proportional coefficient of the PID controller gain.
+   *
+   * @param Kp proportional coefficient
+   */
+  void SetP(double Kp);
+
+  /**
+   * Sets the integral coefficient of the PID controller gain.
+   *
+   * @param Ki integral coefficient
+   */
+  void SetI(double Ki);
+
+  /**
+   * Sets the differential coefficient of the PID controller gain.
+   *
+   * @param Kd differential coefficient
+   */
+  void SetD(double Kd);
+
+  /**
+   * Gets the proportional coefficient.
+   *
+   * @return proportional coefficient
+   */
+  double GetP() const;
+
+  /**
+   * Gets the integral coefficient.
+   *
+   * @return integral coefficient
+   */
+  double GetI() const;
+
+  /**
+   * Gets the differential coefficient.
+   *
+   * @return differential coefficient
+   */
+  double GetD() const;
+
+  /**
+   * Gets the period of this controller.
+   *
+   * @return The period of the controller.
+   */
+  double GetPeriod() const;
+
+  /**
+   * Returns the current controller output.
+   *
+   * This is always centered around zero and constrained to the min and max
+   * outputs.
+   *
+   * @return The latest calculated output.
+   */
+  double GetOutput() const;
+
+  /**
+   * Sets the setpoint for the PIDController.
+   *
+   * @param setpoint The desired setpoint.
+   */
+  void SetSetpoint(double setpoint);
+
+  /**
+   * Returns the current setpoint of the PIDController.
+   *
+   * @return The current setpoint.
+   */
+  double GetSetpoint() const;
+
+  /**
+   * Returns true if the error is within tolerance of the setpoint.
+   *
+   * This will return false until at least one input value has been computed.
+   *
+   * @param tolerance The maximum allowable error.
+   * @param deltaTolerance The maximum allowable change in error.
+   * @param toleranceType The type of tolerances specified.
+   */
+  bool AtSetpoint(
+      double tolerance,
+      double deltaTolerance = std::numeric_limits<double>::infinity(),
+      Tolerance toleranceType = Tolerance::kAbsolute) const;
+
+  /**
+   * Returns true if the error is within the tolerance of the error.
+   *
+   * Currently this just reports on target as the actual value passes through
+   * the setpoint. Ideally it should be based on being within the tolerance for
+   * some period of time.
+   *
+   * This will return false until at least one input value has been computed.
+   */
+  bool AtSetpoint() const;
+
+  /**
+   * Sets the PID controller to consider the input to be continuous.
+   *
+   * Rather then using the max and min input range as constraints, it considers
+   * them to be the same point and automatically calculates the shortest route
+   * to the setpoint.
+   *
+   * @param continuous true turns on continuous, false turns off continuous
+   */
+  void SetContinuous(bool continuous = true);
+
+  /**
+   * Sets the minimum and maximum values expected from the input.
+   *
+   * @param minimumInput The minimum value expected from the input.
+   * @param maximumInput The maximum value expected from the input.
+   */
+  void SetInputRange(double minimumInput, double maximumInput);
+
+  /**
+   * Sets the minimum and maximum values to write.
+   *
+   * @param minimumOutput the minimum value to write to the output
+   * @param maximumOutput the maximum value to write to the output
+   */
+  void SetOutputRange(double minimumOutput, double maximumOutput);
+
+  /**
+   * Sets the absolute error which is considered tolerable for use with
+   * AtSetpoint().
+   *
+   * @param tolerance      Error which is tolerable.
+   * @param deltaTolerance Change in error per second which is tolerable.
+   */
+  void SetAbsoluteTolerance(
+      double tolerance,
+      double deltaTolerance = std::numeric_limits<double>::infinity());
+
+  /**
+   * Sets the percentage error which is considered tolerable for use with
+   * AtSetpoint().
+   *
+   * @param tolerance      Percent error which is tolerable.
+   * @param deltaTolerance Change in percent error per second which is
+   *                       tolerable.
+   */
+  void SetPercentTolerance(
+      double tolerance,
+      double deltaTolerance = std::numeric_limits<double>::infinity());
+
+  /**
+   * Returns the difference between the setpoint and the measurement.
+   *
+   * @return The error.
+   */
+  double GetError() const;
+
+  /**
+   * Returns the change in error per second.
+   */
+  double GetDeltaError() const;
+
+  /**
+   * Returns the next output of the PID controller.
+   *
+   * @param measurement The current measurement of the process variable.
+   */
+  double Calculate(double measurement);
+
+  /**
+   * Returns the next output of the PID controller.
+   *
+   * @param measurement The current measurement of the process variable.
+   * @param setpoint The new setpoint of the controller.
+   */
+  double Calculate(double measurement, double setpoint);
+
+  /**
+   * Reset the previous error, the integral term, and disable the controller.
+   */
+  void Reset();
+
+  void InitSendable(frc::SendableBuilder& builder) override;
+
+ protected:
+  mutable wpi::mutex m_thisMutex;
+
+  /**
+   * 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 GetContinuousError(double error) const;
+
+ private:
+  // Factor for "proportional" control
+  double m_Kp;
+
+  // Factor for "integral" control
+  double m_Ki;
+
+  // Factor for "derivative" control
+  double m_Kd;
+
+  // The period (in seconds) of the control loop running this controller
+  double m_period;
+
+  // |maximum output|
+  double m_maximumOutput = 1.0;
+
+  // |minimum output|
+  double m_minimumOutput = -1.0;
+
+  // Maximum input - limit setpoint to this
+  double m_maximumInput = 0;
+
+  // Minimum input - limit setpoint to this
+  double m_minimumInput = 0;
+
+  // input range - difference between maximum and minimum
+  double m_inputRange = 0;
+
+  // Do the endpoints wrap around? eg. Absolute encoder
+  bool m_continuous = false;
+
+  // The error at the time of the most recent call to calculate()
+  double m_currError = 0;
+
+  // The error at the time of the second-most-recent call to calculate() (used
+  // to compute velocity)
+  double m_prevError = std::numeric_limits<double>::infinity();
+
+  // The sum of the errors for use in the integral calc
+  double m_totalError = 0;
+
+  Tolerance m_toleranceType = Tolerance::kAbsolute;
+
+  // The percentage or absolute error that is considered at setpoint.
+  double m_tolerance = 0.05;
+  double m_deltaTolerance = std::numeric_limits<double>::infinity();
+
+  double m_setpoint = 0;
+  double m_output = 0;
+
+  /**
+   * Returns the next output of the PID controller.
+   *
+   * Unlike the public functions above, this function doesn't lock the mutex.
+   *
+   * @param measurement The current measurement of the process variable.
+   */
+  double CalculateUnsafe(double measurement);
+};
+
+}  // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc/controller/PIDControllerRunner.h b/wpilibc/src/main/native/include/frc/controller/PIDControllerRunner.h
new file mode 100644
index 0000000000..aa47ca73a0
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/controller/PIDControllerRunner.h
@@ -0,0 +1,73 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <functional>
+
+#include <wpi/mutex.h>
+
+#include "frc/Notifier.h"
+#include "frc/controller/PIDController.h"
+#include "frc/smartdashboard/SendableBase.h"
+
+namespace frc {
+
+class PIDControllerRunner : SendableBase {
+ public:
+  /**
+   * Allocates a PIDControllerRunner.
+   *
+   * @param controller       The controller on which to call Update().
+   * @param controllerOutput The function which updates the plant using the
+   *                         controller output passed as the argument.
+   */
+  PIDControllerRunner(frc2::PIDController& controller,
+                      std::function<double(void)> measurementSource,
+                      std::function<void(double)> controllerOutput);
+
+  ~PIDControllerRunner();
+
+  PIDControllerRunner(PIDControllerRunner&&) = default;
+  PIDControllerRunner& operator=(PIDControllerRunner&&) = default;
+
+  /**
+   * Begins running the controller.
+   */
+  void Enable();
+
+  /**
+   * Stops running the controller.
+   *
+   * This sets the output to zero before stopping.
+   */
+  void Disable();
+
+  /**
+   * Returns whether controller is running.
+   */
+  bool IsEnabled() const;
+
+  void InitSendable(SendableBuilder& builder) override;
+
+ private:
+  Notifier m_notifier{&PIDControllerRunner::Run, this};
+  frc2::PIDController& m_controller;
+  std::function<double(void)> m_measurementSource;
+  std::function<void(double)> m_controllerOutput;
+  bool m_enabled = false;
+
+  mutable wpi::mutex m_thisMutex;
+
+  // Ensures when Disable() is called, m_controllerOutput() won't run if
+  // Controller::Update() is already running at that time.
+  mutable wpi::mutex m_outputMutex;
+
+  void Run();
+};
+
+}  // namespace frc
diff --git a/wpilibc/src/test/native/cpp/PIDInputOutputTest.cpp b/wpilibc/src/test/native/cpp/PIDInputOutputTest.cpp
new file mode 100644
index 0000000000..73ccfb8293
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/PIDInputOutputTest.cpp
@@ -0,0 +1,70 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2014-2019 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 "frc/controller/PIDController.h"
+#include "gtest/gtest.h"
+
+class PIDInputOutputTest : public testing::Test {
+ protected:
+  frc2::PIDController* controller;
+
+  void SetUp() override { controller = new frc2::PIDController(0, 0, 0); }
+
+  void TearDown() override { delete controller; }
+};
+
+TEST_F(PIDInputOutputTest, OutputRangeTest) {
+  controller->SetP(1);
+  controller->SetOutputRange(-50, 50);
+
+  EXPECT_DOUBLE_EQ(-50, controller->Calculate(100, 0));
+  EXPECT_DOUBLE_EQ(50, controller->Calculate(0, 100));
+}
+
+TEST_F(PIDInputOutputTest, InputRangeTest) {
+  controller->SetP(1);
+  controller->SetOutputRange(-1000, 1000);
+  controller->SetInputRange(-50, 50);
+
+  EXPECT_DOUBLE_EQ(-100, controller->Calculate(100, 0));
+  EXPECT_DOUBLE_EQ(50, controller->Calculate(0, 100));
+}
+
+TEST_F(PIDInputOutputTest, ContinuousInputTest) {
+  controller->SetP(1);
+  controller->SetInputRange(-180, 180);
+  controller->SetContinuous(true);
+
+  EXPECT_TRUE(controller->Calculate(-179, 179) < 0);
+}
+
+TEST_F(PIDInputOutputTest, ProportionalGainOutputTest) {
+  controller->SetP(4);
+
+  EXPECT_DOUBLE_EQ(-.1, controller->Calculate(.025, 0));
+}
+
+TEST_F(PIDInputOutputTest, IntegralGainOutputTest) {
+  controller->SetI(4);
+
+  double out = 0;
+
+  for (int i = 0; i < 5; i++) {
+    out = controller->Calculate(.025, 0);
+  }
+
+  EXPECT_DOUBLE_EQ(-.5 * controller->GetPeriod(), out);
+}
+
+TEST_F(PIDInputOutputTest, DerivativeGainOutputTest) {
+  controller->SetD(4);
+
+  controller->Calculate(0, 0);
+
+  EXPECT_DOUBLE_EQ(-.01 / controller->GetPeriod(),
+                   controller->Calculate(.0025, 0));
+}
diff --git a/wpilibcIntegrationTests/src/main/native/cpp/PIDToleranceTest.cpp b/wpilibc/src/test/native/cpp/PIDToleranceTest.cpp
similarity index 53%
rename from wpilibcIntegrationTests/src/main/native/cpp/PIDToleranceTest.cpp
rename to wpilibc/src/test/native/cpp/PIDToleranceTest.cpp
index ec3b85a94f..e978d9c02c 100644
--- a/wpilibcIntegrationTests/src/main/native/cpp/PIDToleranceTest.cpp
+++ b/wpilibc/src/test/native/cpp/PIDToleranceTest.cpp
@@ -1,15 +1,13 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2014-2019 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 "TestBench.h"
-#include "frc/PIDController.h"
-#include "frc/PIDOutput.h"
-#include "frc/PIDSource.h"
 #include "frc/Timer.h"
+#include "frc/controller/PIDController.h"
+#include "frc/controller/PIDControllerRunner.h"
 #include "gtest/gtest.h"
 
 using namespace frc;
@@ -20,80 +18,86 @@ class PIDToleranceTest : public testing::Test {
   const double range = 200;
   const double tolerance = 10.0;
 
-  class FakeInput : public PIDSource {
+  class FakeInput {
    public:
     double val = 0;
 
-    void SetPIDSourceType(PIDSourceType pidSource) {}
-
-    PIDSourceType GetPIDSourceType() { return PIDSourceType::kDisplacement; }
-
-    double PIDGet() { return val; }
+    double Get() { return val; }
   };
 
-  class FakeOutput : public PIDOutput {
-    void PIDWrite(double output) {}
+  class FakeOutput {
+   public:
+    void Set(double output) {}
   };
 
   FakeInput inp;
   FakeOutput out;
-  PIDController* pid;
+  frc2::PIDController* pidController;
+  frc::PIDControllerRunner* pidRunner;
 
   void SetUp() override {
-    pid = new PIDController(0.5, 0.0, 0.0, &inp, &out);
-    pid->SetInputRange(-range / 2, range / 2);
+    pidController = new frc2::PIDController(0.5, 0.0, 0.0);
+    pidController->SetInputRange(-range / 2, range / 2);
+    pidRunner =
+        new frc::PIDControllerRunner(*pidController, [&] { return inp.Get(); },
+                                     [&](double output) { out.Set(output); });
   }
 
-  void TearDown() override { delete pid; }
+  void TearDown() override {
+    delete pidRunner;
+    delete pidController;
+  }
 
   void Reset() { inp.val = 0; }
 };
 
+TEST_F(PIDToleranceTest, Initial) { EXPECT_FALSE(pidController->AtSetpoint()); }
+
 TEST_F(PIDToleranceTest, Absolute) {
   Reset();
 
-  pid->SetAbsoluteTolerance(tolerance);
-  pid->SetSetpoint(setpoint);
-  pid->Enable();
+  pidController->SetAbsoluteTolerance(tolerance);
+  pidController->SetSetpoint(setpoint);
+  pidRunner->Enable();
 
-  EXPECT_FALSE(pid->OnTarget())
+  EXPECT_FALSE(pidController->AtSetpoint())
       << "Error was in tolerance when it should not have been. Error was "
-      << pid->GetError();
+      << pidController->GetError();
 
   inp.val = setpoint + tolerance / 2;
   Wait(1.0);
 
-  EXPECT_TRUE(pid->OnTarget())
+  EXPECT_TRUE(pidController->AtSetpoint())
       << "Error was not in tolerance when it should have been. Error was "
-      << pid->GetError();
+      << pidController->GetError();
 
   inp.val = setpoint + 10 * tolerance;
   Wait(1.0);
 
-  EXPECT_FALSE(pid->OnTarget())
+  EXPECT_FALSE(pidController->AtSetpoint())
       << "Error was in tolerance when it should not have been. Error was "
-      << pid->GetError();
+      << pidController->GetError();
 }
 
 TEST_F(PIDToleranceTest, Percent) {
   Reset();
 
-  pid->SetPercentTolerance(tolerance);
-  pid->SetSetpoint(setpoint);
-  pid->Enable();
+  pidController->SetPercentTolerance(tolerance);
+  pidController->SetSetpoint(setpoint);
+  pidRunner->Enable();
 
-  EXPECT_FALSE(pid->OnTarget())
+  EXPECT_FALSE(pidController->AtSetpoint())
       << "Error was in tolerance when it should not have been. Error was "
-      << pid->GetError();
+      << pidController->GetError();
 
   inp.val =
       setpoint + (tolerance) / 200 *
                      range;  // half of percent tolerance away from setpoint
   Wait(1.0);
 
-  EXPECT_TRUE(pid->OnTarget())
+  EXPECT_TRUE(pidController->AtSetpoint())
       << "Error was not in tolerance when it should have been. Error was "
-      << pid->GetError();
+      << pidController->GetError();
 
   inp.val =
       setpoint +
@@ -101,7 +105,7 @@ TEST_F(PIDToleranceTest, Percent) {
 
   Wait(1.0);
 
-  EXPECT_FALSE(pid->OnTarget())
+  EXPECT_FALSE(pidController->AtSetpoint())
       << "Error was in tolerance when it should not have been. Error was "
-      << pid->GetError();
+      << pidController->GetError();
 }
diff --git a/wpilibcIntegrationTests/src/main/native/cpp/MotorEncoderTest.cpp b/wpilibcIntegrationTests/src/main/native/cpp/MotorEncoderTest.cpp
index a5b33de28e..d463a26d88 100644
--- a/wpilibcIntegrationTests/src/main/native/cpp/MotorEncoderTest.cpp
+++ b/wpilibcIntegrationTests/src/main/native/cpp/MotorEncoderTest.cpp
@@ -8,10 +8,11 @@
 #include "TestBench.h"
 #include "frc/Encoder.h"
 #include "frc/Jaguar.h"
-#include "frc/PIDController.h"
 #include "frc/Talon.h"
 #include "frc/Timer.h"
 #include "frc/Victor.h"
+#include "frc/controller/PIDController.h"
+#include "frc/controller/PIDControllerRunner.h"
 #include "frc/filters/LinearDigitalFilter.h"
 #include "gtest/gtest.h"
 
@@ -139,22 +140,24 @@ TEST_P(MotorEncoderTest, ClampSpeed) {
 TEST_P(MotorEncoderTest, PositionPIDController) {
   Reset();
   double goal = 1000;
-  m_encoder->SetPIDSourceType(PIDSourceType::kDisplacement);
-  PIDController pid(0.001, 0.0005, 0.0, m_encoder, m_speedController);
-  pid.SetAbsoluteTolerance(50.0);
-  pid.SetOutputRange(-0.2, 0.2);
-  pid.SetSetpoint(goal);
+  frc2::PIDController pidController(0.001, 0.01, 0.0);
+  pidController.SetAbsoluteTolerance(50.0);
+  pidController.SetOutputRange(-0.2, 0.2);
+  pidController.SetSetpoint(goal);
 
-  /* 10 seconds should be plenty time to get to the setpoint */
-  pid.Enable();
+  /* 10 seconds should be plenty time to get to the reference */
+  frc::PIDControllerRunner pidRunner(
+      pidController, [&] { return m_encoder->GetDistance(); },
+      [&](double output) { m_speedController->Set(output); });
+  pidRunner.Enable();
   Wait(10.0);
-  pid.Disable();
+  pidRunner.Disable();
 
-  RecordProperty("PIDError", pid.GetError());
+  RecordProperty("PIDError", pidController.GetError());
 
-  EXPECT_TRUE(pid.OnTarget())
+  EXPECT_TRUE(pidController.AtSetpoint())
       << "PID loop did not converge within 10 seconds. Goal was: " << goal
-      << " Error was: " << pid.GetError();
+      << " Error was: " << pidController.GetError();
 }
 
 /**
@@ -164,20 +167,23 @@ TEST_P(MotorEncoderTest, VelocityPIDController) {
   Reset();
 
   m_encoder->SetPIDSourceType(PIDSourceType::kRate);
-  PIDController pid(1e-5, 0.0, 3e-5, 8e-5, m_filter, m_speedController);
-  pid.SetAbsoluteTolerance(200.0);
-  pid.SetOutputRange(-0.3, 0.3);
-  pid.SetSetpoint(600);
+  frc2::PIDController pidController(1e-5, 0.0, 0.0006);
+  pidController.SetAbsoluteTolerance(200.0);
+  pidController.SetOutputRange(-0.3, 0.3);
+  pidController.SetSetpoint(600);
 
-  /* 10 seconds should be plenty time to get to the setpoint */
-  pid.Enable();
+  /* 10 seconds should be plenty time to get to the reference */
+  frc::PIDControllerRunner pidRunner(
+      pidController, [&] { return m_filter->PIDGet(); },
+      [&](double output) { m_speedController->Set(output + 8e-5); });
+  pidRunner.Enable();
   Wait(10.0);
-  pid.Disable();
-  RecordProperty("PIDError", pid.GetError());
+  pidRunner.Disable();
+  RecordProperty("PIDError", pidController.GetError());
 
-  EXPECT_TRUE(pid.OnTarget())
+  EXPECT_TRUE(pidController.AtSetpoint())
       << "PID loop did not converge within 10 seconds. Goal was: " << 600
-      << " Error was: " << pid.GetError();
+      << " Error was: " << pidController.GetError();
 }
 
 /**
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Controller.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Controller.java
index 32fc7c3cfb..31d50d82f0 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Controller.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Controller.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2019 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.                                                               */
@@ -11,7 +11,10 @@ package edu.wpi.first.wpilibj;
  * An interface for controllers. Controllers run control loops, the most command are PID controllers
  * and there variants, but this includes anything that is controlling an actuator in a separate
  * thread.
+ *
+ * @deprecated None of the 2020 FRC controllers use this.
  */
+@Deprecated(since = "2020", forRemoval = true)
 public interface Controller {
   /**
    * Allows the control loop to run.
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDController.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDController.java
index da4118f8c7..b87e406e1a 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDController.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDController.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2019 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.                                                               */
@@ -18,7 +18,10 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
  * <p>This feedback controller runs in discrete time, so time deltas are not used in the integral
  * and derivative calculations. Therefore, the sample rate affects the controller's behavior for a
  * given set of PID constants.
+ *
+ * @deprecated Use {@link edu.wpi.first.wpilibj.controller.PIDController} instead.
  */
+@Deprecated(since = "2020", forRemoval = true)
 public class PIDController extends PIDBase implements Controller {
   Notifier m_controlLoop = new Notifier(this::calculate);
 
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDOutput.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDOutput.java
index 0ef9403bda..e9af2ff7e5 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDOutput.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDOutput.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2019 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.                                                               */
@@ -9,8 +9,11 @@ package edu.wpi.first.wpilibj;
 
 /**
  * This interface allows PIDController to write it's results to its output.
+ *
+ * @deprecated Use DoubleConsumer and new PIDController class.
  */
 @FunctionalInterface
+@Deprecated(since = "2020", forRemoval = true)
 public interface PIDOutput {
   /**
    * Set the output to the value calculated by PIDController.
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDSource.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDSource.java
index 841a232b8f..3c4f1f5ce1 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDSource.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDSource.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2019 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.                                                               */
@@ -9,7 +9,10 @@ package edu.wpi.first.wpilibj;
 
 /**
  * This interface allows for PIDController to automatically read from this object.
+ *
+ * @deprecated Use DoubleSupplier and new PIDController class.
  */
+@Deprecated(since = "2020", forRemoval = true)
 public interface PIDSource {
   /**
    * Set which parameter of the device you are using as a process control variable.
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDSourceType.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDSourceType.java
index 31aa79abd2..7508be54b2 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDSourceType.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDSourceType.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2015-2019 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.                                                               */
@@ -10,6 +10,7 @@ package edu.wpi.first.wpilibj;
 /**
  * A description for the type of output value to provide to a PIDController.
  */
+@Deprecated(since = "2020", forRemoval = true)
 public enum PIDSourceType {
   kDisplacement,
   kRate
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
new file mode 100644
index 0000000000..4e8507b8ec
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java
@@ -0,0 +1,570 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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 java.util.concurrent.locks.ReentrantLock;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.SendableBase;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+
+/**
+ * Implements a PID control loop.
+ */
+@SuppressWarnings("PMD.TooManyFields")
+public class PIDController extends SendableBase {
+  protected final ReentrantLock m_thisMutex = new ReentrantLock();
+
+  private static int instances;
+
+  // Factor for "proportional" control
+  @SuppressWarnings("MemberName")
+  private double m_Kp;
+
+  // Factor for "integral" control
+  @SuppressWarnings("MemberName")
+  private double m_Ki;
+
+  // Factor for "derivative" control
+  @SuppressWarnings("MemberName")
+  private double m_Kd;
+
+  // The period (in seconds) of the loop that calls the controller
+  private final double m_period;
+
+  // |maximum output|
+  private double m_maximumOutput = 1.0;
+
+  // |minimum output|
+  private double m_minimumOutput = -1.0;
+
+  // Maximum input - limit setpoint to this
+  private double m_maximumInput;
+
+  // Minimum input - limit setpoint to this
+  private double m_minimumInput;
+
+  // input range - difference between maximum and minimum
+  private double m_inputRange;
+
+  // Do the endpoints wrap around? eg. Absolute encoder
+  private boolean m_continuous;
+
+  // The error at the time of the most recent call to calculate()
+  private double m_currError;
+
+  // The error at the time of the second-most-recent call to calculate() (used to compute velocity)
+  private double m_prevError = Double.POSITIVE_INFINITY;
+
+  // The sum of the errors for use in the integral calc
+  private double m_totalError;
+
+  enum Tolerance {
+    kAbsolute, kPercent;
+  }
+
+  private Tolerance m_toleranceType = Tolerance.kAbsolute;
+
+  // The percentage or absolute error that is considered at setpoint.
+  private double m_tolerance = 0.05;
+  private double m_deltaTolerance = Double.POSITIVE_INFINITY;
+
+  private double m_setpoint;
+  private double m_output;
+
+  /**
+   * Allocates a PIDController with the given constants for Kp, Ki, and Kd and a default period of
+   * 0.02 seconds.
+   *
+   * @param Kp The proportional coefficient.
+   * @param Ki The integral coefficient.
+   * @param Kd The derivative coefficient.
+   */
+  @SuppressWarnings("ParameterName")
+  public PIDController(double Kp, double Ki, double Kd) {
+    this(Kp, Ki, Kd, 0.02);
+  }
+
+  /**
+   * Allocates a PIDController with the given constants for Kp, Ki, and Kd.
+   *
+   * @param Kp     The proportional coefficient.
+   * @param Ki     The integral coefficient.
+   * @param Kd     The derivative coefficient.
+   * @param period The period between controller updates in seconds.
+   */
+  @SuppressWarnings("ParameterName")
+  public PIDController(double Kp, double Ki, double Kd, double period) {
+    m_Kp = Kp;
+    m_Ki = Ki;
+    m_Kd = Kd;
+
+    m_period = period;
+
+    instances++;
+    HAL.report(tResourceType.kResourceType_PIDController, instances);
+  }
+
+  /**
+   * Sets the PID Controller gain parameters.
+   *
+   * <p>Set the proportional, integral, and differential coefficients.
+   *
+   * @param Kp The proportional coefficient.
+   * @param Ki The integral coefficient.
+   * @param Kd The derivative coefficient.
+   */
+  @SuppressWarnings("ParameterName")
+  public void setPID(double Kp, double Ki, double Kd) {
+    m_thisMutex.lock();
+    try {
+      m_Kp = Kp;
+      m_Ki = Ki;
+      m_Kd = Kd;
+    } finally {
+      m_thisMutex.unlock();
+    }
+  }
+
+  /**
+   * Sets the Proportional coefficient of the PID controller gain.
+   *
+   * @param Kp proportional coefficient
+   */
+  @SuppressWarnings("ParameterName")
+  public void setP(double Kp) {
+    m_thisMutex.lock();
+    try {
+      m_Kp = Kp;
+    } finally {
+      m_thisMutex.unlock();
+    }
+  }
+
+  /**
+   * Sets the Integral coefficient of the PID controller gain.
+   *
+   * @param Ki integral coefficient
+   */
+  @SuppressWarnings("ParameterName")
+  public void setI(double Ki) {
+    m_thisMutex.lock();
+    try {
+      m_Ki = Ki;
+    } finally {
+      m_thisMutex.unlock();
+    }
+  }
+
+  /**
+   * Sets the Differential coefficient of the PID controller gain.
+   *
+   * @param Kd differential coefficient
+   */
+  @SuppressWarnings("ParameterName")
+  public void setD(double Kd) {
+    m_thisMutex.lock();
+    try {
+      m_Kd = Kd;
+    } finally {
+      m_thisMutex.unlock();
+    }
+  }
+
+  /**
+   * Get the Proportional coefficient.
+   *
+   * @return proportional coefficient
+   */
+  public double getP() {
+    m_thisMutex.lock();
+    try {
+      return m_Kp;
+    } finally {
+      m_thisMutex.unlock();
+    }
+  }
+
+  /**
+   * Get the Integral coefficient.
+   *
+   * @return integral coefficient
+   */
+  public double getI() {
+    m_thisMutex.lock();
+    try {
+      return m_Ki;
+    } finally {
+      m_thisMutex.unlock();
+    }
+  }
+
+  /**
+   * Get the Differential coefficient.
+   *
+   * @return differential coefficient
+   */
+  public double getD() {
+    m_thisMutex.lock();
+    try {
+      return m_Kd;
+    } finally {
+      m_thisMutex.unlock();
+    }
+  }
+
+  /**
+   * Returns the period of this controller.
+   *
+   * @return the period of the controller.
+   */
+  public double getPeriod() {
+    return m_period;
+  }
+
+  /**
+   * Returns the current controller output.
+   *
+   * <p>This is always centered around zero and constrained to the min and max outputs.
+   *
+   * @return The latest calculated output.
+   */
+  public double getOutput() {
+    m_thisMutex.lock();
+    try {
+      return m_output;
+    } finally {
+      m_thisMutex.unlock();
+    }
+  }
+
+  /**
+   * Sets the setpoint for the PIDController.
+   *
+   * @param setpoint The desired setpoint.
+   */
+  public void setSetpoint(double setpoint) {
+    m_thisMutex.lock();
+    try {
+      if (m_maximumInput > m_minimumInput) {
+        m_setpoint = clamp(setpoint, m_minimumInput, m_maximumInput);
+      } else {
+        m_setpoint = setpoint;
+      }
+    } finally {
+      m_thisMutex.unlock();
+    }
+  }
+
+  /**
+   * Returns the current setpoint of the PIDController.
+   *
+   * @return The current setpoint.
+   */
+  public double getSetpoint() {
+    m_thisMutex.lock();
+    try {
+      return m_setpoint;
+    } finally {
+      m_thisMutex.unlock();
+    }
+  }
+
+  /**
+   * Returns true if the error is within the percentage of the total input range,
+   * determined by SetTolerance. This asssumes that the maximum and minimum
+   * input were set using SetInput.
+   *
+   * <p>This will return false until at least one input value has been computed.
+   *
+   * @return Whether the error is within the acceptable bounds.
+   */
+  public boolean atSetpoint() {
+    return atSetpoint(m_tolerance, m_deltaTolerance, m_toleranceType);
+  }
+
+  /**
+   * Returns true if the error and change in error are below the specified tolerances.
+   *
+   * @param tolerance The maximum allowable error.
+   * @param deltaTolerance The maximum allowable change in error from the previous iteration.
+   * @param toleranceType Whether the given tolerance values are absolute, or percentages of the
+   *                      total input range.
+   * @return Whether the error is within the acceptable bounds.
+   */
+  public boolean atSetpoint(double tolerance, double deltaTolerance, Tolerance toleranceType) {
+    double error = getError();
+
+    m_thisMutex.lock();
+    try {
+      double deltaError = (error - m_prevError) / getPeriod();
+      if (toleranceType == Tolerance.kPercent) {
+        return Math.abs(error) < tolerance / 100 * m_inputRange
+            && Math.abs(deltaError) < deltaTolerance / 100 * m_inputRange;
+      } else {
+        return Math.abs(error) < tolerance
+            && Math.abs(deltaError) < deltaTolerance;
+      }
+    } finally {
+      m_thisMutex.unlock();
+    }
+  }
+
+  /**
+   * Sets the PID controller to consider the input to be continuous.
+   *
+   * <p>Rather then using the max and min input range as constraints, it considers them to be the
+   * same point and automatically calculates the shortest route to the setpoint.
+   */
+  public void setContinuous() {
+    setContinuous(true);
+  }
+
+  /**
+   * Sets the PID controller to consider the input to be continuous,
+   *
+   * <p>Rather then using the max and min input range as constraints, it considers them to be the
+   * same point and automatically calculates the shortest route to the setpoint.
+   *
+   * @param continuous true turns on continuous, false turns off continuous
+   */
+  public void setContinuous(boolean continuous) {
+    m_thisMutex.lock();
+    try {
+      m_continuous = continuous;
+    } finally {
+      m_thisMutex.unlock();
+    }
+  }
+
+  /**
+   * Sets the minimum and maximum values expected from the input.
+   *
+   * @param minimumInput The minimum value expected from the input.
+   * @param maximumInput The maximum value expected from the input.
+   */
+  public void setInputRange(double minimumInput, double maximumInput) {
+    m_thisMutex.lock();
+    try {
+      m_minimumInput = minimumInput;
+      m_maximumInput = maximumInput;
+      m_inputRange = maximumInput - minimumInput;
+
+      // Clamp setpoint to new input
+      if (m_maximumInput > m_minimumInput) {
+        m_setpoint = clamp(m_setpoint, m_minimumInput, m_maximumInput);
+      }
+    } finally {
+      m_thisMutex.unlock();
+    }
+  }
+
+  /**
+   * Sets the minimum and maximum values to write.
+   *
+   * @param minimumOutput the minimum value to write to the output
+   * @param maximumOutput the maximum value to write to the output
+   */
+  public void setOutputRange(double minimumOutput, double maximumOutput) {
+    m_thisMutex.lock();
+    try {
+      m_minimumOutput = minimumOutput;
+      m_maximumOutput = maximumOutput;
+    } finally {
+      m_thisMutex.unlock();
+    }
+  }
+
+  /**
+   * Sets the absolute error which is considered tolerable for use with atSetpoint().
+   *
+   * @param tolerance Absolute error which is tolerable.
+   */
+  public void setAbsoluteTolerance(double tolerance) {
+    setAbsoluteTolerance(tolerance, Double.POSITIVE_INFINITY);
+  }
+
+  /**
+   * Sets the absolute error which is considered tolerable for use with atSetpoint().
+   *
+   * @param tolerance      Absolute error which is tolerable.
+   * @param deltaTolerance Change in absolute error per second which is tolerable.
+   */
+  public void setAbsoluteTolerance(double tolerance, double deltaTolerance) {
+    m_thisMutex.lock();
+    try {
+      m_toleranceType = Tolerance.kAbsolute;
+      m_tolerance = tolerance;
+      m_deltaTolerance = deltaTolerance;
+    } finally {
+      m_thisMutex.unlock();
+    }
+  }
+
+  /**
+   * Sets the percent error which is considered tolerable for use with atSetpoint().
+   *
+   * @param tolerance Percent error which is tolerable.
+   */
+  public void setPercentTolerance(double tolerance) {
+    setPercentTolerance(tolerance, Double.POSITIVE_INFINITY);
+  }
+
+  /**
+   * Sets the percent error which is considered tolerable for use with atSetpoint().
+   *
+   * @param tolerance      Percent error which is tolerable.
+   * @param deltaTolerance Change in percent error per second which is tolerable.
+   */
+  public void setPercentTolerance(double tolerance, double deltaTolerance) {
+    m_thisMutex.lock();
+    try {
+      m_toleranceType = Tolerance.kPercent;
+      m_tolerance = tolerance;
+      m_deltaTolerance = deltaTolerance;
+    } finally {
+      m_thisMutex.unlock();
+    }
+  }
+
+  /**
+   * Returns the difference between the setpoint and the measurement.
+   *
+   * @return The error.
+   */
+  public double getError() {
+    m_thisMutex.lock();
+    try {
+      return getContinuousError(m_currError);
+    } finally {
+      m_thisMutex.unlock();
+    }
+  }
+
+  /**
+   * Returns the change in error per second.
+   */
+  public double getDeltaError() {
+    double error = getError();
+
+    m_thisMutex.lock();
+    try {
+      return (error - m_prevError) / getPeriod();
+    } finally {
+      m_thisMutex.unlock();
+    }
+  }
+
+  /**
+   * Returns the next output of the PID controller.
+   *
+   * @param measurement The current measurement of the process variable.
+   */
+  public double calculate(double measurement) {
+    m_thisMutex.lock();
+    try {
+      return calculateUnsafe(measurement);
+    } finally {
+      m_thisMutex.unlock();
+    }
+  }
+
+  /**
+   * Returns the next output of the PID controller.
+   *
+   * @param measurement The current measurement of the process variable.
+   * @param setpoint The new setpoint of the controller.
+   */
+  public double calculate(double measurement, double setpoint) {
+    m_thisMutex.lock();
+    try {
+      // Set setpoint to provided value
+      if (m_maximumInput > m_minimumInput) {
+        m_setpoint = clamp(setpoint, m_minimumInput, m_maximumInput);
+      } else {
+        m_setpoint = setpoint;
+      }
+
+      return calculateUnsafe(measurement);
+    } finally {
+      m_thisMutex.unlock();
+    }
+  }
+
+  /**
+   * Resets the previous error and the integral term. Also disables the controller.
+   */
+  public void reset() {
+    m_thisMutex.lock();
+    try {
+      m_prevError = 0;
+      m_totalError = 0;
+      m_output = 0;
+    } finally {
+      m_thisMutex.unlock();
+    }
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    builder.setSmartDashboardType("PIDController");
+    builder.addDoubleProperty("p", this::getP, this::setP);
+    builder.addDoubleProperty("i", this::getI, this::setI);
+    builder.addDoubleProperty("d", this::getD, this::setD);
+    builder.addDoubleProperty("setpoint", this::getSetpoint, this::setSetpoint);
+  }
+
+  /**
+   * 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 && m_inputRange > 0) {
+      error %= m_inputRange;
+      if (Math.abs(error) > m_inputRange / 2) {
+        if (error > 0) {
+          return error - m_inputRange;
+        } else {
+          return error + m_inputRange;
+        }
+      }
+    }
+
+    return error;
+  }
+
+  /**
+   * Returns the next output of the PID controller.
+   *
+   * <p>Unlike the public functions above, this function doesn't lock the mutex.
+   *
+   * @param measurement The current measurement of the process variable.
+   */
+  private double calculateUnsafe(double measurement) {
+    m_prevError = m_currError;
+    m_currError = getContinuousError(m_setpoint - measurement);
+
+    if (m_Ki != 0) {
+      m_totalError = clamp(m_totalError + m_currError * getPeriod(), m_minimumOutput / m_Ki,
+          m_maximumOutput / m_Ki);
+    }
+
+    m_output = clamp(m_Kp * m_currError + m_Ki * m_totalError
+        + m_Kd * (m_currError - m_prevError) / getPeriod(), m_minimumOutput, m_maximumOutput);
+
+    return m_output;
+  }
+
+  private static double clamp(double value, double low, double high) {
+    return Math.max(low, Math.min(value, high));
+  }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDControllerRunner.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDControllerRunner.java
new file mode 100644
index 0000000000..7d6edb866d
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDControllerRunner.java
@@ -0,0 +1,129 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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 java.util.concurrent.locks.ReentrantLock;
+import java.util.function.DoubleConsumer;
+import java.util.function.DoubleSupplier;
+
+import edu.wpi.first.wpilibj.Notifier;
+import edu.wpi.first.wpilibj.SendableBase;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+
+public class PIDControllerRunner extends SendableBase {
+  private final Notifier m_notifier = new Notifier(this::run);
+  private final PIDController m_controller;
+  private final DoubleConsumer m_controllerOutput;
+  private final DoubleSupplier m_measurementSource;
+  private boolean m_enabled;
+
+  private final ReentrantLock m_thisMutex = new ReentrantLock();
+
+  // Ensures when disable() is called, m_controllerOutput() won't run if
+  // Controller.update() is already running at that time.
+  private final ReentrantLock m_outputMutex = new ReentrantLock();
+
+  /**
+   * Allocates a PIDControllerRunner.
+   *
+   * @param controller        The controller on which to call update().
+   * @param measurementSource The function that supplies the current process variable measurement.
+   * @param controllerOutput  The function which updates the plant using the controller output
+   *                          passed as the argument.
+   */
+  public PIDControllerRunner(PIDController controller, DoubleSupplier measurementSource,
+                          DoubleConsumer controllerOutput) {
+    m_controller = controller;
+    m_controllerOutput = controllerOutput;
+    m_measurementSource = measurementSource;
+    m_notifier.startPeriodic(m_controller.getPeriod());
+  }
+
+  /**
+   * Begins running the controller.
+   */
+  public void enable() {
+    m_thisMutex.lock();
+    try {
+      m_enabled = true;
+    } finally {
+      m_thisMutex.unlock();
+    }
+  }
+
+  /**
+   * Stops running the controller.
+   *
+   * <p>This sets the output to zero before stopping.
+   */
+  public void disable() {
+    // Ensures m_enabled modification and m_controllerOutput() call occur
+    // atomically
+    m_outputMutex.lock();
+    try {
+      m_thisMutex.lock();
+      try {
+        m_enabled = false;
+      } finally {
+        m_thisMutex.unlock();
+      }
+
+      m_controllerOutput.accept(0.0);
+    } finally {
+      m_outputMutex.unlock();
+    }
+  }
+
+  /**
+   * Returns whether controller is running.
+   */
+  public boolean isEnabled() {
+    m_thisMutex.lock();
+    try {
+      return m_enabled;
+    } finally {
+      m_thisMutex.unlock();
+    }
+  }
+
+
+  private void run() {
+    // Ensures m_enabled check and m_controllerOutput() call occur atomically
+    m_outputMutex.lock();
+    try {
+      m_thisMutex.lock();
+      try {
+        if (m_enabled) {
+          // Don't block other PIDControllerRunner operations on output
+          m_thisMutex.unlock();
+
+          m_controllerOutput.accept(m_controller.calculate(m_measurementSource.getAsDouble()));
+        }
+      } finally {
+        if (m_thisMutex.isHeldByCurrentThread()) {
+          m_thisMutex.unlock();
+        }
+      }
+    } finally {
+      m_outputMutex.unlock();
+    }
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    m_controller.initSendable(builder);
+    builder.setSafeState(this::disable);
+    builder.addBooleanProperty("enabled", this::isEnabled, enabled -> {
+      if (enabled) {
+        enable();
+      } else {
+        disable();
+      }
+    });
+  }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/PIDInputOutputTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/PIDInputOutputTest.java
new file mode 100644
index 0000000000..6c73553aad
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/PIDInputOutputTest.java
@@ -0,0 +1,87 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 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;
+
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.controller.PIDController;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class PIDInputOutputTest {
+  private PIDController m_controller;
+
+  @BeforeEach
+  void setUp() {
+    m_controller = new PIDController(0, 0, 0);
+  }
+
+  @Test
+  void outputRangeTest() {
+    m_controller.setP(1);
+    m_controller.setOutputRange(-50, 50);
+
+    assertAll(
+        () -> assertEquals(-50, m_controller.calculate(100, 0), 1e-5),
+        () -> assertEquals(50, m_controller.calculate(0, 100), 1e-5)
+    );
+  }
+
+  @Test
+  void inputRangeTest() {
+    m_controller.setP(1);
+    m_controller.setOutputRange(-1000, 1000);
+    m_controller.setInputRange(-50, 50);
+
+    assertAll(
+        () -> assertEquals(-100, m_controller.calculate(100, 0), 1e-5),
+        () -> assertEquals(50, m_controller.calculate(0, 100), 1e-5)
+    );
+  }
+
+  @Test
+  void continuousInputTest() {
+    m_controller.setP(1);
+    m_controller.setInputRange(-180, 180);
+    m_controller.setContinuous(true);
+
+    assertTrue(m_controller.calculate(-179, 179) < 0.0);
+  }
+
+  @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(.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);
+  }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/PIDToleranceTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/PIDToleranceTest.java
index cbf3f99531..838aca4abc 100644
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/PIDToleranceTest.java
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/PIDToleranceTest.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2019 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.                                                               */
@@ -11,93 +11,92 @@ import org.junit.jupiter.api.AfterEach;
 import org.junit.jupiter.api.BeforeEach;
 import org.junit.jupiter.api.Test;
 
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.controller.PIDControllerRunner;
+
 import static org.junit.jupiter.api.Assertions.assertFalse;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
 class PIDToleranceTest {
-  private PIDController m_pid;
-  private static final double m_setPoint = 50.0;
+  private PIDController m_pidController;
+  private PIDControllerRunner m_pidRunner;
+  private static final double m_setpoint = 50.0;
   private static final double m_tolerance = 10.0;
   private static final double m_range = 200;
 
-  private class FakeInput implements PIDSource {
+  private static class FakeInput {
     public double m_val;
 
     FakeInput() {
       m_val = 0;
     }
 
-    @Override
-    public PIDSourceType getPIDSourceType() {
-      return PIDSourceType.kDisplacement;
-    }
-
-    @Override
-    public double pidGet() {
+    public double getMeasurement() {
       return m_val;
     }
-
-    @Override
-    public void setPIDSourceType(PIDSourceType arg0) {
-    }
   }
 
   private FakeInput m_inp;
-  private final PIDOutput m_out = new PIDOutput() {
-    @Override
-    public void pidWrite(double out) {
-    }
-
-  };
-
 
   @BeforeEach
   void setUp() {
     m_inp = new FakeInput();
-    m_pid = new PIDController(0.05, 0.0, 0.0, m_inp, m_out);
-    m_pid.setInputRange(-m_range / 2, m_range / 2);
+    m_pidController = new PIDController(0.05, 0.0, 0.0);
+    m_pidRunner = new PIDControllerRunner(m_pidController, m_inp::getMeasurement, x -> { });
+    m_pidController.setInputRange(-m_range / 2, m_range / 2);
   }
 
   @AfterEach
   void tearDown() {
-    m_pid.close();
-    m_pid = null;
+    m_pidController.close();
+    m_pidController = null;
+  }
+
+  @Test
+  void initialToleranceTest() {
+    assertFalse(m_pidController.atSetpoint());
   }
 
   @Test
   void absoluteToleranceTest() {
-    m_pid.setAbsoluteTolerance(m_tolerance);
-    m_pid.setSetpoint(m_setPoint);
-    m_pid.enable();
+    m_pidController.setAbsoluteTolerance(m_tolerance);
+    m_pidController.setSetpoint(m_setpoint);
+    m_pidRunner.enable();
     Timer.delay(1);
-    assertFalse(m_pid.onTarget(), "Error was in tolerance when it should not have been. Error was "
-        + m_pid.getError());
-    m_inp.m_val = m_setPoint + m_tolerance / 2;
+    assertFalse(m_pidController.atSetpoint(),
+        "Error was in tolerance when it should not have been. Error was "
+        + m_pidController.getError());
+    m_inp.m_val = m_setpoint + m_tolerance / 2;
     Timer.delay(1.0);
-    assertTrue(m_pid.onTarget(), "Error was not in tolerance when it should have been. Error was "
-        + m_pid.getError());
-    m_inp.m_val = m_setPoint + 10 * m_tolerance;
+    assertTrue(m_pidController.atSetpoint(),
+        "Error was not in tolerance when it should have been. Error was "
+        + m_pidController.getError());
+    m_inp.m_val = m_setpoint + 10 * m_tolerance;
     Timer.delay(1.0);
-    assertFalse(m_pid.onTarget(), "Error was in tolerance when it should not have been. Error was "
-        + m_pid.getError());
+    assertFalse(m_pidController.atSetpoint(),
+        "Error was in tolerance when it should not have been. Error was "
+        + m_pidController.getError());
   }
 
   @Test
   void percentToleranceTest() {
-    m_pid.setPercentTolerance(m_tolerance);
-    m_pid.setSetpoint(m_setPoint);
-    m_pid.enable();
-    assertFalse(m_pid.onTarget(), "Error was in tolerance when it should not have been. Error was "
-        + m_pid.getError());
+    m_pidController.setPercentTolerance(m_tolerance);
+    m_pidController.setSetpoint(m_setpoint);
+    m_pidRunner.enable();
+    assertFalse(m_pidController.atSetpoint(),
+        "Error was in tolerance when it should not have been. Error was "
+        + m_pidController.getError());
     //half of percent tolerance away from setPoint
-    m_inp.m_val = m_setPoint + m_tolerance / 200 * m_range;
+    m_inp.m_val = m_setpoint + m_tolerance / 200 * m_range;
     Timer.delay(1.0);
-    assertTrue(m_pid.onTarget(), "Error was not in tolerance when it should have been. Error was "
-        + m_pid.getError());
+    assertTrue(m_pidController.atSetpoint(),
+        "Error was not in tolerance when it should have been. Error was "
+        + m_pidController.getError());
     //double percent tolerance away from setPoint
-    m_inp.m_val = m_setPoint + m_tolerance / 50 * m_range;
+    m_inp.m_val = m_setpoint + m_tolerance / 50 * m_range;
     Timer.delay(1.0);
-    assertFalse(m_pid.onTarget(), "Error was in tolerance when it should not have been. Error was "
-        + m_pid.getError());
+    assertFalse(m_pidController.atSetpoint(),
+        "Error was in tolerance when it should not have been. Error was "
+        + m_pidController.getError());
   }
 }
diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java
index 59400c7c15..4f63d94178 100644
--- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java
+++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2019 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.                                                               */
@@ -19,6 +19,8 @@ import org.junit.runner.RunWith;
 import org.junit.runners.Parameterized;
 import org.junit.runners.Parameterized.Parameters;
 
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.controller.PIDControllerRunner;
 import edu.wpi.first.wpilibj.filters.LinearDigitalFilter;
 import edu.wpi.first.wpilibj.fixtures.MotorEncoderFixture;
 import edu.wpi.first.wpilibj.test.AbstractComsSetup;
@@ -175,42 +177,44 @@ public class MotorEncoderTest extends AbstractComsSetup {
 
   @Test
   public void testPositionPIDController() {
-    me.getEncoder().setPIDSourceType(PIDSourceType.kDisplacement);
-    PIDController pid = new PIDController(0.001, 0.0005, 0, me.getEncoder(), me.getMotor());
-    pid.setAbsoluteTolerance(50.0);
-    pid.setOutputRange(-0.2, 0.2);
-    pid.setSetpoint(1000);
+    PIDController pidController = new PIDController(0.001, 0.0005, 0);
+    pidController.setAbsoluteTolerance(50.0);
+    pidController.setOutputRange(-0.2, 0.2);
+    pidController.setSetpoint(1000);
 
-    pid.enable();
+    PIDControllerRunner pidRunner = new PIDControllerRunner(pidController,
+        me.getEncoder()::getDistance, output -> me.getMotor().set(output));
+    pidRunner.enable();
     Timer.delay(10.0);
-    pid.disable();
+    pidRunner.disable();
 
     assertTrue(
-        "PID loop did not reach setpoint within 10 seconds. The current error was" + pid
-            .getError(), pid.onTarget());
+        "PID loop did not reach reference within 10 seconds. The current error was" + pidController
+            .getError(), pidController.atSetpoint());
 
-    pid.close();
+    pidController.close();
   }
 
   @Test
   public void testVelocityPIDController() {
     me.getEncoder().setPIDSourceType(PIDSourceType.kRate);
     LinearDigitalFilter filter = LinearDigitalFilter.movingAverage(me.getEncoder(), 50);
-    PIDController pid =
-        new PIDController(1e-5, 0.0, 3e-5, 8e-5, filter, me.getMotor());
-    pid.setAbsoluteTolerance(200);
-    pid.setOutputRange(-0.3, 0.3);
-    pid.setSetpoint(600);
+    PIDController pidController = new PIDController(1e-5, 0.0, 0.0006);
+    pidController.setAbsoluteTolerance(200);
+    pidController.setOutputRange(-0.3, 0.3);
+    pidController.setSetpoint(600);
 
-    pid.enable();
+    PIDControllerRunner pidRunner = new PIDControllerRunner(pidController, filter::pidGet,
+        output -> me.getMotor().set(output + 8e-5));
+    pidRunner.enable();
     Timer.delay(10.0);
-    pid.disable();
+    pidRunner.disable();
 
     assertTrue(
-        "PID loop did not reach setpoint within 10 seconds. The error was: " + pid.getError(),
-        pid.onTarget());
+        "PID loop did not reach reference within 10 seconds. The error was: "
+        + pidController.getError(), pidController.atSetpoint());
 
-    pid.close();
+    pidController.close();
   }
 
   /**
diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PIDTest.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PIDTest.java
index ad05703306..f5805f6b14 100644
--- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PIDTest.java
+++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PIDTest.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2019 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,6 +23,8 @@ import org.junit.runners.Parameterized.Parameters;
 
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.controller.PIDControllerRunner;
 import edu.wpi.first.wpilibj.fixtures.MotorEncoderFixture;
 import edu.wpi.first.wpilibj.smartdashboard.SendableBuilderImpl;
 import edu.wpi.first.wpilibj.test.AbstractComsSetup;
@@ -50,6 +52,7 @@ public class PIDTest extends AbstractComsSetup {
   private static final double outputRange = 0.25;
 
   private PIDController m_controller = null;
+  private PIDControllerRunner m_runner = null;
   private static MotorEncoderFixture me = null;
 
   @SuppressWarnings({"MemberName", "EmptyLineSeparator", "MultipleVariableDeclarations"})
@@ -108,16 +111,19 @@ public class PIDTest extends AbstractComsSetup {
     m_table = NetworkTableInstance.getDefault().getTable("TEST_PID");
     m_builder = new SendableBuilderImpl();
     m_builder.setTable(m_table);
-    m_controller = new PIDController(k_p, k_i, k_d, me.getEncoder(), me.getMotor());
+    m_controller = new PIDController(k_p, k_i, k_d);
+    m_runner = new PIDControllerRunner(m_controller,
+        me.getEncoder()::getDistance, me.getMotor()::set);
     m_controller.initSendable(m_builder);
   }
 
   @After
   public void tearDown() throws Exception {
     logger.fine("Teardown: " + me.getType());
-    m_controller.disable();
+    m_runner.disable();
     m_controller.close();
     m_controller = null;
+    m_runner = null;
     me.reset();
   }
 
@@ -133,16 +139,16 @@ public class PIDTest extends AbstractComsSetup {
   public void testInitialSettings() {
     setupAbsoluteTolerance();
     setupOutputRange();
-    double setpoint = 2500.0;
-    m_controller.setSetpoint(setpoint);
-    assertFalse("PID did not begin disabled", m_controller.isEnabled());
-    assertEquals("PID.getError() did not start at " + setpoint, setpoint,
+    double reference = 2500.0;
+    m_controller.setSetpoint(reference);
+    assertFalse("PID did not begin disabled", m_runner.isEnabled());
+    assertEquals("PID.getError() did not start at " + reference, reference,
         m_controller.getError(), 0);
     m_builder.updateTable();
-    assertEquals(k_p, m_table.getEntry("p").getDouble(9999999), 0);
-    assertEquals(k_i, m_table.getEntry("i").getDouble(9999999), 0);
-    assertEquals(k_d, m_table.getEntry("d").getDouble(9999999), 0);
-    assertEquals(setpoint, m_table.getEntry("setpoint").getDouble(9999999), 0);
+    assertEquals(k_p, m_table.getEntry("Kp").getDouble(9999999), 0);
+    assertEquals(k_i, m_table.getEntry("Ki").getDouble(9999999), 0);
+    assertEquals(k_d, m_table.getEntry("Kd").getDouble(9999999), 0);
+    assertEquals(reference, m_table.getEntry("reference").getDouble(9999999), 0);
     assertFalse(m_table.getEntry("enabled").getBoolean(true));
   }
 
@@ -150,17 +156,17 @@ public class PIDTest extends AbstractComsSetup {
   public void testRestartAfterEnable() {
     setupAbsoluteTolerance();
     setupOutputRange();
-    double setpoint = 2500.0;
-    m_controller.setSetpoint(setpoint);
-    m_controller.enable();
+    double reference = 2500.0;
+    m_controller.setSetpoint(reference);
+    m_runner.enable();
     m_builder.updateTable();
     assertTrue(m_table.getEntry("enabled").getBoolean(false));
-    assertTrue(m_controller.isEnabled());
+    assertTrue(m_runner.isEnabled());
     assertThat(0.0, is(not(me.getMotor().get())));
     m_controller.reset();
     m_builder.updateTable();
     assertFalse(m_table.getEntry("enabled").getBoolean(true));
-    assertFalse(m_controller.isEnabled());
+    assertFalse(m_runner.isEnabled());
     assertEquals(0, me.getMotor().get(), 0);
   }
 
@@ -168,11 +174,11 @@ public class PIDTest extends AbstractComsSetup {
   public void testSetSetpoint() {
     setupAbsoluteTolerance();
     setupOutputRange();
-    Double setpoint = 2500.0;
-    m_controller.disable();
-    m_controller.setSetpoint(setpoint);
-    m_controller.enable();
-    assertEquals("Did not correctly set set-point", setpoint, new Double(m_controller
+    Double reference = 2500.0;
+    m_runner.disable();
+    m_controller.setSetpoint(reference);
+    m_runner.enable();
+    assertEquals("Did not correctly set reference", reference, new Double(m_controller
         .getSetpoint()));
   }
 
@@ -180,16 +186,16 @@ public class PIDTest extends AbstractComsSetup {
   public void testRotateToTarget() {
     setupAbsoluteTolerance();
     setupOutputRange();
-    double setpoint = 1000.0;
-    assertEquals(pidData() + "did not start at 0", 0, m_controller.get(), 0);
-    m_controller.setSetpoint(setpoint);
-    assertEquals(pidData() + "did not have an error of " + setpoint, setpoint,
+    double reference = 1000.0;
+    assertEquals(pidData() + "did not start at 0", 0, m_controller.getOutput(), 0);
+    m_controller.setSetpoint(reference);
+    assertEquals(pidData() + "did not have an error of " + reference, reference,
         m_controller.getError(), 0);
-    m_controller.enable();
+    m_runner.enable();
     Timer.delay(5);
-    m_controller.disable();
+    m_runner.disable();
     assertTrue(pidData() + "Was not on Target. Controller Error: " + m_controller.getError(),
-        m_controller.onTarget());
+        m_controller.atSetpoint());
   }
 
   private String pidData() {
@@ -201,6 +207,6 @@ public class PIDTest extends AbstractComsSetup {
   @Test(expected = RuntimeException.class)
   public void testOnTargetNoToleranceSet() {
     setupOutputRange();
-    m_controller.onTarget();
+    m_controller.atSetpoint();
   }
 }