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..0030d016818 100644 --- a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/subsystems/Drive.cpp +++ b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/subsystems/Drive.cpp @@ -6,6 +6,7 @@ #include +#include #include 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 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); }); +} diff --git a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/Constants.h index 91e59d1f29e..05caf56fa31 100644 --- a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/Constants.h +++ b/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 { 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..6a9c32b9782 100644 --- a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Drive.h +++ b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Drive.h @@ -6,11 +6,15 @@ #include +#include #include +#include +#include #include #include #include #include +#include #include #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 m_controller{ + DriveConstants::kTurnP, + DriveConstants::kTurnI, + DriveConstants::kTurnD, + {DriveConstants::kMaxTurnRate, DriveConstants::kMaxTurnAcceleration}}; + frc::SimpleMotorFeedforward m_feedforward{ + DriveConstants::ks, DriveConstants::kv, DriveConstants::ka}; }; diff --git a/wpilibcExamples/src/main/cpp/examples/examples.json b/wpilibcExamples/src/main/cpp/examples/examples.json index fd846b8468a..ef53f478c0e 100644 --- a/wpilibcExamples/src/main/cpp/examples/examples.json +++ b/wpilibcExamples/src/main/cpp/examples/examples.json @@ -394,6 +394,8 @@ "Pneumatics", "Digital Input", "PID", + "Gyro", + "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 1b513b4fdc1..8a108ac3540 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 @@ -388,6 +388,8 @@ "Pneumatics", "Digital Input", "PID", + "Gyro", + "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..e0bdb757ba0 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,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 { 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..7e9e8e0370a 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 @@ -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)); + } }