[examples] Improvements to Elevator Simulation Example (#4937)

Co-authored-by: Abhay Shukla <105139789+aboombadev@users.noreply.github.com>
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
Co-authored-by: Ryan Blue <ryanzblue@gmail.com>

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

@@ -9,7 +9,9 @@
 #include <frc/RobotController.h>
 #include <frc/StateSpaceUtil.h>
 #include <frc/TimedRobot.h>
+#include <frc/controller/ElevatorFeedforward.h>
 #include <frc/controller/PIDController.h>
+#include <frc/controller/ProfiledPIDController.h>
 #include <frc/motorcontrol/PWMSparkMax.h>
 #include <frc/simulation/BatterySim.h>
 #include <frc/simulation/ElevatorSim.h>
@@ -27,8 +29,7 @@
 #include <units/moment_of_inertia.h>
 
 /**
- * This is a sample program to demonstrate how to use a state-space controller
- * to control an arm.
+ * This is a sample program to demonstrate the use of elevator simulation.
  */
 class Robot : public frc::TimedRobot {
   static constexpr int kMotorPort = 0;
@@ -37,10 +38,19 @@ class Robot : public frc::TimedRobot {
   static constexpr int kJoystickPort = 0;
 
   static constexpr double kElevatorKp = 5.0;
+  static constexpr double kElevatorKi = 0.0;
+  static constexpr double kElevatorKd = 0.0;
+
+  static constexpr units::volt_t kElevatorkS = 0.0_V;
+  static constexpr units::volt_t kElevatorkG = 0.0_V;
+  static constexpr auto kElevatorkV = 0.762_V / 1_mps;
+  static constexpr auto kElevatorkA = 0.762_V / 1_mps_sq;
+
   static constexpr double kElevatorGearing = 10.0;
   static constexpr units::meter_t kElevatorDrumRadius = 2_in;
   static constexpr units::kilogram_t kCarriageMass = 4.0_kg;
 
+  static constexpr units::meter_t kSetpoint = 30_in;
   static constexpr units::meter_t kMinElevatorHeight = 2_in;
   static constexpr units::meter_t kMaxElevatorHeight = 50_in;
 
@@ -53,7 +63,13 @@ class Robot : public frc::TimedRobot {
   frc::DCMotor m_elevatorGearbox = frc::DCMotor::Vex775Pro(4);
 
   // Standard classes for controlling our elevator
-  frc2::PIDController m_controller{kElevatorKp, 0, 0};
+  frc::TrapezoidProfile<units::meters>::Constraints m_constraints{2.45_mps,
+                                                                  2.45_mps_sq};
+  frc::ProfiledPIDController<units::meters> m_controller{
+      kElevatorKp, kElevatorKi, kElevatorKd, m_constraints};
+
+  frc::ElevatorFeedforward m_feedforward{kElevatorkS, kElevatorkG, kElevatorkV,
+                                         kElevatorkA};
   frc::Encoder m_encoder{kEncoderAChannel, kEncoderBChannel};
   frc::PWMSparkMax m_motor{kMotorPort};
   frc::Joystick m_joystick{kJoystickPort};
@@ -111,15 +127,21 @@ class Robot : public frc::TimedRobot {
 
   void TeleopPeriodic() override {
     if (m_joystick.GetTrigger()) {
-      // Here, we run PID control like normal, with a constant setpoint of 30in.
-      double pidOutput = m_controller.Calculate(m_encoder.GetDistance(),
-                                                units::meter_t{30_in}.value());
-      m_motor.SetVoltage(units::volt_t{pidOutput});
+      // Here, we set the constant setpoint of 30in.
+      m_controller.SetGoal(kSetpoint);
     } else {
-      // Otherwise, we disable the motor.
-      m_motor.Set(0.0);
+      // Otherwise, we update the setpoint to 0.
+      m_controller.SetGoal(0.0_m);
     }
+    // With the setpoint value we run PID control like normal
+    double pidOutput =
+        m_controller.Calculate(units::meter_t{m_encoder.GetDistance()});
+    units::volt_t feedforwardOutput =
+        m_feedforward.Calculate(m_controller.GetSetpoint().velocity);
+    m_motor.SetVoltage(units::volt_t{pidOutput} + feedforwardOutput);
   }
+  // To view the Elevator in the simulator, select Network Tables ->
+  // SmartDashboard -> Elevator Sim
 
   void DisabledInit() override {
     // This just makes sure that our simulation code knows that the motor's off.

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

@@ -756,7 +756,8 @@
       "Elevator",
       "State-Space",
       "Simulation",
-      "Mechanism2d"
+      "Mechanism2d",
+      "Profiled PID"
     ],
     "foldername": "ElevatorSimulation",
     "gradlebase": "cpp",

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

@@ -5,8 +5,10 @@
 package edu.wpi.first.wpilibj.examples.elevatorsimulation;
 
 import edu.wpi.first.math.VecBuilder;
-import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.ElevatorFeedforward;
+import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.Joystick;
@@ -22,18 +24,27 @@ import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
-/** This is a sample program to demonstrate the use of elevator simulation with existing code. */
+/** This is a sample program to demonstrate the use of elevator simulation. */
 public class Robot extends TimedRobot {
   private static final int kMotorPort = 0;
   private static final int kEncoderAChannel = 0;
   private static final int kEncoderBChannel = 1;
   private static final int kJoystickPort = 0;
 
-  private static final double kElevatorKp = 5.0;
+  private static final double kElevatorKp = 5;
+  private static final double kElevatorKi = 0;
+  private static final double kElevatorKd = 0;
+
+  private static final double kElevatorkS = 0.0; // volts (V)
+  private static final double kElevatorkG = 0.762; // volts (V)
+  private static final double kElevatorkV = 0.762; // volt per velocity (V/(m/s))
+  private static final double kElevatorkA = 0.0; // volt per acceleration (V/(m/s²))
+
   private static final double kElevatorGearing = 10.0;
   private static final double kElevatorDrumRadius = Units.inchesToMeters(2.0);
   private static final double kCarriageMass = 4.0; // kg
 
+  private static final double kSetpoint = Units.inchesToMeters(30);
   private static final double kMinElevatorHeight = Units.inchesToMeters(2);
   private static final double kMaxElevatorHeight = Units.inchesToMeters(50);
 
@@ -42,10 +53,15 @@ public class Robot extends TimedRobot {
   private static final double kElevatorEncoderDistPerPulse =
       2.0 * Math.PI * kElevatorDrumRadius / 4096;
 
+  // This gearbox represents a gearbox containing 4 Vex 775pro motors.
   private final DCMotor m_elevatorGearbox = DCMotor.getVex775Pro(4);
 
   // Standard classes for controlling our elevator
-  private final PIDController m_controller = new PIDController(kElevatorKp, 0, 0);
+  private final ProfiledPIDController m_controller =
+      new ProfiledPIDController(
+          kElevatorKp, kElevatorKi, kElevatorKd, new TrapezoidProfile.Constraints(2.45, 2.45));
+  ElevatorFeedforward m_feedforward =
+      new ElevatorFeedforward(kElevatorkS, kElevatorkG, kElevatorkV, kElevatorkA);
   private final Encoder m_encoder = new Encoder(kEncoderAChannel, kEncoderBChannel);
   private final PWMSparkMax m_motor = new PWMSparkMax(kMotorPort);
   private final Joystick m_joystick = new Joystick(kJoystickPort);
@@ -103,14 +119,18 @@ public class Robot extends TimedRobot {
   @Override
   public void teleopPeriodic() {
     if (m_joystick.getTrigger()) {
-      // Here, we run PID control like normal, with a constant setpoint of 30in.
-      double pidOutput = m_controller.calculate(m_encoder.getDistance(), Units.inchesToMeters(30));
-      m_motor.setVoltage(pidOutput);
+      // Here, we set the constant setpoint of 30in.
+      m_controller.setGoal(kSetpoint);
     } else {
-      // Otherwise, we disable the motor.
-      m_motor.set(0.0);
+      // Otherwise, we update the setpoint to 0.
+      m_controller.setGoal(0.0);
     }
+    // With the setpoint value we run PID control like normal
+    double pidOutput = m_controller.calculate(m_encoder.getDistance());
+    double feedforwardOutput = m_feedforward.calculate(m_controller.getSetpoint().velocity);
+    m_motor.setVoltage(pidOutput + feedforwardOutput);
   }
+  // To view the Elevator in the simulator, select Network Tables -> SmartDashboard -> Elevator Sim
 
   @Override
   public void disabledInit() {

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

@@ -793,7 +793,8 @@
       "Elevator",
       "State-Space",
       "Simulation",
-      "Mechanism2d"
+      "Mechanism2d",
+      "Profiled PID"
     ],
     "foldername": "elevatorsimulation",
     "gradlebase": "java",