Add ProfiledPIDController

wpilibc/src/main/native/cpp/controller/ProfiledPIDController.cpp

@@ -0,0 +1,146 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 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/ProfiledPIDController.h"
+
+#include <algorithm>
+#include <cmath>
+
+#include "frc/smartdashboard/SendableBuilder.h"
+
+using namespace frc;
+
+ProfiledPIDController::ProfiledPIDController(
+    double Kp, double Ki, double Kd,
+    frc::TrapezoidProfile::Constraints constraints, units::second_t period)
+    : m_controller(Kp, Ki, Kd, period), m_constraints(constraints) {}
+
+void ProfiledPIDController::SetP(double Kp) { m_controller.SetP(Kp); }
+
+void ProfiledPIDController::SetI(double Ki) { m_controller.SetI(Ki); }
+
+void ProfiledPIDController::SetD(double Kd) { m_controller.SetD(Kd); }
+
+double ProfiledPIDController::GetP() const { return m_controller.GetP(); }
+
+double ProfiledPIDController::GetI() const { return m_controller.GetI(); }
+
+double ProfiledPIDController::GetD() const { return m_controller.GetD(); }
+
+units::second_t ProfiledPIDController::GetPeriod() const {
+  return m_controller.GetPeriod();
+}
+
+void ProfiledPIDController::SetGoal(units::meter_t goal) {
+  m_goal = {goal, 0_mps};
+}
+
+units::meter_t ProfiledPIDController::GetGoal() const {
+  return m_goal.position;
+}
+
+bool ProfiledPIDController::AtGoal(
+    double positionTolerance, double velocityTolerance,
+    frc2::PIDController::Tolerance toleranceType) const {
+  return AtSetpoint(positionTolerance, velocityTolerance, toleranceType) &&
+         m_goal == m_setpoint;
+}
+
+bool ProfiledPIDController::AtGoal() const {
+  return AtSetpoint() && m_goal == m_setpoint;
+}
+
+void ProfiledPIDController::SetConstraints(
+    frc::TrapezoidProfile::Constraints constraints) {
+  m_constraints = constraints;
+}
+
+double ProfiledPIDController::GetSetpoint() const {
+  return m_controller.GetSetpoint();
+}
+
+bool ProfiledPIDController::AtSetpoint(
+    double positionTolerance, double velocityTolerance,
+    frc2::PIDController::Tolerance toleranceType) const {
+  return m_controller.AtSetpoint(positionTolerance, velocityTolerance,
+                                 toleranceType);
+}
+
+bool ProfiledPIDController::AtSetpoint() const {
+  return m_controller.AtSetpoint();
+}
+
+void ProfiledPIDController::SetInputRange(double minimumInput,
+                                          double maximumInput) {
+  m_controller.SetInputRange(minimumInput, maximumInput);
+}
+
+void ProfiledPIDController::EnableContinuousInput(double minimumInput,
+                                                  double maximumInput) {
+  m_controller.EnableContinuousInput(minimumInput, maximumInput);
+}
+
+void ProfiledPIDController::DisableContinuousInput() {
+  m_controller.DisableContinuousInput();
+}
+
+void ProfiledPIDController::SetOutputRange(double minimumOutput,
+                                           double maximumOutput) {
+  m_controller.SetOutputRange(minimumOutput, maximumOutput);
+}
+
+void ProfiledPIDController::SetAbsoluteTolerance(double positionTolerance,
+                                                 double velocityTolerance) {
+  m_controller.SetAbsoluteTolerance(positionTolerance, velocityTolerance);
+}
+
+void ProfiledPIDController::SetPercentTolerance(double positionTolerance,
+                                                double velocityTolerance) {
+  m_controller.SetPercentTolerance(positionTolerance, velocityTolerance);
+}
+
+double ProfiledPIDController::GetPositionError() const {
+  return m_controller.GetPositionError();
+}
+
+double ProfiledPIDController::GetVelocityError() const {
+  return m_controller.GetVelocityError();
+}
+
+double ProfiledPIDController::Calculate(double measurement) {
+  frc::TrapezoidProfile profile{m_constraints, m_goal, m_setpoint};
+  m_setpoint = profile.Calculate(GetPeriod());
+  return m_controller.Calculate(measurement, m_setpoint.position.to<double>());
+}
+
+double ProfiledPIDController::Calculate(double measurement,
+                                        units::meter_t goal) {
+  SetGoal(goal);
+  return Calculate(measurement);
+}
+
+double ProfiledPIDController::Calculate(
+    double measurement, units::meter_t goal,
+    frc::TrapezoidProfile::Constraints constraints) {
+  SetConstraints(constraints);
+  return Calculate(measurement, goal);
+}
+
+void ProfiledPIDController::Reset() { m_controller.Reset(); }
+
+void ProfiledPIDController::InitSendable(frc::SendableBuilder& builder) {
+  builder.SetSmartDashboardType("ProfiledPIDController");
+  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(
+      "goal", [this] { return GetGoal().to<double>(); },
+      [this](double value) { SetGoal(units::meter_t{value}); });
+}

wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h

@@ -0,0 +1,289 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 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 <units/units.h>
+
+#include "frc/controller/PIDController.h"
+#include "frc/smartdashboard/SendableBase.h"
+#include "frc/trajectory/TrapezoidProfile.h"
+
+namespace frc {
+
+/**
+ * Implements a PID control loop whose setpoint is constrained by a trapezoid
+ * profile.
+ */
+class ProfiledPIDController : public SendableBase {
+ public:
+  /**
+   * Allocates a ProfiledPIDController 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 constraints Velocity and acceleration constraints for goal.
+   * @param period      The period between controller updates in seconds. The
+   *                    default is 20 milliseconds.
+   */
+  ProfiledPIDController(double Kp, double Ki, double Kd,
+                        frc::TrapezoidProfile::Constraints constraints,
+                        units::second_t period = 20_ms);
+
+  ~ProfiledPIDController() override = default;
+
+  ProfiledPIDController(const ProfiledPIDController&) = default;
+  ProfiledPIDController& operator=(const ProfiledPIDController&) = default;
+  ProfiledPIDController(ProfiledPIDController&&) = default;
+  ProfiledPIDController& operator=(ProfiledPIDController&&) = default;
+
+  /**
+   * 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.
+   */
+  units::second_t GetPeriod() const;
+
+  /**
+   * Sets the goal for the ProfiledPIDController.
+   *
+   * @param goal The desired unprofiled setpoint.
+   */
+  void SetGoal(units::meter_t goal);
+
+  /**
+   * Gets the goal for the ProfiledPIDController.
+   */
+  units::meter_t GetGoal() 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 positionTolerance The maximum allowable position error.
+   * @param velocityTolerance The maximum allowable velocity error.
+   * @param toleranceType     The type of tolerance specified.
+   */
+  bool AtGoal(
+      double positionTolerance,
+      double velocityTolerance = std::numeric_limits<double>::infinity(),
+      frc2::PIDController::Tolerance toleranceType =
+          frc2::PIDController::Tolerance::kAbsolute) const;
+
+  /**
+   * Returns true if the error is within the tolerance of the error.
+   *
+   * This will return false until at least one input value has been computed.
+   */
+  bool AtGoal() const;
+
+  /**
+   * Set velocity and acceleration constraints for goal.
+   *
+   * @param constraints Velocity and acceleration constraints for goal.
+   */
+  void SetConstraints(frc::TrapezoidProfile::Constraints constraints);
+
+  /**
+   * Returns the current setpoint of the ProfiledPIDController.
+   *
+   * @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 positionTolerance The maximum allowable position error.
+   * @param velocityTolerance The maximum allowable velocity error.
+   * @param toleranceType     The type of tolerance specified.
+   */
+  bool AtSetpoint(
+      double positionTolerance,
+      double velocityTolerance = std::numeric_limits<double>::infinity(),
+      frc2::PIDController::Tolerance toleranceType =
+          frc2::PIDController::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 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);
+
+  /**
+   * Enables continuous input.
+   *
+   * 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 minimumInput The minimum value expected from the input.
+   * @param maximumInput The maximum value expected from the input.
+   */
+  void EnableContinuousInput(double minimumInput, double maximumInput);
+
+  /**
+   * Disables continuous input.
+   */
+  void DisableContinuousInput();
+
+  /**
+   * 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 positionTolerance Position error which is tolerable.
+   * @param velocityTolerance Velocity error which is tolerable.
+   */
+  void SetAbsoluteTolerance(
+      double positionTolerance,
+      double velocityTolerance = std::numeric_limits<double>::infinity());
+
+  /**
+   * Sets the percent error which is considered tolerable for use with
+   * AtSetpoint().
+   *
+   * @param positionTolerance Position error which is tolerable.
+   * @param velocityTolerance Velocity error which is tolerable.
+   */
+  void SetPercentTolerance(
+      double positionTolerance,
+      double velocityTolerance = std::numeric_limits<double>::infinity());
+
+  /**
+   * Returns the difference between the setpoint and the measurement.
+   *
+   * @return The error.
+   */
+  double GetPositionError() const;
+
+  /**
+   * Returns the change in error per second.
+   */
+  double GetVelocityError() 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 goal The new goal of the controller.
+   */
+  double Calculate(double measurement, units::meter_t goal);
+
+  /**
+   * Returns the next output of the PID controller.
+   *
+   * @param measurement The current measurement of the process variable.
+   * @param goal        The new goal of the controller.
+   * @param constraints Velocity and acceleration constraints for goal.
+   */
+  double Calculate(double measurement, units::meter_t goal,
+                   frc::TrapezoidProfile::Constraints constraints);
+
+  /**
+   * Reset the previous error, the integral term, and disable the controller.
+   */
+  void Reset();
+
+  void InitSendable(frc::SendableBuilder& builder) override;
+
+ private:
+  frc2::PIDController m_controller;
+  frc::TrapezoidProfile::State m_goal;
+  frc::TrapezoidProfile::State m_setpoint;
+  frc::TrapezoidProfile::Constraints m_constraints;
+};
+
+}  // namespace frc

wpilibcExamples/src/main/cpp/examples/ElevatorProfiledPID/cpp/Robot.cpp

@@ -0,0 +1,45 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 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/Encoder.h>
+#include <frc/Joystick.h>
+#include <frc/Spark.h>
+#include <frc/TimedRobot.h>
+#include <frc/controller/ProfiledPIDController.h>
+#include <frc/trajectory/TrapezoidProfile.h>
+
+class Robot : public frc::TimedRobot {
+ public:
+  static constexpr units::second_t kDt = 20_ms;
+
+  Robot() { m_encoder.SetDistancePerPulse(1.0 / 360.0 * 2.0 * 3.1415 * 1.5); }
+
+  void TeleopPeriodic() override {
+    if (m_joystick.GetRawButtonPressed(2)) {
+      m_controller.SetGoal(5_m);
+    } else if (m_joystick.GetRawButtonPressed(3)) {
+      m_controller.SetGoal(0_m);
+    }
+
+    // Run controller and update motor output
+    m_motor.Set(m_controller.Calculate(m_encoder.GetDistance()));
+  }
+
+ private:
+  frc::Joystick m_joystick{1};
+  frc::Encoder m_encoder{1, 2};
+  frc::Spark m_motor{1};
+
+  // Create a PID controller whose setpoint's change is subject to maximum
+  // velocity and acceleration constraints.
+  frc::TrapezoidProfile::Constraints m_constraints{1.75_mps, 0.75_mps_sq};
+  frc::ProfiledPIDController m_controller{1.3, 0.0, 0.7, m_constraints, kDt};
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif

wpilibcExamples/src/main/cpp/examples/examples.json

@@ -178,6 +178,18 @@
     "foldername": "ElevatorTrapezoidProfile",
     "gradlebase": "cpp"
   },
+  {
+    "name": "Elevator with profiled PID controller",
+    "description": "An example to demonstrate the use of an encoder and trapezoid profiled PID control to reach elevator position setpoints.",
+    "tags": [
+      "Digital",
+      "Sensors",
+      "Actuators",
+      "Joystick"
+    ],
+    "foldername": "ElevatorProfiledPID",
+    "gradlebase": "cpp"
+  },
   {
     "name": "Getting Started",
     "description": "An example program which demonstrates the simplest autonomous and teleoperated routines.",

wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ProfiledPIDController.java

@@ -0,0 +1,422 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 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 edu.wpi.first.wpilibj.SendableBase;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+
+/**
+ * Implements a PID control loop whose setpoint is constrained by a trapezoid
+ * profile.
+ */
+@SuppressWarnings("PMD.TooManyMethods")
+public class ProfiledPIDController extends SendableBase {
+  private PIDController m_controller;
+  private TrapezoidProfile.State m_goal = new TrapezoidProfile.State();
+  private TrapezoidProfile.State m_setpoint = new TrapezoidProfile.State();
+  private TrapezoidProfile.Constraints m_constraints;
+
+  /**
+   * Allocates a ProfiledPIDController 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 constraints Velocity and acceleration constraints for goal.
+   */
+  @SuppressWarnings("ParameterName")
+  public ProfiledPIDController(double Kp, double Ki, double Kd,
+                        TrapezoidProfile.Constraints constraints) {
+    this(Kp, Ki, Kd, constraints, 0.02);
+  }
+
+  /**
+   * Allocates a ProfiledPIDController 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 constraints Velocity and acceleration constraints for goal.
+   * @param period      The period between controller updates in seconds. The
+   *                    default is 0.02 seconds.
+   */
+  @SuppressWarnings("ParameterName")
+  public ProfiledPIDController(double Kp, double Ki, double Kd,
+                        TrapezoidProfile.Constraints constraints,
+                        double period) {
+    m_controller = new PIDController(Kp, Ki, Kd, period);
+    m_constraints = constraints;
+  }
+
+  /**
+   * Sets the PID Controller gain parameters.
+   *
+   * <p>Sets the proportional, integral, and differential coefficients.
+   *
+   * @param Kp Proportional coefficient
+   * @param Ki Integral coefficient
+   * @param Kd Differential coefficient
+   */
+  @SuppressWarnings("ParameterName")
+  public void setPID(double Kp, double Ki, double Kd) {
+    m_controller.setPID(Kp, Ki, Kd);
+  }
+
+  /**
+   * Sets the proportional coefficient of the PID controller gain.
+   *
+   * @param Kp proportional coefficient
+   */
+  @SuppressWarnings("ParameterName")
+  public void setP(double Kp) {
+    m_controller.setP(Kp);
+  }
+
+  /**
+   * Sets the integral coefficient of the PID controller gain.
+   *
+   * @param Ki integral coefficient
+   */
+  @SuppressWarnings("ParameterName")
+  public void setI(double Ki) {
+    m_controller.setI(Ki);
+  }
+
+  /**
+   * Sets the differential coefficient of the PID controller gain.
+   *
+   * @param Kd differential coefficient
+   */
+  @SuppressWarnings("ParameterName")
+  public void setD(double Kd) {
+    m_controller.setD(Kd);
+  }
+
+  /**
+   * Gets the proportional coefficient.
+   *
+   * @return proportional coefficient
+   */
+  public double getP() {
+    return m_controller.getP();
+  }
+
+  /**
+   * Gets the integral coefficient.
+   *
+   * @return integral coefficient
+   */
+  public double getI() {
+    return m_controller.getI();
+  }
+
+  /**
+   * Gets the differential coefficient.
+   *
+   * @return differential coefficient
+   */
+  public double getD() {
+    return m_controller.getD();
+  }
+
+  /**
+   * Gets the period of this controller.
+   *
+   * @return The period of the controller.
+   */
+  public double getPeriod() {
+    return m_controller.getPeriod();
+  }
+
+  /**
+   * Sets the goal for the ProfiledPIDController.
+   *
+   * @param goal The desired unprofiled setpoint.
+   */
+  public void setGoal(double goal) {
+    m_goal = new TrapezoidProfile.State(goal, 0);
+  }
+
+  /**
+   * Gets the goal for the ProfiledPIDController.
+   */
+  public double getGoal() {
+    return m_goal.position;
+  }
+
+  /**
+   * Returns true if the error is within tolerance of the setpoint.
+   *
+   * <p>This will return false until at least one input value has been computed.
+   *
+   * @param positionTolerance The maximum allowable position error.
+   */
+  public boolean atGoal(double positionTolerance) {
+    return atGoal(positionTolerance, Double.POSITIVE_INFINITY, PIDController.Tolerance.kAbsolute);
+  }
+
+  /**
+   * Returns true if the error is within tolerance of the setpoint.
+   *
+   * <p>This will return false until at least one input value has been computed.
+   *
+   * @param positionTolerance The maximum allowable position error.
+   * @param velocityTolerance The maximum allowable velocity error.
+   */
+  public boolean atGoal(double positionTolerance, double velocityTolerance) {
+    return atGoal(positionTolerance, velocityTolerance, PIDController.Tolerance.kAbsolute);
+  }
+
+  /**
+   * Returns true if the error is within tolerance of the setpoint.
+   *
+   * <p>This will return false until at least one input value has been computed.
+   *
+   * @param positionTolerance The maximum allowable position error.
+   * @param velocityTolerance The maximum allowable velocity error.
+   * @param toleranceType     The type of tolerance specified.
+   */
+  public boolean atGoal(
+      double positionTolerance,
+      double velocityTolerance,
+      PIDController.Tolerance toleranceType) {
+    return atSetpoint(positionTolerance, velocityTolerance, toleranceType)
+        && m_goal.equals(m_setpoint);
+  }
+
+  /**
+   * Returns true if the error is within the tolerance of the error.
+   *
+   * <p>This will return false until at least one input value has been computed.
+   */
+  public boolean atGoal() {
+    return atSetpoint() && m_goal.equals(m_setpoint);
+  }
+
+  /**
+   * Set velocity and acceleration constraints for goal.
+   *
+   * @param constraints Velocity and acceleration constraints for goal.
+   */
+  public void setConstraints(TrapezoidProfile.Constraints constraints) {
+    m_constraints = constraints;
+  }
+
+  /**
+   * Returns the current setpoint of the ProfiledPIDController.
+   *
+   * @return The current setpoint.
+   */
+  public double getSetpoint() {
+    return m_controller.getSetpoint();
+  }
+
+  /**
+   * Returns true if the error is within tolerance of the setpoint.
+   *
+   * <p>This will return false until at least one input value has been computed.
+   *
+   * @param positionTolerance The maximum allowable position error.
+   */
+  public boolean atSetpoint(double positionTolerance) {
+    return atSetpoint(positionTolerance, Double.POSITIVE_INFINITY,
+        PIDController.Tolerance.kAbsolute);
+  }
+
+  /**
+   * Returns true if the error is within tolerance of the setpoint.
+   *
+   * <p>This will return false until at least one input value has been computed.
+   *
+   * @param positionTolerance The maximum allowable position error.
+   * @param velocityTolerance The maximum allowable velocity error.
+   */
+  public boolean atSetpoint(double positionTolerance, double velocityTolerance) {
+    return atSetpoint(positionTolerance, velocityTolerance, PIDController.Tolerance.kAbsolute);
+  }
+
+  /**
+   * Returns true if the error is within tolerance of the setpoint.
+   *
+   * <p>This will return false until at least one input value has been computed.
+   *
+   * @param positionTolerance The maximum allowable position error.
+   * @param velocityTolerance The maximum allowable velocity error.
+   * @param toleranceType     The type of tolerance specified.
+   */
+  public boolean atSetpoint(
+      double positionTolerance,
+      double velocityTolerance,
+      PIDController.Tolerance toleranceType) {
+    return m_controller.atSetpoint(positionTolerance, velocityTolerance, toleranceType);
+  }
+
+  /**
+   * Returns true if the error is within the tolerance of the error.
+   *
+   * <p>This will return false until at least one input value has been computed.
+   */
+  public boolean atSetpoint() {
+    return m_controller.atSetpoint();
+  }
+
+  /**
+   * 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_controller.setInputRange(minimumInput, maximumInput);
+  }
+
+  /**
+   * Enables continuous input.
+   *
+   * <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 minimumInput The minimum value expected from the input.
+   * @param maximumInput The maximum value expected from the input.
+   */
+  public void enableContinuousInput(double minimumInput, double maximumInput) {
+    m_controller.enableContinuousInput(minimumInput, maximumInput);
+  }
+
+  /**
+   * Disables continuous input.
+   */
+  public void disableContinuousInput() {
+    m_controller.disableContinuousInput();
+  }
+
+  /**
+   * 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_controller.setOutputRange(minimumOutput, maximumOutput);
+  }
+
+  /**
+   * Sets the absolute error which is considered tolerable for use with
+   * atSetpoint().
+   *
+   * @param positionTolerance Position error which is tolerable.
+   */
+  public void setAbsoluteTolerance(double positionTolerance) {
+    setAbsoluteTolerance(positionTolerance, Double.POSITIVE_INFINITY);
+  }
+
+  /**
+   * Sets the absolute error which is considered tolerable for use with
+   * atSetpoint().
+   *
+   * @param positionTolerance Position error which is tolerable.
+   * @param velocityTolerance Velocity error which is tolerable.
+   */
+  public void setAbsoluteTolerance(double positionTolerance, double velocityTolerance) {
+    m_controller.setAbsoluteTolerance(positionTolerance, velocityTolerance);
+  }
+
+  /**
+   * Sets the percent error which is considered tolerable for use with
+   * atSetpoint().
+   *
+   * @param positionTolerance Position error which is tolerable.
+   */
+  public void setPercentTolerance(double positionTolerance) {
+    m_controller.setPercentTolerance(positionTolerance, Double.POSITIVE_INFINITY);
+  }
+
+  /**
+   * Sets the percent error which is considered tolerable for use with
+   * atSetpoint().
+   *
+   * @param positionTolerance Position error which is tolerable.
+   * @param velocityTolerance Velocity error which is tolerable.
+   */
+  public void setPercentTolerance(
+      double positionTolerance,
+      double velocityTolerance) {
+    m_controller.setPercentTolerance(positionTolerance, velocityTolerance);
+  }
+
+  /**
+   * Returns the difference between the setpoint and the measurement.
+   *
+   * @return The error.
+   */
+  public double getPositionError() {
+    return m_controller.getPositionError();
+  }
+
+  /**
+   * Returns the change in error per second.
+   */
+  public double getVelocityError() {
+    return m_controller.getVelocityError();
+  }
+
+  /**
+   * Returns the next output of the PID controller.
+   *
+   * @param measurement The current measurement of the process variable.
+   */
+  public double calculate(double measurement) {
+    var profile = new TrapezoidProfile(m_constraints, m_goal, m_setpoint);
+    m_setpoint = profile.calculate(getPeriod());
+    return m_controller.calculate(measurement, m_setpoint.position);
+  }
+
+  /**
+   * Returns the next output of the PID controller.
+   *
+   * @param measurement The current measurement of the process variable.
+   * @param goal The new goal of the controller.
+   */
+  public double calculate(double measurement, double goal) {
+    setGoal(goal);
+    return calculate(measurement);
+  }
+
+  /**
+   * Returns the next output of the PID controller.
+   *
+   * @param measurement The current measurement of the process variable.
+   * @param goal        The new goal of the controller.
+   * @param constraints Velocity and acceleration constraints for goal.
+   */
+  public double calculate(double measurement, double goal,
+                   TrapezoidProfile.Constraints constraints) {
+    setConstraints(constraints);
+    return calculate(measurement, goal);
+  }
+
+  /**
+   * Reset the previous error, the integral term, and disable the controller.
+   */
+  public void reset() {
+    m_controller.reset();
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    builder.setSmartDashboardType("ProfiledPIDController");
+    builder.addDoubleProperty("p", this::getP, this::setP);
+    builder.addDoubleProperty("i", this::getI, this::setI);
+    builder.addDoubleProperty("d", this::getD, this::setD);
+    builder.addDoubleProperty("goal", this::getGoal, this::setGoal);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorprofiledpid/Main.java

@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 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.examples.elevatorprofiledpid;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all.
+ * Unless you know what you are doing, do not modify this file except to
+ * change the parameter class to the startRobot call.
+ */
+public final class Main {
+  private Main() {
+  }
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorprofiledpid/Robot.java

@@ -0,0 +1,47 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 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.examples.elevatorprofiledpid;
+
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.Spark;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+
+public class Robot extends TimedRobot {
+  private static double kDt = 0.02;
+
+  private final Joystick m_joystick = new Joystick(1);
+  private final Encoder m_encoder = new Encoder(1, 2);
+  private final Spark m_motor = new Spark(1);
+
+  // Create a PID controller whose setpoint's change is subject to maximum
+  // velocity and acceleration constraints.
+  private final TrapezoidProfile.Constraints m_constraints =
+      new TrapezoidProfile.Constraints(1.75, 0.75);
+  private final ProfiledPIDController m_controller =
+      new ProfiledPIDController(1.3, 0.0, 0.7, m_constraints, kDt);
+
+  @Override
+  public void robotInit() {
+    m_encoder.setDistancePerPulse(1.0 / 360.0 * 2.0 * 3.1415 * 1.5);
+  }
+
+  @Override
+  public void teleopPeriodic() {
+    if (m_joystick.getRawButtonPressed(2)) {
+      m_controller.setGoal(5);
+    } else if (m_joystick.getRawButtonPressed(3)) {
+      m_controller.setGoal(0);
+    }
+
+    // Run controller and update motor output
+    m_motor.set(m_controller.calculate(m_encoder.getDistance()));
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json

@@ -144,6 +144,19 @@
     "gradlebase": "java",
     "mainclass": "Main"
   },
+  {
+    "name": "Elevator with profiled PID controller",
+    "description": "An example to demonstrate the use of an encoder and trapezoid profiled PID control to reach elevator position setpoints.",
+    "tags": [
+      "Digital",
+      "Sensors",
+      "Actuators",
+      "Joystick"
+    ],
+    "foldername": "elevatorprofiledpid",
+    "gradlebase": "java",
+    "mainclass": "Main"
+  },
   {
     "name": "Gyro",
     "description": "An example program showing how to drive straight with using a gyro sensor.",