[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};
 };