pathplannerupdates

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

@@ -8,6 +8,10 @@
 import edu.wpi.first.math.util.Units;
 import frc.lib.config.SwerveModuleConstants;
 
+import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
+import com.pathplanner.lib.util.PIDConstants;
+import com.pathplanner.lib.util.ReplanningConfig;
+
 public final class Constants {
 
     public static final class ModuleConstants {
@@ -197,6 +201,14 @@ public static final class Mod3 {
             public static final SwerveModuleConstants constants =
                     new SwerveModuleConstants(driveMotorID, angleMotorID, angleOffset);
         }
+        public static final double maxModuleSpeed = 4.5; // M/S
+        public static final HolonomicPathFollowerConfig pathFollowerConfig = new HolonomicPathFollowerConfig(
+                new PIDConstants(5.0, 0, 0), // Translation constants 
+                new PIDConstants(5.0, 0, 0), // Rotation constants 
+                maxModuleSpeed, 
+                .6, // Drive base radius (distance from center to furthest module) 
+                new ReplanningConfig()
+              );
     }
 
     public static final class AutoConstants {

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

@@ -8,6 +8,7 @@
 import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -44,6 +45,26 @@ public SwerveSubsystem() {
 
         //Odomentry with our kinematics object from constants, gyro position and x/y position of each module
 
+         AutoBuilder.configureHolonomic(
+      this::getPose, 
+      this::resetOdometry, 
+      this::getSpeeds, 
+      this::driveRobotRelative, 
+      Constants.Swerve.pathFollowerConfig,
+      () -> {
+          // Boolean supplier that controls when the path will be mirrored for the red alliance
+          // This will flip the path being followed to the red side of the field.
+          // THE ORIGIN WILL REMAIN ON THE BLUE SIDE
+
+          var alliance = DriverStation.getAlliance();
+          if (alliance.isPresent()) {
+              return alliance.get() == DriverStation.Alliance.Red;
+          }
+          return false;
+      },
+      this
+    );
+
         field = new Field2d();
         SmartDashboard.putData("Field", field);
     }
@@ -92,6 +113,20 @@ public Pose2d getPose() {
     public void resetOdometry(Pose2d pose) {
         swerveOdometry.resetPosition(getYaw(), getPositions(), pose);
     }
+    public ChassisSpeeds getSpeeds() {
+        return Constants.Swerve.swerveKinematics.toChassisSpeeds(getStates());
+      }
+
+      public void driveFieldRelative(ChassisSpeeds fieldRelativeSpeeds) {
+        driveRobotRelative(ChassisSpeeds.fromFieldRelativeSpeeds(fieldRelativeSpeeds, getPose().getRotation()));
+      }
+
+      public void driveRobotRelative(ChassisSpeeds robotRelativeSpeeds) {
+        ChassisSpeeds targetSpeeds = ChassisSpeeds.discretize(robotRelativeSpeeds, 0.02);
+
+        SwerveModuleState[] targetStates = Constants.Swerve.swerveKinematics.toSwerveModuleStates(targetSpeeds);
+        setModuleStates(targetStates);
+      }
 
     public SwerveModuleState[] getStates() {
         SwerveModuleState[] states = new SwerveModuleState[4];
@@ -153,6 +188,7 @@ public double getRoll() {
         return 0.0;//gyro.getRoll();
     }
 
+
     // SmartDashboard.putNumber("gyro filtered X", gyro.getXFilteredAccelAngle()); // loops between
     // about 14...0...360...346
     // SmartDashboard.putNumber("gyro filtered Y", gyro.getYFilteredAccelAngle()); // forward and back