[wpilib] Remove PIDController, PIDOutput, PIDSource

Move them to the old commands vendordep so that PIDCommand and PIDSubsystem
continue to work.

This also removes Filter and LinearDigitalFilter.

wpilibOldCommands/src/main/native/cpp/PIDBase.cpp

@@ -0,0 +1,358 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/PIDBase.h"
+
+#include <algorithm>
+#include <cmath>
+
+#include <hal/FRCUsageReporting.h>
+
+#include "frc/PIDOutput.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+template <class T>
+constexpr const T& clamp(const T& value, const T& low, const T& high) {
+  return std::max(low, std::min(value, high));
+}
+
+PIDBase::PIDBase(double Kp, double Ki, double Kd, PIDSource& source,
+                 PIDOutput& output)
+    : PIDBase(Kp, Ki, Kd, 0.0, source, output) {}
+
+PIDBase::PIDBase(double Kp, double Ki, double Kd, double Kf, PIDSource& source,
+                 PIDOutput& output) {
+  m_P = Kp;
+  m_I = Ki;
+  m_D = Kd;
+  m_F = Kf;
+
+  m_pidInput = &source;
+  m_filter = LinearFilter<double>::MovingAverage(1);
+
+  m_pidOutput = &output;
+
+  m_setpointTimer.Start();
+
+  static int instances = 0;
+  instances++;
+  HAL_Report(HALUsageReporting::kResourceType_PIDController, instances);
+  SendableRegistry::GetInstance().Add(this, "PIDController", instances);
+}
+
+double PIDBase::Get() const {
+  std::scoped_lock lock(m_thisMutex);
+  return m_result;
+}
+
+void PIDBase::SetContinuous(bool continuous) {
+  std::scoped_lock lock(m_thisMutex);
+  m_continuous = continuous;
+}
+
+void PIDBase::SetInputRange(double minimumInput, double maximumInput) {
+  {
+    std::scoped_lock lock(m_thisMutex);
+    m_minimumInput = minimumInput;
+    m_maximumInput = maximumInput;
+    m_inputRange = maximumInput - minimumInput;
+  }
+
+  SetSetpoint(m_setpoint);
+}
+
+void PIDBase::SetOutputRange(double minimumOutput, double maximumOutput) {
+  std::scoped_lock lock(m_thisMutex);
+  m_minimumOutput = minimumOutput;
+  m_maximumOutput = maximumOutput;
+}
+
+void PIDBase::SetPID(double p, double i, double d) {
+  {
+    std::scoped_lock lock(m_thisMutex);
+    m_P = p;
+    m_I = i;
+    m_D = d;
+  }
+}
+
+void PIDBase::SetPID(double p, double i, double d, double f) {
+  std::scoped_lock lock(m_thisMutex);
+  m_P = p;
+  m_I = i;
+  m_D = d;
+  m_F = f;
+}
+
+void PIDBase::SetP(double p) {
+  std::scoped_lock lock(m_thisMutex);
+  m_P = p;
+}
+
+void PIDBase::SetI(double i) {
+  std::scoped_lock lock(m_thisMutex);
+  m_I = i;
+}
+
+void PIDBase::SetD(double d) {
+  std::scoped_lock lock(m_thisMutex);
+  m_D = d;
+}
+
+void PIDBase::SetF(double f) {
+  std::scoped_lock lock(m_thisMutex);
+  m_F = f;
+}
+
+double PIDBase::GetP() const {
+  std::scoped_lock lock(m_thisMutex);
+  return m_P;
+}
+
+double PIDBase::GetI() const {
+  std::scoped_lock lock(m_thisMutex);
+  return m_I;
+}
+
+double PIDBase::GetD() const {
+  std::scoped_lock lock(m_thisMutex);
+  return m_D;
+}
+
+double PIDBase::GetF() const {
+  std::scoped_lock lock(m_thisMutex);
+  return m_F;
+}
+
+void PIDBase::SetSetpoint(double setpoint) {
+  {
+    std::scoped_lock lock(m_thisMutex);
+
+    if (m_maximumInput > m_minimumInput) {
+      if (setpoint > m_maximumInput) {
+        m_setpoint = m_maximumInput;
+      } else if (setpoint < m_minimumInput) {
+        m_setpoint = m_minimumInput;
+      } else {
+        m_setpoint = setpoint;
+      }
+    } else {
+      m_setpoint = setpoint;
+    }
+  }
+}
+
+double PIDBase::GetSetpoint() const {
+  std::scoped_lock lock(m_thisMutex);
+  return m_setpoint;
+}
+
+double PIDBase::GetDeltaSetpoint() const {
+  std::scoped_lock lock(m_thisMutex);
+  return (m_setpoint - m_prevSetpoint) / m_setpointTimer.Get();
+}
+
+double PIDBase::GetError() const {
+  double setpoint = GetSetpoint();
+  {
+    std::scoped_lock lock(m_thisMutex);
+    return GetContinuousError(setpoint - m_pidInput->PIDGet());
+  }
+}
+
+double PIDBase::GetAvgError() const {
+  return GetError();
+}
+
+void PIDBase::SetPIDSourceType(PIDSourceType pidSource) {
+  m_pidInput->SetPIDSourceType(pidSource);
+}
+
+PIDSourceType PIDBase::GetPIDSourceType() const {
+  return m_pidInput->GetPIDSourceType();
+}
+
+void PIDBase::SetTolerance(double percent) {
+  std::scoped_lock lock(m_thisMutex);
+  m_toleranceType = kPercentTolerance;
+  m_tolerance = percent;
+}
+
+void PIDBase::SetAbsoluteTolerance(double absTolerance) {
+  std::scoped_lock lock(m_thisMutex);
+  m_toleranceType = kAbsoluteTolerance;
+  m_tolerance = absTolerance;
+}
+
+void PIDBase::SetPercentTolerance(double percent) {
+  std::scoped_lock lock(m_thisMutex);
+  m_toleranceType = kPercentTolerance;
+  m_tolerance = percent;
+}
+
+void PIDBase::SetToleranceBuffer(int bufLength) {
+  std::scoped_lock lock(m_thisMutex);
+  m_filter = LinearFilter<double>::MovingAverage(bufLength);
+}
+
+bool PIDBase::OnTarget() const {
+  double error = GetError();
+
+  std::scoped_lock lock(m_thisMutex);
+  switch (m_toleranceType) {
+    case kPercentTolerance:
+      return std::fabs(error) < m_tolerance / 100 * m_inputRange;
+      break;
+    case kAbsoluteTolerance:
+      return std::fabs(error) < m_tolerance;
+      break;
+    case kNoTolerance:
+      // TODO: this case needs an error
+      return false;
+  }
+  return false;
+}
+
+void PIDBase::Reset() {
+  std::scoped_lock lock(m_thisMutex);
+  m_prevError = 0;
+  m_totalError = 0;
+  m_result = 0;
+}
+
+void PIDBase::PIDWrite(double output) {
+  SetSetpoint(output);
+}
+
+void PIDBase::InitSendable(SendableBuilder& builder) {
+  builder.SetSmartDashboardType("PIDController");
+  builder.SetSafeState([=]() { Reset(); });
+  builder.AddDoubleProperty(
+      "p", [=]() { return GetP(); }, [=](double value) { SetP(value); });
+  builder.AddDoubleProperty(
+      "i", [=]() { return GetI(); }, [=](double value) { SetI(value); });
+  builder.AddDoubleProperty(
+      "d", [=]() { return GetD(); }, [=](double value) { SetD(value); });
+  builder.AddDoubleProperty(
+      "f", [=]() { return GetF(); }, [=](double value) { SetF(value); });
+  builder.AddDoubleProperty(
+      "setpoint", [=]() { return GetSetpoint(); },
+      [=](double value) { SetSetpoint(value); });
+}
+
+void PIDBase::Calculate() {
+  if (m_pidInput == nullptr || m_pidOutput == nullptr) {
+    return;
+  }
+
+  bool enabled;
+  {
+    std::scoped_lock lock(m_thisMutex);
+    enabled = m_enabled;
+  }
+
+  if (enabled) {
+    double input;
+
+    // Storage for function inputs
+    PIDSourceType pidSourceType;
+    double P;
+    double I;
+    double D;
+    double feedForward = CalculateFeedForward();
+    double minimumOutput;
+    double maximumOutput;
+
+    // Storage for function input-outputs
+    double prevError;
+    double error;
+    double totalError;
+
+    {
+      std::scoped_lock lock(m_thisMutex);
+
+      input = m_filter.Calculate(m_pidInput->PIDGet());
+
+      pidSourceType = m_pidInput->GetPIDSourceType();
+      P = m_P;
+      I = m_I;
+      D = m_D;
+      minimumOutput = m_minimumOutput;
+      maximumOutput = m_maximumOutput;
+
+      prevError = m_prevError;
+      error = GetContinuousError(m_setpoint - input);
+      totalError = m_totalError;
+    }
+
+    // Storage for function outputs
+    double result;
+
+    if (pidSourceType == PIDSourceType::kRate) {
+      if (P != 0) {
+        totalError =
+            clamp(totalError + error, minimumOutput / P, maximumOutput / P);
+      }
+
+      result = D * error + P * totalError + feedForward;
+    } else {
+      if (I != 0) {
+        totalError =
+            clamp(totalError + error, minimumOutput / I, maximumOutput / I);
+      }
+
+      result =
+          P * error + I * totalError + D * (error - prevError) + feedForward;
+    }
+
+    result = clamp(result, minimumOutput, maximumOutput);
+
+    {
+      // Ensures m_enabled check and PIDWrite() call occur atomically
+      std::scoped_lock pidWriteLock(m_pidWriteMutex);
+      std::unique_lock mainLock(m_thisMutex);
+      if (m_enabled) {
+        // Don't block other PIDBase operations on PIDWrite()
+        mainLock.unlock();
+
+        m_pidOutput->PIDWrite(result);
+      }
+    }
+
+    std::scoped_lock lock(m_thisMutex);
+    m_prevError = m_error;
+    m_error = error;
+    m_totalError = totalError;
+    m_result = result;
+  }
+}
+
+double PIDBase::CalculateFeedForward() {
+  if (m_pidInput->GetPIDSourceType() == PIDSourceType::kRate) {
+    return m_F * GetSetpoint();
+  } else {
+    double temp = m_F * GetDeltaSetpoint();
+    m_prevSetpoint = m_setpoint;
+    m_setpointTimer.Reset();
+    return temp;
+  }
+}
+
+double PIDBase::GetContinuousError(double error) const {
+  if (m_continuous && m_inputRange != 0) {
+    error = std::fmod(error, m_inputRange);
+    if (std::fabs(error) > m_inputRange / 2) {
+      if (error > 0) {
+        return error - m_inputRange;
+      } else {
+        return error + m_inputRange;
+      }
+    }
+  }
+
+  return error;
+}

wpilibOldCommands/src/main/native/cpp/PIDController.cpp

@@ -0,0 +1,83 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/PIDController.h"
+
+#include "frc/Notifier.h"
+#include "frc/PIDOutput.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+
+using namespace frc;
+
+PIDController::PIDController(double Kp, double Ki, double Kd, PIDSource* source,
+                             PIDOutput* output, double period)
+    : PIDController(Kp, Ki, Kd, 0.0, *source, *output, period) {}
+
+PIDController::PIDController(double Kp, double Ki, double Kd, double Kf,
+                             PIDSource* source, PIDOutput* output,
+                             double period)
+    : PIDController(Kp, Ki, Kd, Kf, *source, *output, period) {}
+
+PIDController::PIDController(double Kp, double Ki, double Kd, PIDSource& source,
+                             PIDOutput& output, double period)
+    : PIDController(Kp, Ki, Kd, 0.0, source, output, period) {}
+
+PIDController::PIDController(double Kp, double Ki, double Kd, double Kf,
+                             PIDSource& source, PIDOutput& output,
+                             double period)
+    : PIDBase(Kp, Ki, Kd, Kf, source, output) {
+  m_controlLoop = std::make_unique<Notifier>(&PIDController::Calculate, this);
+  m_controlLoop->StartPeriodic(units::second_t(period));
+}
+
+PIDController::~PIDController() {
+  // Forcefully stopping the notifier so the callback can successfully run.
+  m_controlLoop->Stop();
+}
+
+void PIDController::Enable() {
+  {
+    std::scoped_lock lock(m_thisMutex);
+    m_enabled = true;
+  }
+}
+
+void PIDController::Disable() {
+  {
+    // Ensures m_enabled modification and PIDWrite() call occur atomically
+    std::scoped_lock pidWriteLock(m_pidWriteMutex);
+    {
+      std::scoped_lock mainLock(m_thisMutex);
+      m_enabled = false;
+    }
+
+    m_pidOutput->PIDWrite(0);
+  }
+}
+
+void PIDController::SetEnabled(bool enable) {
+  if (enable) {
+    Enable();
+  } else {
+    Disable();
+  }
+}
+
+bool PIDController::IsEnabled() const {
+  std::scoped_lock lock(m_thisMutex);
+  return m_enabled;
+}
+
+void PIDController::Reset() {
+  Disable();
+
+  PIDBase::Reset();
+}
+
+void PIDController::InitSendable(SendableBuilder& builder) {
+  PIDBase::InitSendable(builder);
+  builder.AddBooleanProperty(
+      "enabled", [=]() { return IsEnabled(); },
+      [=](bool value) { SetEnabled(value); });
+}

wpilibOldCommands/src/main/native/cpp/PIDSource.cpp

@@ -0,0 +1,15 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/PIDSource.h"
+
+using namespace frc;
+
+void PIDSource::SetPIDSourceType(PIDSourceType pidSource) {
+  m_pidSource = pidSource;
+}
+
+PIDSourceType PIDSource::GetPIDSourceType() const {
+  return m_pidSource;
+}

wpilibOldCommands/src/main/native/include/frc/PIDBase.h

@@ -0,0 +1,407 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+#include <string>
+
+#include <wpi/deprecated.h>
+#include <wpi/mutex.h>
+
+#include "frc/Base.h"
+#include "frc/LinearFilter.h"
+#include "frc/PIDInterface.h"
+#include "frc/PIDOutput.h"
+#include "frc/PIDSource.h"
+#include "frc/Timer.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
+
+namespace frc {
+
+class SendableBuilder;
+
+/**
+ * Class implements a PID Control Loop.
+ *
+ * Creates a separate thread which reads the given PIDSource and takes care of
+ * the integral calculations, as well as writing the given PIDOutput.
+ *
+ * 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 All APIs which use this have been deprecated.
+ */
+class PIDBase : public PIDInterface,
+                public PIDOutput,
+                public Sendable,
+                public SendableHelper<PIDBase> {
+ public:
+  /**
+   * Allocate a PID object with the given constants for P, I, D.
+   *
+   * @param Kp     the proportional coefficient
+   * @param Ki     the integral coefficient
+   * @param Kd     the derivative coefficient
+   * @param source The PIDSource object that is used to get values
+   * @param output The PIDOutput object that is set to the output value
+   */
+  WPI_DEPRECATED("All APIs which use this have been deprecated.")
+  PIDBase(double p, double i, double d, PIDSource& source, PIDOutput& output);
+
+  /**
+   * Allocate a PID object with the given constants for P, I, D.
+   *
+   * @param Kp     the proportional coefficient
+   * @param Ki     the integral coefficient
+   * @param Kd     the derivative coefficient
+   * @param source The PIDSource object that is used to get values
+   * @param output The PIDOutput object that is set to the output value
+   */
+  WPI_DEPRECATED("All APIs which use this have been deprecated.")
+  PIDBase(double p, double i, double d, double f, PIDSource& source,
+          PIDOutput& output);
+
+  ~PIDBase() override = default;
+
+  /**
+   * Return the current PID result.
+   *
+   * This is always centered on zero and constrained the the max and min outs.
+   *
+   * @return the latest calculated output
+   */
+  virtual double Get() const;
+
+  /**
+   * Set 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
+   */
+  virtual void SetContinuous(bool continuous = true);
+
+  /**
+   * Sets the maximum and minimum values expected from the input.
+   *
+   * @param minimumInput the minimum value expected from the input
+   * @param maximumInput the maximum value expected from the output
+   */
+  virtual 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
+   */
+  virtual void SetOutputRange(double minimumOutput, double maximumOutput);
+
+  /**
+   * Set the PID Controller gain parameters.
+   *
+   * Set the proportional, integral, and differential coefficients.
+   *
+   * @param p Proportional coefficient
+   * @param i Integral coefficient
+   * @param d Differential coefficient
+   */
+  void SetPID(double p, double i, double d) override;
+
+  /**
+   * Set the PID Controller gain parameters.
+   *
+   * Set the proportional, integral, and differential coefficients.
+   *
+   * @param p Proportional coefficient
+   * @param i Integral coefficient
+   * @param d Differential coefficient
+   * @param f Feed forward coefficient
+   */
+  virtual void SetPID(double p, double i, double d, double f);
+
+  /**
+   * Set the Proportional coefficient of the PID controller gain.
+   *
+   * @param p proportional coefficient
+   */
+  void SetP(double p);
+
+  /**
+   * Set the Integral coefficient of the PID controller gain.
+   *
+   * @param i integral coefficient
+   */
+  void SetI(double i);
+
+  /**
+   * Set the Differential coefficient of the PID controller gain.
+   *
+   * @param d differential coefficient
+   */
+  void SetD(double d);
+
+  /**
+   * Get the Feed forward coefficient of the PID controller gain.
+   *
+   * @param f Feed forward coefficient
+   */
+  void SetF(double f);
+
+  /**
+   * Get the Proportional coefficient.
+   *
+   * @return proportional coefficient
+   */
+  double GetP() const override;
+
+  /**
+   * Get the Integral coefficient.
+   *
+   * @return integral coefficient
+   */
+  double GetI() const override;
+
+  /**
+   * Get the Differential coefficient.
+   *
+   * @return differential coefficient
+   */
+  double GetD() const override;
+
+  /**
+   * Get the Feed forward coefficient.
+   *
+   * @return Feed forward coefficient
+   */
+  virtual double GetF() const;
+
+  /**
+   * Set the setpoint for the PIDBase.
+   *
+   * @param setpoint the desired setpoint
+   */
+  void SetSetpoint(double setpoint) override;
+
+  /**
+   * Returns the current setpoint of the PIDBase.
+   *
+   * @return the current setpoint
+   */
+  double GetSetpoint() const override;
+
+  /**
+   * Returns the change in setpoint over time of the PIDBase.
+   *
+   * @return the change in setpoint over time
+   */
+  double GetDeltaSetpoint() const;
+
+  /**
+   * Returns the current difference of the input from the setpoint.
+   *
+   * @return the current error
+   */
+  virtual double GetError() const;
+
+  /**
+   * Returns the current average of the error over the past few iterations.
+   *
+   * You can specify the number of iterations to average with
+   * SetToleranceBuffer() (defaults to 1). This is the same value that is used
+   * for OnTarget().
+   *
+   * @return the average error
+   */
+  WPI_DEPRECATED("Use a LinearFilter as the input and GetError().")
+  virtual double GetAvgError() const;
+
+  /**
+   * Sets what type of input the PID controller will use.
+   */
+  virtual void SetPIDSourceType(PIDSourceType pidSource);
+
+  /**
+   * Returns the type of input the PID controller is using.
+   *
+   * @return the PID controller input type
+   */
+  virtual PIDSourceType GetPIDSourceType() const;
+
+  /**
+   * Set the percentage error which is considered tolerable for use with
+   * OnTarget.
+   *
+   * @param percentage error which is tolerable
+   */
+  WPI_DEPRECATED("Use SetPercentTolerance() instead.")
+  virtual void SetTolerance(double percent);
+
+  /**
+   * Set the absolute error which is considered tolerable for use with
+   * OnTarget.
+   *
+   * @param percentage error which is tolerable
+   */
+  virtual void SetAbsoluteTolerance(double absValue);
+
+  /**
+   * Set the percentage error which is considered tolerable for use with
+   * OnTarget.
+   *
+   * @param percentage error which is tolerable
+   */
+  virtual void SetPercentTolerance(double percentValue);
+
+  /**
+   * Set the number of previous error samples to average for tolerancing. When
+   * determining whether a mechanism is on target, the user may want to use a
+   * rolling average of previous measurements instead of a precise position or
+   * velocity. This is useful for noisy sensors which return a few erroneous
+   * measurements when the mechanism is on target. However, the mechanism will
+   * not register as on target for at least the specified bufLength cycles.
+   *
+   * @param bufLength Number of previous cycles to average. Defaults to 1.
+   */
+  WPI_DEPRECATED("Use a LinearDigitalFilter as the input.")
+  virtual void SetToleranceBuffer(int buf = 1);
+
+  /**
+   * Return 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.
+   *
+   * 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.
+   */
+  virtual bool OnTarget() const;
+
+  /**
+   * Reset the previous error, the integral term, and disable the controller.
+   */
+  void Reset() override;
+
+  /**
+   * Passes the output directly to SetSetpoint().
+   *
+   * PIDControllers can be nested by passing a PIDController as another
+   * PIDController's output. In that case, the output of the parent controller
+   * becomes the input (i.e., the reference) of the child.
+   *
+   * It is the caller's responsibility to put the data into a valid form for
+   * SetSetpoint().
+   */
+  void PIDWrite(double output) override;
+
+  void InitSendable(SendableBuilder& builder) override;
+
+ protected:
+  // Is the pid controller enabled
+  bool m_enabled = false;
+
+  mutable wpi::mutex m_thisMutex;
+
+  // Ensures when Disable() is called, PIDWrite() won't run if Calculate()
+  // is already running at that time.
+  mutable wpi::mutex m_pidWriteMutex;
+
+  PIDSource* m_pidInput;
+  PIDOutput* m_pidOutput;
+  Timer m_setpointTimer;
+
+  /**
+   * Read the input, calculate the output accordingly, and write to the output.
+   * This should only be called by the Notifier.
+   */
+  virtual void Calculate();
+
+  /**
+   * Calculate the feed forward term.
+   *
+   * Both of the provided feed forward calculations are velocity feed forwards.
+   * If a different feed forward calculation is desired, the user can override
+   * this function and provide his or her own. This function does no
+   * synchronization because the PIDBase class only calls it in synchronized
+   * code, so be careful if calling it oneself.
+   *
+   * If a velocity PID controller is being used, the F term should be set to 1
+   * over the maximum setpoint for the output. If a position PID controller is
+   * being used, the F term should be set to 1 over the maximum speed for the
+   * output measured in setpoint units per this controller's update period (see
+   * the default period in this class's constructor).
+   */
+  virtual double CalculateFeedForward();
+
+  /**
+   * 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_P;
+
+  // Factor for "integral" control
+  double m_I;
+
+  // Factor for "derivative" control
+  double m_D;
+
+  // Factor for "feed forward" control
+  double m_F;
+
+  // |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 prior error (used to compute velocity)
+  double m_prevError = 0;
+
+  // The sum of the errors for use in the integral calc
+  double m_totalError = 0;
+
+  enum {
+    kAbsoluteTolerance,
+    kPercentTolerance,
+    kNoTolerance
+  } m_toleranceType = kNoTolerance;
+
+  // The percetage or absolute error that is considered on target.
+  double m_tolerance = 0.05;
+
+  double m_setpoint = 0;
+  double m_prevSetpoint = 0;
+  double m_error = 0;
+  double m_result = 0;
+
+  LinearFilter<double> m_filter{{}, {}};
+};
+
+}  // namespace frc

wpilibOldCommands/src/main/native/include/frc/PIDController.h

@@ -0,0 +1,136 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+#include <string>
+
+#include <wpi/deprecated.h>
+#include <wpi/mutex.h>
+
+#include "frc/Base.h"
+#include "frc/Controller.h"
+#include "frc/Notifier.h"
+#include "frc/PIDBase.h"
+#include "frc/PIDSource.h"
+#include "frc/Timer.h"
+
+namespace frc {
+
+class PIDOutput;
+
+/**
+ * Class implements a PID Control Loop.
+ *
+ * Creates a separate thread which reads the given PIDSource and takes care of
+ * the integral calculations, as well as writing the given PIDOutput.
+ *
+ * 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 frc2::PIDController class instead.
+ */
+class PIDController : public PIDBase, public Controller {
+ public:
+  /**
+   * Allocate a PID object with the given constants for P, I, D.
+   *
+   * @param Kp     the proportional coefficient
+   * @param Ki     the integral coefficient
+   * @param Kd     the derivative coefficient
+   * @param source The PIDSource object that is used to get values
+   * @param output The PIDOutput object that is set to the output value
+   * @param period the loop time for doing calculations in seconds. This
+   *               particularly affects calculations of the integral and
+   *               differential 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);
+
+  /**
+   * Allocate a PID object with the given constants for P, I, D.
+   *
+   * @param Kp     the proportional coefficient
+   * @param Ki     the integral coefficient
+   * @param Kd     the derivative coefficient
+   * @param source The PIDSource object that is used to get values
+   * @param output The PIDOutput object that is set to the output value
+   * @param period the loop time for doing calculations in seconds. This
+   *               particularly affects calculations of the integral and
+   *               differential 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);
+
+  /**
+   * Allocate a PID object with the given constants for P, I, D.
+   *
+   * @param Kp     the proportional coefficient
+   * @param Ki     the integral coefficient
+   * @param Kd     the derivative coefficient
+   * @param source The PIDSource object that is used to get values
+   * @param output The PIDOutput object that is set to the output value
+   * @param period the loop time for doing calculations in seconds. This
+   *               particularly affects calculations of the integral and
+   *               differential 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);
+
+  /**
+   * Allocate a PID object with the given constants for P, I, D.
+   *
+   * @param Kp     the proportional coefficient
+   * @param Ki     the integral coefficient
+   * @param Kd     the derivative coefficient
+   * @param source The PIDSource object that is used to get values
+   * @param output The PIDOutput object that is set to the output value
+   * @param period the loop time for doing calculations in seconds. This
+   *               particularly affects calculations of the integral and
+   *               differential 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);
+
+  ~PIDController() override;
+
+  /**
+   * Begin running the PIDController.
+   */
+  void Enable() override;
+
+  /**
+   * Stop running the PIDController, this sets the output to zero before
+   * stopping.
+   */
+  void Disable() override;
+
+  /**
+   * Set the enabled state of the PIDController.
+   */
+  void SetEnabled(bool enable);
+
+  /**
+   * Return true if PIDController is enabled.
+   */
+  bool IsEnabled() const;
+
+  /**
+   * Reset the previous error, the integral term, and disable the controller.
+   */
+  void Reset() override;
+
+  void InitSendable(SendableBuilder& builder) override;
+
+ private:
+  std::unique_ptr<Notifier> m_controlLoop;
+};
+
+}  // namespace frc

wpilibOldCommands/src/main/native/include/frc/PIDInterface.h

@@ -0,0 +1,34 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/deprecated.h>
+
+namespace frc {
+
+/**
+ * Interface for PID Control Loop.
+ *
+ * @deprecated All APIs which use this have been deprecated.
+ */
+class PIDInterface {
+ public:
+  WPI_DEPRECATED("All APIs which use this have been deprecated.")
+  PIDInterface() = default;
+  PIDInterface(PIDInterface&&) = default;
+  PIDInterface& operator=(PIDInterface&&) = default;
+
+  virtual void SetPID(double p, double i, double d) = 0;
+  virtual double GetP() const = 0;
+  virtual double GetI() const = 0;
+  virtual double GetD() const = 0;
+
+  virtual void SetSetpoint(double setpoint) = 0;
+  virtual double GetSetpoint() const = 0;
+
+  virtual void Reset() = 0;
+};
+
+}  // namespace frc

wpilibOldCommands/src/main/native/include/frc/PIDOutput.h

@@ -0,0 +1,22 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "frc/Base.h"
+
+namespace frc {
+
+/**
+ * PIDOutput interface is a generic output for the PID class.
+ *
+ * PWMs use this class. Users implement this interface to allow for a
+ * PIDController to read directly from the inputs.
+ */
+class PIDOutput {
+ public:
+  virtual void PIDWrite(double output) = 0;
+};
+
+}  // namespace frc

wpilibOldCommands/src/main/native/include/frc/PIDSource.h

@@ -0,0 +1,36 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+namespace frc {
+
+enum class PIDSourceType { kDisplacement, kRate };
+
+/**
+ * PIDSource interface is a generic sensor source for the PID class.
+ *
+ * All sensors that can be used with the PID class will implement the PIDSource
+ * that returns a standard value that will be used in the PID code.
+ */
+class PIDSource {
+ public:
+  virtual ~PIDSource() = default;
+
+  /**
+   * Set which parameter you are using as a process control variable.
+   *
+   * @param pidSource An enum to select the parameter.
+   */
+  virtual void SetPIDSourceType(PIDSourceType pidSource);
+
+  virtual PIDSourceType GetPIDSourceType() const;
+
+  virtual double PIDGet() = 0;
+
+ protected:
+  PIDSourceType m_pidSource = PIDSourceType::kDisplacement;
+};
+
+}  // namespace frc