Add ProfiledPIDController

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