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.

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;
 

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);
 

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

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