Create AcceleratedChassisSpeeds

src/main/java/frc/robot/Constants.java

@@ -109,7 +109,10 @@ public static final class DriveConstants {
         new Translation2d(-kWheelBase / 2, -kTrackWidth / 2));
 
     public static final double kMaxSpeedMetersPerSecond = 3.6576;
-    public static final double kMaxAngularSpeedRadiansPerSecond = 15.24/3;
+    public static final double kMaxAngularSpeedRadiansPerSecond = 15.24 / 3;
+
+    public static final double kMaxAccelerationMetersPerSecondSquared = kMaxSpeedMetersPerSecond * 4;
+    public static final double kMaxAngularAccelerationRadiansPerSecondSquared = kMaxAngularSpeedRadiansPerSecond * 4;
 
     public static final double kTurningStopTime = 0.2; // TODO: tune heading correction stop time
     public static final double kSpeedIncreasePerPeriod = 0.15;

src/main/java/frc/robot/Robot.java

@@ -37,6 +37,7 @@ public void robotPeriodic() {
 
   @Override
   public void disabledInit() {
+    m_robotContainer.ensureRobotStopped();
   }
 
   @Override
@@ -51,6 +52,7 @@ public void disabledPeriodic() {
 
   @Override
   public void autonomousInit() {
+    m_robotContainer.ensureRobotStopped();
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
 
     if (m_autonomousCommand != null) {
@@ -64,6 +66,7 @@ public void autonomousPeriodic() {
 
   @Override
   public void teleopInit() {
+    m_robotContainer.ensureRobotStopped();
     if (m_autonomousCommand != null) {
       m_autonomousCommand.cancel();
     }
@@ -75,6 +78,7 @@ public void teleopPeriodic() {
 
   @Override
   public void testInit() {
+    m_robotContainer.ensureRobotStopped();
     CommandScheduler.getInstance().cancelAll();
   }
 

src/main/java/frc/robot/RobotContainer.java

@@ -190,4 +190,11 @@ public Command getAutonomousCommand() {
                 AutonConstants.maxVelocity - (path.equals("Charger-comms") ? 0.75 : 0),
                 AutonConstants.maxAcceleration)));
   }
+
+  /**
+   * Ensure the robot is stopped
+   */
+  public void ensureRobotStopped() {
+    m_robotDrive.stop();
+  }
 }

src/main/java/frc/robot/subsystems/DriveSubsystem.java

@@ -25,6 +25,7 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.DriveConstants;
+import frc.robot.util.AcceleratedChassisSpeeds;
 import frc.robot.Robot;
 
 public class DriveSubsystem extends SubsystemBase {
@@ -78,8 +79,9 @@ public class DriveSubsystem extends SubsystemBase {
 
   private final Field2d m_field = new Field2d();
 
-  private double m_currentXSpeed = 0;
-  private double m_currentYSpeed = 0;
+  private AcceleratedChassisSpeeds m_acceleratedChassisSpeeds = new AcceleratedChassisSpeeds(
+      DriveConstants.kMaxAccelerationMetersPerSecondSquared,
+      DriveConstants.kMaxAngularAccelerationRadiansPerSecondSquared);
 
   /** Creates a new {@link DriveSubsystem}. */
   public DriveSubsystem() {
@@ -170,30 +172,29 @@ public void resetOdometry(Pose2d pose) {
    * @param fieldRelative Whether the provided x and y speeds are relative to the
    *                      field.
    */
-  public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
-    if (rot != 0) {
+  public void drive(double xSpeed, double ySpeed, double rotation, boolean fieldRelative) {
+    if (rotation != 0) {
       m_headingCorrectionTimer.reset();
     }
 
-    double rotation = rot;
+    Rotation2d gyroRotation = m_gyro.getRotation2d();
 
-    double currentAngle = MathUtil.angleModulus(m_gyro.getRotation2d().getRadians());
-
-    m_currentXSpeed = calculateAcceleration(m_currentXSpeed, xSpeed);
-    m_currentYSpeed = calculateAcceleration(m_currentYSpeed, ySpeed);
-
-    if ((m_currentXSpeed == 0 && m_currentYSpeed == 0)
+    ChassisSpeeds currentSpeeds = m_acceleratedChassisSpeeds.getCurrentSpeeds();
+    double currentAngle = MathUtil.angleModulus(gyroRotation.getRadians());
+    if ((currentSpeeds.vxMetersPerSecond == 0 && currentSpeeds.vyMetersPerSecond == 0)
         || m_headingCorrectionTimer.get() < DriveConstants.kTurningStopTime) {
       m_headingCorrectionPID.setSetpoint(currentAngle);
     } else {
       rotation = m_headingCorrectionPID.calculate(currentAngle);
     }
 
+    ChassisSpeeds desiredSpeeds = fieldRelative
+        ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rotation,
+            Robot.isReal() ? gyroRotation : new Rotation2d(m_gyroAngle))
+        : new ChassisSpeeds(xSpeed, ySpeed, rotation);
+
     var swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates(
-        fieldRelative
-            ? ChassisSpeeds.fromFieldRelativeSpeeds(m_currentXSpeed, m_currentYSpeed, rotation,
-                Robot.isReal() ? m_gyro.getRotation2d() : new Rotation2d(m_gyroAngle))
-            : new ChassisSpeeds(m_currentXSpeed, m_currentYSpeed, rotation));
+        m_acceleratedChassisSpeeds.calculateNewSpeeds(desiredSpeeds));
     setModuleStates(swerveModuleStates);
   }
 
@@ -232,20 +233,11 @@ public double getGyroPitch() {
   }
 
   /**
-   * Slowly accelerates the bot to the desired speed.
-   * 
-   * @param currentSpeed The current speed.
-   * @param desiredSpeed The desired speed.
-   * 
-   * @return The new speed to use.
+   * Stops the robot from moving
    */
-  private double calculateAcceleration(double currentSpeed, double desiredSpeed) {
-    if (Math.abs(currentSpeed - desiredSpeed) < DriveConstants.kSpeedIncreasePerPeriod) {
-      return desiredSpeed;
-    } else if (currentSpeed > desiredSpeed) {
-      return currentSpeed - DriveConstants.kSpeedIncreasePerPeriod;
-    } else {
-      return currentSpeed + DriveConstants.kSpeedIncreasePerPeriod;
-    }
+  public void stop() {
+    m_acceleratedChassisSpeeds.reset();
+    setModuleStates(
+        DriveConstants.kDriveKinematics.toSwerveModuleStates(m_acceleratedChassisSpeeds.getCurrentSpeeds()));
   }
 }

src/main/java/frc/robot/util/AcceleratedChassisSpeeds.java

@@ -0,0 +1,59 @@
+package frc.robot.util;
+
+import edu.wpi.first.math.filter.SlewRateLimiter;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+
+/**
+ * Calculates acceleration for ChassisSpeeds using given acceleration constants.
+ * This object maintains state and should be reused.
+ */
+public class AcceleratedChassisSpeeds {
+  private SlewRateLimiter m_xTranslationRateLimiter;
+  private SlewRateLimiter m_yTranslationRateLimiter;
+  private SlewRateLimiter m_rotationRateLimiter;
+  private ChassisSpeeds m_currentChassisSpeeds;
+
+  /**
+   * Create an accelerated ChassisSpeeds
+   * @param translationMaxAcceleration The max acceleration for translation, in m/s^2
+   * @param rotationMaxAcceleration The max acceleration for rotation, in radians/s^2
+   */
+  public AcceleratedChassisSpeeds(double translationMaxAcceleration, double rotationMaxAcceleration) {
+    m_xTranslationRateLimiter = new SlewRateLimiter(translationMaxAcceleration);
+    m_yTranslationRateLimiter = new SlewRateLimiter(translationMaxAcceleration);
+    m_rotationRateLimiter = new SlewRateLimiter(rotationMaxAcceleration);
+    m_currentChassisSpeeds = new ChassisSpeeds();
+  }
+
+  /**
+   * Retrieves the current ChassisSpeeds
+   * @return The current speeds
+   */
+  public ChassisSpeeds getCurrentSpeeds() {
+    return m_currentChassisSpeeds;
+  }
+
+  /**
+   * Calculates and saves the new ChassisSpeeds, given the acceleration
+   * constants and the provided desired speeds.
+   * @param desiredSpeeds The desired speeds
+   * @return The new calculated speeds
+   */
+  public ChassisSpeeds calculateNewSpeeds(ChassisSpeeds desiredSpeeds) {
+    m_currentChassisSpeeds = new ChassisSpeeds(
+        m_xTranslationRateLimiter.calculate(desiredSpeeds.vxMetersPerSecond),
+        m_yTranslationRateLimiter.calculate(desiredSpeeds.vyMetersPerSecond),
+        m_rotationRateLimiter.calculate(desiredSpeeds.omegaRadiansPerSecond));
+    return m_currentChassisSpeeds;
+  }
+
+  /**
+   * Resets the speeds to 0
+   */
+  public void reset() {
+    m_xTranslationRateLimiter.reset(0);
+    m_yTranslationRateLimiter.reset(0);
+    m_rotationRateLimiter.reset(0);
+    m_currentChassisSpeeds = new ChassisSpeeds();
+  }
+}