diff --git a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/subsystems/Drive.cpp b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/subsystems/Drive.cpp index e03ce432124..67eee460675 100644 --- a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/subsystems/Drive.cpp +++ b/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); }); +} diff --git a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/Constants.h index 91e59d1f29e..6fb6e865fcc 100644 --- a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/Constants.h +++ b/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 { diff --git a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Drive.h b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Drive.h index 8432e57b6b0..4b99dabae1d 100644 --- a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Drive.h +++ b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Drive.h @@ -6,7 +6,9 @@ #include +#include #include +#include #include #include #include @@ -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 m_controller{ + DriveConstants::kTurnP, + DriveConstants::kTurnI, + DriveConstants::kTurnD, + {DriveConstants::kMaxTurnRate, DriveConstants::kMaxTurnAcceleration}}; }; diff --git a/wpilibcExamples/src/main/cpp/examples/examples.json b/wpilibcExamples/src/main/cpp/examples/examples.json index 2aed3990203..58957baa6f3 100644 --- a/wpilibcExamples/src/main/cpp/examples/examples.json +++ b/wpilibcExamples/src/main/cpp/examples/examples.json @@ -412,6 +412,7 @@ "Pneumatics", "Digital Input", "PID", + "Profiled PID", "XboxController" ], "foldername": "RapidReactCommandBot", diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json index 720ab3c0bec..0d9f2c30b3d 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json @@ -407,6 +407,7 @@ "Pneumatics", "Digital Input", "PID", + "Profiled PID", "XboxController" ], "foldername": "rapidreactcommandbot", diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/Constants.java index 17d025035fc..57839f325e9 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/Constants.java +++ b/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 { diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Drive.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Drive.java index 18f12001ca6..08d34aa673a 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Drive.java +++ b/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)); + } }