Add ProfiledPIDController

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.",