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.

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.

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

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.

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.

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

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

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();
+      }
+    });
+  }
+}