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 67eee460675..fe8c511fbb1 100644 --- a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/subsystems/Drive.cpp +++ b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/subsystems/Drive.cpp @@ -5,6 +5,7 @@ #include "subsystems/Drive.h" #include +#include #include @@ -57,7 +58,12 @@ frc2::CommandPtr Drive::TurnToAngleCommand(units::degree_t angle) { [this, angle] { m_drive.ArcadeDrive( 0, m_controller.Calculate(m_gyro.GetRotation2d().Radians(), - angle)); + 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 6fb6e865fcc..fe6d9bf5f7e 100644 --- a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/Constants.h +++ b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/Constants.h @@ -38,6 +38,14 @@ 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; + +// These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT! +// These characterization values MUST be determined either experimentally or +// theoretically for *your* robot's drive. The SysId tool provides a convenient +// method for obtaining these values for your robot. +inline constexpr auto ks = 1_V; +inline constexpr auto kv = 0.8_V * 1_s / 1_rad; +inline constexpr auto ka = 0.15 * 1_s * 1_s / 1_rad; } // 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 4b99dabae1d..5b89c25a577 100644 --- a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Drive.h +++ b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Drive.h @@ -9,6 +9,7 @@ #include #include #include +#include #include #include #include @@ -16,6 +17,7 @@ #include #include "Constants.h" +#include "units/angle.h" class Drive : public frc2::SubsystemBase { public: @@ -73,4 +75,6 @@ class Drive : public frc2::SubsystemBase { DriveConstants::kTurnI, DriveConstants::kTurnD, {DriveConstants::kMaxTurnRate, DriveConstants::kMaxTurnAcceleration}}; + frc::SimpleMotorFeedforward m_feedforward{ + DriveConstants::ks, DriveConstants::kv, DriveConstants::ka}; }; 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 57839f325e9..1178e3f8672 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 @@ -41,6 +41,14 @@ public static final class DriveConstants { public static final double kMaxTurnRateDegPerS = 100; public static final double kMaxTurnAccelerationDegPerSSquared = 300; + + // These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT! + // These characterization values MUST be determined either experimentally or theoretically + // for *your* robot's drive. + // The SysId tool provides a convenient method for obtaining these values for your robot. + 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 08d34aa673a..9bef04fa459 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,13 +4,17 @@ package edu.wpi.first.wpilibj.examples.rapidreactcommandbot.subsystems; +import static edu.wpi.first.units.Units.*; + 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; @@ -56,6 +60,11 @@ public class Drive extends SubsystemBase { 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() { @@ -130,7 +139,13 @@ public Command turnToAngleCommand(double angleDeg) { () -> m_controller.reset(m_gyro.getRotation2d().getDegrees()), () -> m_drive.arcadeDrive( - 0, m_controller.calculate(m_gyro.getRotation2d().getDegrees(), angleDeg))) + 0, + m_controller.calculate(m_gyro.getRotation2d().getDegrees(), angleDeg) + // Divide feedforward voltage by battery voltage to normalize it to [-1, 1] + + m_feedforward + .calculate(RadiansPerSecond.of(m_controller.getSetpoint().velocity)) + .in(Volts) + / RobotController.getBatteryVoltage())) .until(m_controller::atGoal) .finallyDo(() -> m_drive.arcadeDrive(0, 0)); }