Add feedforward

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() {
@@ -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); });

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_V * 1_s * 1_s / 1_rad;
 }  // namespace DriveConstants
 
 namespace IntakeConstants {

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

@@ -9,10 +9,12 @@
 #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"
@@ -73,4 +75,6 @@ class Drive : public frc2::SubsystemBase {
       DriveConstants::kTurnI,
       DriveConstants::kTurnD,
       {DriveConstants::kMaxTurnRate, DriveConstants::kMaxTurnAcceleration}};
+  frc::SimpleMotorFeedforward<units::radians> m_feedforward{
+      DriveConstants::ks, DriveConstants::kv, DriveConstants::ka};
 };

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 {

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

@@ -4,13 +4,18 @@
 
 package edu.wpi.first.wpilibj.examples.rapidreactcommandbot.subsystems;
 
+import static edu.wpi.first.units.Units.RadiansPerSecond;
+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;
@@ -56,6 +61,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 +140,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));
   }