Drivetrain velocity and accelration coinstrants. CHANGE CONSTANTS

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

@@ -72,11 +72,18 @@ public static final class Auton
   }
 
   public static class DrivetrainConstants {
-    public static final double kMAX_SPEED_METERS_PER_SECOND = 2;
-    public static final double kMAX_ACCELERATION_METERS_PER_SECOND_SQUARED = 2;
-    public static final double kMAX_ANGULAR_SPEED_RADIANS_PER_SECOND = 2 * Math.PI * 0.6;
-    public static final double kMAX_ANGULAR_ACCELERATION_RADIANS_PER_SECOND_SQUARED = 2 * Math.PI;
-
+    // TODO: CHANGE THEESEANDKJANSDKLJWENF LKJDSBLGKJFNadzs
+    public static final double kMAX_DRIVETRAIN_TRANSLATIONAL_VELOCITY_METERS_PER_SECOND = 2;
+    public static final double kMAX_DRIVETRAIN_TRANSLATIONAL_ACCELERATION_METERS_PER_SECOND_SQUARED = 2;
+    public static final double kMAX_DRIVETRAIN_TRANSLATIONAL_DECELERATION_METERS_PER_SECOND_SQUARED = 2;
+
+    public static final double kMAX_DRIVETRAIN_ANGULAR_VELOCITY_RADIANS_PER_SECOND = 2 * Math.PI * 0.6;
+    public static final double kMAX_DRIVETRAIN_ANGULAR_ACCELERATION_RADIANS_PER_SECOND_SQUARED = 2 * Math.PI;
+    public static final double kMAX_DRIVETRAIN_ANGULAR_DECELERATION_RADIANS_PER_SECOND_SQUARED = 2 * Math.PI;
+
+    public static final double kMAX_MODULE_DRIVE_VELOCITY_METERS_PER_SECOND = 4.5;
+    public static final double kMAX_MODULE_DRIVE_ACCELERATION_METERS_PER_SECOND = 2.5;
+    public static final double kMAX_MODULE_DRIVE_DECELERATION_METERS_PER_SECOND = 2.5;
 
     // x, y from center of the robot
     public static final Translation2d kFRONT_RIGHT_POSITION_METERS = new Translation2d(0.38, -0.38);

src/main/java/frc/robot/commands/autoAligment/DriveToPose.java

@@ -2,10 +2,11 @@
 
 import com.pathplanner.lib.auto.AutoBuilder;
 
-import com.pathplanner.lib.path.PathConstraints; 
-
-import edu.wpi.first.math.geometry.Pose2d; 
+import com.pathplanner.lib.path.PathConstraints;
+import com.pathplanner.lib.util.GeometryUtil;
 
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj2.command.Command; 
 
 import frc.robot.Constants; 

src/main/java/frc/robot/commands/drivetrain/AbsoluteFieldDrive.java

@@ -46,9 +46,9 @@ public void initialize() {
     public void execute() {
         // Calculate speeds based on input and max speed constants.
         ChassisSpeeds speeds = new ChassisSpeeds(
-            vX.getAsDouble() * Constants.DrivetrainConstants.kMAX_SPEED_METERS_PER_SECOND * invert,
-            vY.getAsDouble() * Constants.DrivetrainConstants.kMAX_SPEED_METERS_PER_SECOND * invert,
-            heading.getAsDouble() * Constants.DrivetrainConstants.kMAX_ANGULAR_SPEED_RADIANS_PER_SECOND * invert
+            vX.getAsDouble() * Constants.DrivetrainConstants.kMAX_DRIVETRAIN_TRANSLATIONAL_VELOCITY_METERS_PER_SECOND * invert,
+            vY.getAsDouble() * Constants.DrivetrainConstants.kMAX_DRIVETRAIN_TRANSLATIONAL_VELOCITY_METERS_PER_SECOND * invert,
+            heading.getAsDouble() * Constants.DrivetrainConstants.kMAX_DRIVETRAIN_ANGULAR_VELOCITY_RADIANS_PER_SECOND * invert
         );
 
         // Drive the swerve drive subsystem with field-relative speeds.

src/main/java/frc/robot/subsystems/drivetrain/swerve/DriveFFController.java

@@ -26,14 +26,14 @@ public DriveFFController() {
 
             default:
                 points.add(new double[] {0, 0, 0});
-                points.add(new double[] {DrivetrainConstants.kMAX_SPEED_METERS_PER_SECOND / 2, 0, 0});
-                points.add(new double[] {DrivetrainConstants.kMAX_SPEED_METERS_PER_SECOND, 0, 0});
-                points.add(new double[] {0, DrivetrainConstants.kMAX_ANGULAR_SPEED_RADIANS_PER_SECOND / 2, 0});
-                points.add(new double[] {0, DrivetrainConstants.kMAX_ANGULAR_SPEED_RADIANS_PER_SECOND, 0});
-                points.add(new double[] {DrivetrainConstants.kMAX_SPEED_METERS_PER_SECOND, DrivetrainConstants.kMAX_ANGULAR_SPEED_RADIANS_PER_SECOND, 0.635});
-                points.add(new double[] {DrivetrainConstants.kMAX_SPEED_METERS_PER_SECOND / 2, DrivetrainConstants.kMAX_ANGULAR_SPEED_RADIANS_PER_SECOND, 0.33});
-                points.add(new double[] {DrivetrainConstants.kMAX_SPEED_METERS_PER_SECOND, DrivetrainConstants.kMAX_ANGULAR_SPEED_RADIANS_PER_SECOND / 2, 0.4});
-                points.add(new double[] {DrivetrainConstants.kMAX_SPEED_METERS_PER_SECOND / 2, DrivetrainConstants.kMAX_ANGULAR_SPEED_RADIANS_PER_SECOND / 2, 0.15});
+                points.add(new double[] {DrivetrainConstants.kMAX_DRIVETRAIN_TRANSLATIONAL_VELOCITY_METERS_PER_SECOND / 2, 0, 0});
+                points.add(new double[] {DrivetrainConstants.kMAX_DRIVETRAIN_TRANSLATIONAL_VELOCITY_METERS_PER_SECOND, 0, 0});
+                points.add(new double[] {0, DrivetrainConstants.kMAX_DRIVETRAIN_ANGULAR_VELOCITY_RADIANS_PER_SECOND / 2, 0});
+                points.add(new double[] {0, DrivetrainConstants.kMAX_DRIVETRAIN_ANGULAR_VELOCITY_RADIANS_PER_SECOND, 0});
+                points.add(new double[] {DrivetrainConstants.kMAX_DRIVETRAIN_TRANSLATIONAL_VELOCITY_METERS_PER_SECOND, DrivetrainConstants.kMAX_DRIVETRAIN_ANGULAR_VELOCITY_RADIANS_PER_SECOND, 0.635});
+                points.add(new double[] {DrivetrainConstants.kMAX_DRIVETRAIN_TRANSLATIONAL_VELOCITY_METERS_PER_SECOND / 2, DrivetrainConstants.kMAX_DRIVETRAIN_ANGULAR_VELOCITY_RADIANS_PER_SECOND, 0.33});
+                points.add(new double[] {DrivetrainConstants.kMAX_DRIVETRAIN_TRANSLATIONAL_VELOCITY_METERS_PER_SECOND, DrivetrainConstants.kMAX_DRIVETRAIN_ANGULAR_VELOCITY_RADIANS_PER_SECOND / 2, 0.4});
+                points.add(new double[] {DrivetrainConstants.kMAX_DRIVETRAIN_TRANSLATIONAL_VELOCITY_METERS_PER_SECOND / 2, DrivetrainConstants.kMAX_DRIVETRAIN_ANGULAR_VELOCITY_RADIANS_PER_SECOND / 2, 0.15});
                 break;
         }
     }
@@ -158,11 +158,11 @@ private double distance(double[] a, double[] b) {
         double r;
         switch (Constants.currentMode) {
             case SIM:
-                r = Math.sqrt(Math.pow((a[0] - b[0]) / DrivetrainConstants.kMAX_SPEED_METERS_PER_SECOND, 2) + Math.pow((a[1] - b[1]) / DrivetrainConstants.kMAX_ANGULAR_SPEED_RADIANS_PER_SECOND, 2));
+                r = Math.sqrt(Math.pow((a[0] - b[0]) / DrivetrainConstants.kMAX_DRIVETRAIN_TRANSLATIONAL_VELOCITY_METERS_PER_SECOND, 2) + Math.pow((a[1] - b[1]) / DrivetrainConstants.kMAX_DRIVETRAIN_ANGULAR_VELOCITY_RADIANS_PER_SECOND, 2));
                 break;
 
             default:
-                r = Math.sqrt(Math.pow((a[0] - b[0]) / DrivetrainConstants.kMAX_SPEED_METERS_PER_SECOND, 2) + Math.pow((a[1] - b[1]) / DrivetrainConstants.kMAX_ANGULAR_SPEED_RADIANS_PER_SECOND, 2));
+                r = Math.sqrt(Math.pow((a[0] - b[0]) / DrivetrainConstants.kMAX_DRIVETRAIN_TRANSLATIONAL_VELOCITY_METERS_PER_SECOND, 2) + Math.pow((a[1] - b[1]) / DrivetrainConstants.kMAX_DRIVETRAIN_ANGULAR_VELOCITY_RADIANS_PER_SECOND, 2));
                 break;
         }
 

src/main/java/frc/robot/subsystems/drivetrain/swerve/SwerveDrive.java

@@ -10,8 +10,8 @@
 
 import edu.wpi.first.math.Pair;
 import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.estimator.PoseEstimator;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
+import edu.wpi.first.math.filter.SlewRateLimiter;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Rotation3d;
@@ -31,6 +31,8 @@
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism;
 import frc.robot.Constants;
+import frc.robot.Constants.DrivetrainConstants;
+import frc.robot.lib.util.RebelUtil;
 
 public class SwerveDrive extends SubsystemBase {
 
@@ -92,6 +94,23 @@ public class SwerveDrive extends SubsystemBase {
 
     private Queue<Pair<Pose2d,Double>> poseQueue = new LinkedList<Pair<Pose2d,Double>>();
 
+    private final SlewRateLimiter vxSlewRateLimiter = 
+        new SlewRateLimiter(
+            Constants.DrivetrainConstants.kMAX_DRIVETRAIN_TRANSLATIONAL_ACCELERATION_METERS_PER_SECOND_SQUARED, 
+            Constants.DrivetrainConstants.kMAX_DRIVETRAIN_TRANSLATIONAL_DECELERATION_METERS_PER_SECOND_SQUARED,
+            0);
+    private final SlewRateLimiter vySlewRateLimiter = 
+        new SlewRateLimiter(
+            Constants.DrivetrainConstants.kMAX_DRIVETRAIN_TRANSLATIONAL_ACCELERATION_METERS_PER_SECOND_SQUARED, 
+            Constants.DrivetrainConstants.kMAX_DRIVETRAIN_TRANSLATIONAL_DECELERATION_METERS_PER_SECOND_SQUARED,
+            0);
+    private final SlewRateLimiter omegaSlewRateLimiter = 
+        new SlewRateLimiter(
+            Constants.DrivetrainConstants.kMAX_DRIVETRAIN_ANGULAR_ACCELERATION_RADIANS_PER_SECOND_SQUARED, 
+            Constants.DrivetrainConstants.kMAX_DRIVETRAIN_ANGULAR_DECELERATION_RADIANS_PER_SECOND_SQUARED,
+            0);
+    private final SlewRateLimiter[] moduleDriveSlewRateLimiters = new SlewRateLimiter[4];
+
     public SwerveDrive() {
         switch (Constants.currentMode) {
             case SIM:
@@ -148,6 +167,14 @@ public SwerveDrive() {
         odometryThread = new Notifier(this::updateOdometry);
         odometryThread.startPeriodic(0.02);
 
+        for (int i = 0; i < 4; i++) {
+            moduleDriveSlewRateLimiters[i] = 
+                new SlewRateLimiter(
+                    Constants.DrivetrainConstants.kMAX_MODULE_DRIVE_ACCELERATION_METERS_PER_SECOND, 
+                    Constants.DrivetrainConstants.kMAX_MODULE_DRIVE_DECELERATION_METERS_PER_SECOND,
+                    0);
+        }
+
     }
 
     private void updateInputs() {
@@ -193,18 +220,6 @@ public void periodic() {
         if (Timer.getFPGATimestamp() - poseQueue.peek().getSecond().doubleValue() > 1) {
             poseQueue.poll();
         }
-
-        // if (DriverStation.isAutonomous()) {
-        //     // type PathPlannerAuto
-        //     String name = RobotContainer.getInstance().getSelectedAuto();
-        //     // PathPlannerAuto auto = (PathPlannerAuto) RobotContainer.getInstance().getAutonomousCommand();
-        //     // auto.initialize();
-        //     // List<PathPlannerPath> paths = PathPlannerAuto.getPathGroupFromAutoFile(name);
-
-        //     Command auto = AutoBuilder.buildAuto(name);
-        //     CommandScheduler.getInstance().
-
-        // }
 
         m_poseEstimator.update(yaw, new SwerveModulePosition[] {
             new SwerveModulePosition(moduleInputs[0].drivePositionMeters, new Rotation2d(moduleInputs[0].anglePositionRad)),
@@ -228,6 +243,26 @@ public void periodic() {
             m_poseEstimator.getEstimatedPosition().getRotation().getRadians()
         });
 
+        desiredFieldRelativeSpeeds.vxMetersPerSecond = 
+            RebelUtil.constrain(
+                desiredFieldRelativeSpeeds.vxMetersPerSecond,
+                -DrivetrainConstants.kMAX_DRIVETRAIN_TRANSLATIONAL_VELOCITY_METERS_PER_SECOND,
+                DrivetrainConstants.kMAX_DRIVETRAIN_TRANSLATIONAL_VELOCITY_METERS_PER_SECOND);
+        desiredFieldRelativeSpeeds.vyMetersPerSecond = 
+            RebelUtil.constrain(
+                desiredFieldRelativeSpeeds.vyMetersPerSecond,
+                -DrivetrainConstants.kMAX_DRIVETRAIN_TRANSLATIONAL_VELOCITY_METERS_PER_SECOND,
+                DrivetrainConstants.kMAX_DRIVETRAIN_TRANSLATIONAL_VELOCITY_METERS_PER_SECOND);
+        desiredFieldRelativeSpeeds.omegaRadiansPerSecond = 
+            RebelUtil.constrain(
+                desiredFieldRelativeSpeeds.omegaRadiansPerSecond,
+                -DrivetrainConstants.kMAX_DRIVETRAIN_ANGULAR_VELOCITY_RADIANS_PER_SECOND,
+                DrivetrainConstants.kMAX_DRIVETRAIN_ANGULAR_VELOCITY_RADIANS_PER_SECOND);
+
+        desiredFieldRelativeSpeeds.vxMetersPerSecond = vxSlewRateLimiter.calculate(desiredFieldRelativeSpeeds.vxMetersPerSecond);
+        desiredFieldRelativeSpeeds.vyMetersPerSecond = vySlewRateLimiter.calculate(desiredFieldRelativeSpeeds.vyMetersPerSecond);
+        desiredFieldRelativeSpeeds.omegaRadiansPerSecond = omegaSlewRateLimiter.calculate(desiredFieldRelativeSpeeds.omegaRadiansPerSecond);
+
         desiredRobotRelativeSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(desiredFieldRelativeSpeeds, getPose().getRotation());
         Logger.recordOutput("SwerveDrive/desiredRobotRelativeSpeeds", desiredRobotRelativeSpeeds);
         Logger.recordOutput("SwerveDrive/measuredRobotRelativeSpeeds", measuredRobotRelativeSpeeds);
@@ -247,6 +282,7 @@ public void periodic() {
                                             desiredFieldRelativeSpeeds.vxMetersPerSecond)*/;                                       
         correctedSpeeds.vyMetersPerSecond = correctedSpeeds.vyMetersPerSecond - estimatedVyDriftMetersPerSecond;
         correctedSpeeds.omegaRadiansPerSecond = correctedSpeeds.omegaRadiansPerSecond /* + m_angleFeedbackController.calculate(measuredFieldRelativeSpeeds.omegaRadiansPerSecond, desiredFieldRelativeSpeeds.omegaRadiansPerSecond)*/;
+
         correctedSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(correctedSpeeds, getPose().getRotation());
 
         Logger.recordOutput("SwerveDrive/correctedSpeeds", correctedSpeeds);
@@ -264,6 +300,12 @@ public void periodic() {
             Logger.recordOutput("SwerveDrive/unoptimizedDesiredModuleStates", desiredModuleStates);
 
             desiredModuleStates[i] = SwerveModuleState.optimize(desiredModuleStates[i], measuredModuleStates[i].angle);
+            desiredModuleStates[i].speedMetersPerSecond = 
+                RebelUtil.constrain(
+                    desiredModuleStates[i].speedMetersPerSecond, 
+                    -DrivetrainConstants.kMAX_MODULE_DRIVE_VELOCITY_METERS_PER_SECOND, 
+                    DrivetrainConstants.kMAX_MODULE_DRIVE_VELOCITY_METERS_PER_SECOND);
+            desiredModuleStates[i].speedMetersPerSecond = moduleDriveSlewRateLimiters[i].calculate(desiredModuleStates[i].speedMetersPerSecond);
             modules[i].setState(desiredModuleStates[i]);
         }
 
@@ -319,7 +361,6 @@ public Pose2d getPose() {
 
     public Pose2d getPoseAtTimestamp(double time) {
         double lowestError = Double.MAX_VALUE;
-        double currentTime = Timer.getFPGATimestamp();
         Pose2d pose = poseQueue.peek().getFirst();
         Logger.recordOutput("SwerveDrive/queueLength", poseQueue.size());
         for (Pair<Pose2d, Double> pair : poseQueue) {