Add ProfiledPIDController command to RapidReactCommandBot

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

@@ -50,3 +50,15 @@ 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().Radians()); },
+             [this, angle] {
+               m_drive.ArcadeDrive(
+                   0, m_controller.Calculate(m_gyro.GetRotation2d().Radians(),
+                                             angle));
+             })
+      .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,15 @@ inline constexpr double kEncoderDistancePerPulse =
     // Assumes the encoders are directly mounted on the wheel shafts
     ((kWheelDiameter * std::numbers::pi) / kEncoderCPR).value();
 
+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;
 }  // namespace DriveConstants
 
 namespace IntakeConstants {

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

@@ -6,7 +6,9 @@
 
 #include <functional>
 
+#include <frc/ADXRS450_Gyro.h>
 #include <frc/Encoder.h>
+#include <frc/controller/ProfiledPIDController.h>
 #include <frc/drive/DifferentialDrive.h>
 #include <frc/motorcontrol/PWMSparkMax.h>
 #include <frc2/command/CommandPtr.h>
@@ -38,6 +40,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 +65,12 @@ 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}};
 };

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

@@ -412,6 +412,7 @@
       "Pneumatics",
       "Digital Input",
       "PID",
+      "Profiled PID",
       "XboxController"
     ],
     "foldername": "RapidReactCommandBot",

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

@@ -407,6 +407,7 @@
       "Pneumatics",
       "Digital Input",
       "PID",
+      "Profiled PID",
       "XboxController"
     ],
     "foldername": "rapidreactcommandbot",

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

@@ -31,6 +31,16 @@ public static final class DriveConstants {
     public static final double kEncoderDistancePerPulse =
         // Assumes the encoders are directly mounted on the wheel shafts
         (kWheelDiameterMeters * Math.PI) / kEncoderCPR;
+
+    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 class ShooterConstants {

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

@@ -6,7 +6,10 @@
 
 import edu.wpi.first.epilogue.Logged;
 import edu.wpi.first.epilogue.NotLogged;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+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.drive.DifferentialDrive;
 import edu.wpi.first.wpilibj.examples.rapidreactcommandbot.Constants.DriveConstants;
@@ -44,6 +47,16 @@ 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));
+
   /** Creates a new Drive subsystem. */
   public Drive() {
     SendableRegistry.addChild(m_drive, m_leftLeader);
@@ -60,6 +73,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 +118,20 @@ 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)))
+        .until(m_controller::atGoal)
+        .finallyDo(() -> m_drive.arcadeDrive(0, 0));
+  }
 }