[examples] Add ProfiledPID command to RapidReactCommandBot (#7030)

wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/subsystems/Drive.cpp

@@ -6,6 +6,7 @@
 
 #include <utility>
 
+#include <frc/RobotController.h>
 #include <frc2/command/Commands.h>
 
 Drive::Drive() {
@@ -23,6 +24,14 @@ Drive::Drive() {
   // Sets the distance per pulse for the encoders
   m_leftEncoder.SetDistancePerPulse(DriveConstants::kEncoderDistancePerPulse);
   m_rightEncoder.SetDistancePerPulse(DriveConstants::kEncoderDistancePerPulse);
+
+  // Set the controller to be continuous (because it is an angle controller)
+  m_controller.EnableContinuousInput(-180_deg, 180_deg);
+  // Set the controller tolerance - the delta tolerance ensures the robot is
+  // stationary at the setpoint before it is considered as having reached the
+  // reference
+  m_controller.SetTolerance(DriveConstants::kTurnTolerance,
+                            DriveConstants::kTurnRateTolerance);
 }
 
 frc2::CommandPtr Drive::ArcadeDriveCommand(std::function<double()> fwd,
@@ -50,3 +59,20 @@ frc2::CommandPtr Drive::DriveDistanceCommand(units::meter_t distance,
       // Stop the drive when the command ends
       .FinallyDo([this](bool interrupted) { m_drive.StopMotor(); });
 }
+
+frc2::CommandPtr Drive::TurnToAngleCommand(units::degree_t angle) {
+  return StartRun(
+             [this] { m_controller.Reset(m_gyro.GetRotation2d().Degrees()); },
+             [this, angle] {
+               m_drive.ArcadeDrive(
+                   0, m_controller.Calculate(m_gyro.GetRotation2d().Degrees(),
+                                             angle) +
+                          // Divide feedforward voltage by battery voltage to
+                          // normalize it to [-1, 1]
+                          m_feedforward.Calculate(
+                              m_controller.GetSetpoint().velocity) /
+                              frc::RobotController::GetBatteryVoltage());
+             })
+      .Until([this] { return m_controller.AtGoal(); })
+      .FinallyDo([this] { m_drive.ArcadeDrive(0, 0); });
+}

wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/Constants.h

@@ -29,6 +29,23 @@ inline constexpr double kEncoderDistancePerPulse =
     // Assumes the encoders are directly mounted on the wheel shafts
     ((kWheelDiameter * std::numbers::pi) / kEncoderCPR).value();
 
+// These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
+// These values MUST be determined either experimentally or theoretically for
+// *your* robot's drive. The SysId tool provides a convenient method for
+// obtaining feedback and feedforward values for your robot.
+inline constexpr double kTurnP = 1;
+inline constexpr double kTurnI = 0;
+inline constexpr double kTurnD = 0;
+
+inline constexpr auto kTurnTolerance = 5_deg;
+inline constexpr auto kTurnRateTolerance = 10_deg_per_s;
+
+inline constexpr auto kMaxTurnRate = 100_deg_per_s;
+inline constexpr auto kMaxTurnAcceleration = 300_deg_per_s / 1_s;
+
+inline constexpr auto ks = 1_V;
+inline constexpr auto kv = 0.8_V * 1_s / 1_deg;
+inline constexpr auto ka = 0.15_V * 1_s * 1_s / 1_deg;
 }  // namespace DriveConstants
 
 namespace IntakeConstants {

wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Drive.h

@@ -6,11 +6,15 @@
 
 #include <functional>
 
+#include <frc/ADXRS450_Gyro.h>
 #include <frc/Encoder.h>
+#include <frc/controller/ProfiledPIDController.h>
+#include <frc/controller/SimpleMotorFeedforward.h>
 #include <frc/drive/DifferentialDrive.h>
 #include <frc/motorcontrol/PWMSparkMax.h>
 #include <frc2/command/CommandPtr.h>
 #include <frc2/command/SubsystemBase.h>
+#include <units/angle.h>
 #include <units/length.h>
 
 #include "Constants.h"
@@ -38,6 +42,15 @@ class Drive : public frc2::SubsystemBase {
   [[nodiscard]]
   frc2::CommandPtr DriveDistanceCommand(units::meter_t distance, double speed);
 
+  /**
+   * Returns a command that turns to robot to the specified angle using a motion
+   * profile and PID controller.
+   *
+   * @param angle The angle to turn to
+   */
+  [[nodiscard]]
+  frc2::CommandPtr TurnToAngleCommand(units::degree_t angle);
+
  private:
   frc::PWMSparkMax m_leftLeader{DriveConstants::kLeftMotor1Port};
   frc::PWMSparkMax m_leftFollower{DriveConstants::kLeftMotor2Port};
@@ -54,4 +67,14 @@ class Drive : public frc2::SubsystemBase {
   frc::Encoder m_rightEncoder{DriveConstants::kRightEncoderPorts[0],
                               DriveConstants::kRightEncoderPorts[1],
                               DriveConstants::kRightEncoderReversed};
+
+  frc::ADXRS450_Gyro m_gyro;
+
+  frc::ProfiledPIDController<units::radians> m_controller{
+      DriveConstants::kTurnP,
+      DriveConstants::kTurnI,
+      DriveConstants::kTurnD,
+      {DriveConstants::kMaxTurnRate, DriveConstants::kMaxTurnAcceleration}};
+  frc::SimpleMotorFeedforward<units::radians> m_feedforward{
+      DriveConstants::ks, DriveConstants::kv, DriveConstants::ka};
 };

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

@@ -394,6 +394,8 @@
       "Pneumatics",
       "Digital Input",
       "PID",
+      "Gyro",
+      "Profiled PID",
       "XboxController"
     ],
     "foldername": "RapidReactCommandBot",

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

@@ -388,6 +388,8 @@
       "Pneumatics",
       "Digital Input",
       "PID",
+      "Gyro",
+      "Profiled PID",
       "XboxController"
     ],
     "foldername": "rapidreactcommandbot",

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/Constants.java

@@ -31,6 +31,24 @@ public static final class DriveConstants {
     public static final double kEncoderDistancePerPulse =
         // Assumes the encoders are directly mounted on the wheel shafts
         (kWheelDiameterMeters * Math.PI) / kEncoderCPR;
+
+    // These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
+    // These values MUST be determined either experimentally or theoretically for *your* robot's
+    // drive. The SysId tool provides a convenient method for obtaining feedback and feedforward
+    // values for your robot.
+    public static final double kTurnP = 1;
+    public static final double kTurnI = 0;
+    public static final double kTurnD = 0;
+
+    public static final double kTurnToleranceDeg = 5;
+    public static final double kTurnRateToleranceDegPerS = 10; // degrees per second
+
+    public static final double kMaxTurnRateDegPerS = 100;
+    public static final double kMaxTurnAccelerationDegPerSSquared = 300;
+
+    public static final double ksVolts = 1;
+    public static final double kvVoltSecondsPerDegree = 0.8;
+    public static final double kaVoltSecondsSquaredPerDegree = 0.15;
   }
 
   public static final class ShooterConstants {

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Drive.java

@@ -4,10 +4,18 @@
 
 package edu.wpi.first.wpilibj.examples.rapidreactcommandbot.subsystems;
 
+import static edu.wpi.first.units.Units.DegreesPerSecond;
+import static edu.wpi.first.units.Units.Volts;
+
 import edu.wpi.first.epilogue.Logged;
 import edu.wpi.first.epilogue.NotLogged;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.util.sendable.SendableRegistry;
+import edu.wpi.first.wpilibj.ADXRS450_Gyro;
 import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 import edu.wpi.first.wpilibj.examples.rapidreactcommandbot.Constants.DriveConstants;
 import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
@@ -44,6 +52,21 @@ public class Drive extends SubsystemBase {
           DriveConstants.kRightEncoderPorts[1],
           DriveConstants.kRightEncoderReversed);
 
+  private final ADXRS450_Gyro m_gyro = new ADXRS450_Gyro();
+  private final ProfiledPIDController m_controller =
+      new ProfiledPIDController(
+          DriveConstants.kTurnP,
+          DriveConstants.kTurnI,
+          DriveConstants.kTurnD,
+          new TrapezoidProfile.Constraints(
+              DriveConstants.kMaxTurnRateDegPerS,
+              DriveConstants.kMaxTurnAccelerationDegPerSSquared));
+  private final SimpleMotorFeedforward m_feedforward =
+      new SimpleMotorFeedforward(
+          DriveConstants.ksVolts,
+          DriveConstants.kvVoltSecondsPerDegree,
+          DriveConstants.kaVoltSecondsSquaredPerDegree);
+
   /** Creates a new Drive subsystem. */
   public Drive() {
     SendableRegistry.addChild(m_drive, m_leftLeader);
@@ -60,6 +83,13 @@ public Drive() {
     // Sets the distance per pulse for the encoders
     m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
     m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
+
+    // Set the controller to be continuous (because it is an angle controller)
+    m_controller.enableContinuousInput(-180, 180);
+    // Set the controller tolerance - the delta tolerance ensures the robot is stationary at the
+    // setpoint before it is considered as having reached the reference
+    m_controller.setTolerance(
+        DriveConstants.kTurnToleranceDeg, DriveConstants.kTurnRateToleranceDegPerS);
   }
 
   /**
@@ -98,4 +128,26 @@ public Command driveDistanceCommand(double distanceMeters, double speed) {
         // Stop the drive when the command ends
         .finallyDo(interrupted -> m_drive.stopMotor());
   }
+
+  /**
+   * Returns a command that turns to robot to the specified angle using a motion profile and PID
+   * controller.
+   *
+   * @param angleDeg The angle to turn to
+   */
+  public Command turnToAngleCommand(double angleDeg) {
+    return startRun(
+            () -> m_controller.reset(m_gyro.getRotation2d().getDegrees()),
+            () ->
+                m_drive.arcadeDrive(
+                    0,
+                    m_controller.calculate(m_gyro.getRotation2d().getDegrees(), angleDeg)
+                        // Divide feedforward voltage by battery voltage to normalize it to [-1, 1]
+                        + m_feedforward
+                                .calculate(DegreesPerSecond.of(m_controller.getSetpoint().velocity))
+                                .in(Volts)
+                            / RobotController.getBatteryVoltage()))
+        .until(m_controller::atGoal)
+        .finallyDo(() -> m_drive.arcadeDrive(0, 0));
+  }
 }