diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/New Auto.auto
new file mode 100644
index 0000000..70b7ab2
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/New Auto.auto	
@@ -0,0 +1,19 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Example Path"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json
new file mode 100644
index 0000000..23e0db9
--- /dev/null
+++ b/src/main/deploy/pathplanner/navgrid.json
@@ -0,0 +1 @@
+{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path
new file mode 100644
index 0000000..946269c
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Example Path.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.415049342105263,
+        "y": 4.785115131578947
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.905756578947368,
+        "y": 4.794736842105262
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.8294407894736837,
+        "y": 5.862746710526316
+      },
+      "prevControl": {
+        "x": 3.0693256578947365,
+        "y": 5.872368421052631
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -2.4366482468102095
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json
new file mode 100644
index 0000000..16b0598
--- /dev/null
+++ b/src/main/deploy/pathplanner/settings.json
@@ -0,0 +1,32 @@
+{
+  "robotWidth": 0.9,
+  "robotLength": 0.9,
+  "holonomicMode": true,
+  "pathFolders": [],
+  "autoFolders": [],
+  "defaultMaxVel": 3.0,
+  "defaultMaxAccel": 3.0,
+  "defaultMaxAngVel": 540.0,
+  "defaultMaxAngAccel": 720.0,
+  "defaultNominalVoltage": 12.0,
+  "robotMass": 22.6796,
+  "robotMOI": 6.883,
+  "robotTrackwidth": 0.546,
+  "driveWheelRadius": 0.048,
+  "driveGearing": 5.143,
+  "maxDriveSpeed": 5.45,
+  "driveMotorType": "krakenX60",
+  "driveCurrentLimit": 60.0,
+  "wheelCOF": 1.2,
+  "flModuleX": 0.273,
+  "flModuleY": 0.273,
+  "frModuleX": 0.273,
+  "frModuleY": -0.273,
+  "blModuleX": -0.273,
+  "blModuleY": 0.273,
+  "brModuleX": -0.273,
+  "brModuleY": -0.273,
+  "bumperOffsetX": 0.0,
+  "bumperOffsetY": 0.0,
+  "robotFeatures": []
+}
\ No newline at end of file
diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 0108060..249a881 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -99,42 +99,42 @@ public final class Constants {
         public static final double MAX_SPEED_FEET_PER_SECOND = MAX_SPEED_MEETERS_PER_SEC * 3.28084;
         public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI;
 
-        public static final double FORWARD_OFFSET = 0; // 0, 90, 180, 270
+        public static final double FORWARD_OFFSET = 90; // 0, 90, 180, 270
 
         public static final Trim POINTLESS_TRIM = new Trim("Pointless Trim", Double.MAX_VALUE, Double.MIN_VALUE, 1.d, 10);
 
-        private static final class ModuleSpecificConstants {
+        private static final class ModuleSpecificConstants { //2025
             //Front Left
-            private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.229736328125+.25);
-            private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false;
+            private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.229736328125);
+            private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = true;
             private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = true;
             private static final boolean FRONT_LEFT_ENCODER_INVERTED = false;
-            private static final Distance FRONT_LEFT_XPOS = Inches.of(-HALF_WIDTH);
-            private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_HEIGHT);
+            private static final Distance FRONT_LEFT_XPOS = Inches.of(HALF_HEIGHT);
+            private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_WIDTH);
             
             //Front Right
-            private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(-0.371337890625+.5+.25);
-            private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = false;
+            private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(-0.371337890625+.5);
+            private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = true;
             private static final boolean FRONT_RIGHT_STEER_MOTOR_INVERTED = true;
             private static final boolean FRONT_RIGHT_ENCODER_INVERTED = false;
-            private static final Distance FRONT_RIGHT_XPOS = Inches.of(HALF_WIDTH);
-            private static final Distance FRONT_RIGHT_YPOS = Inches.of(HALF_HEIGHT);
+            private static final Distance FRONT_RIGHT_XPOS = Inches.of(HALF_HEIGHT);
+            private static final Distance FRONT_RIGHT_YPOS = Inches.of(-HALF_WIDTH);
 
             //Back Left
-            private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.3828125+.25+.5);
-            private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = false;
+            private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.3828125+.5);
+            private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = true;
             private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = true;
             private static final boolean BACK_LEFT_ENCODER_INVERTED = false;
-            private static final Distance BACK_LEFT_XPOS = Inches.of(-HALF_WIDTH);
-            private static final Distance BACK_LEFT_YPOS = Inches.of(-HALF_HEIGHT);
+            private static final Distance BACK_LEFT_XPOS = Inches.of(-HALF_HEIGHT);
+            private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_WIDTH);
             
             //Back Right
-            private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(-0.01904296875+.25);
-            private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = false;
+            private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(-0.07666015625);
+            private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = true;
             private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = true;
             private static final boolean BACK_RIGHT_ENCODER_INVERTED = false;
-            private static final Distance BACK_RIGHT_XPOS = Inches.of(HALF_WIDTH);
-            private static final Distance BACK_RIGHT_YPOS = Inches.of(-HALF_HEIGHT);
+            private static final Distance BACK_RIGHT_XPOS = Inches.of(-HALF_HEIGHT);
+            private static final Distance BACK_RIGHT_YPOS = Inches.of(-HALF_WIDTH);
         }
 
         public static final class IDs {
@@ -326,4 +326,4 @@ public final class Constants {
         public static final double LEFT_AXIS_DEADBAND = 0.1;
 
     }
-}
\ No newline at end of file
+}
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index b127f61..ecbc21b 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -28,13 +28,14 @@ import edu.wpi.first.wpilibj2.command.button.Trigger;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.Commands;
 
 // Autos
 import frc4388.utility.controller.VirtualController;
 import frc4388.robot.commands.GotoPositionCommand;
 import frc4388.robot.commands.Swerve.neoJoystickPlayback;
 import frc4388.robot.commands.Swerve.neoJoystickRecorder;
-
+import com.pathplanner.lib.commands.PathPlannerAuto;
 // Subsystems
 // import frc4388.robot.subsystems.LED;
 import frc4388.robot.subsystems.Vision;
@@ -101,7 +102,7 @@ public class RobotContainer {
         m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
             m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(),
                                             getDeadbandedDriverController().getRight(),
-                                false);
+                                true);
         }, m_robotSwerveDrive)
         .withName("SwerveDrive DefaultCommand"));
         m_robotSwerveDrive.setToSlow();
@@ -152,18 +153,7 @@ public class RobotContainer {
 
         DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.A_BUTTON)
             .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro()));
-        
-        // @ /* Trim Test Buttons */
-        
-        new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
-            .onTrue(new InstantCommand(() -> SwerveDriveConstants.POINTLESS_TRIM.stepUp()));
-        
-        new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
-            .onTrue(new InstantCommand(() -> SwerveDriveConstants.POINTLESS_TRIM.stepDown()));
-        
-        new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
-            .onTrue(new InstantCommand(() -> SwerveDriveConstants.POINTLESS_TRIM.load()));
-
+            
         // ! /* Speed */
         new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final
             .onTrue(new InstantCommand(()  -> m_robotSwerveDrive.shiftUp()));
@@ -236,9 +226,21 @@ public class RobotContainer {
      */
     public Command getAutonomousCommand() {
         //return autoPlayback;
-        return new GotoPositionCommand(m_robotSwerveDrive, m_vision, AutoConstants.targetpos);
+        //return new GotoPositionCommand(m_robotSwerveDrive, m_vision);
+	try{
+	    // Load the path you want to follow using its name in the GUI
+	    return new PathPlannerAuto("New Auto");
+	} catch (Exception e) {
+	    DriverStation.reportError("Big oops: " + e.getMessage(), e.getStackTrace());
+	    return Commands.none();
+	}
+	// zach told me to do the below comment
+	//return new GotoPositionCommand(m_robotSwerveDrive, m_vision);
+      //  return new GotoPositionCommand(m_robotSwerveDrive, m_vision, AutoConstants.targetpos);
     }
 
+
+
     /**
      * A button binding for two controllers, preferably an {@link DeadbandedXboxController Xbox Controller} and {@link VirtualController Virtual Xbox Controller}
      * @param joystickA A controller
diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
index 14bf6a8..65db294 100644
--- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
@@ -5,6 +5,7 @@
 package frc4388.robot.subsystems;
 
 import java.util.Optional;
+import java.util.function.DoubleSupplier;
 
 import com.ctre.phoenix6.Utils;
 import com.ctre.phoenix6.hardware.CANcoder;
@@ -30,258 +31,304 @@ import frc4388.robot.Constants.VisionConstants;
 import frc4388.utility.Status;
 import frc4388.utility.Subsystem;
 import frc4388.utility.Status.ReportLevel;
+import edu.wpi.first.wpilibj.DriverStation;
+
+import com.pathplanner.lib.controllers.PPHolonomicDriveController;
+import com.pathplanner.lib.config.PIDConstants;
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.commands.PathPlannerAuto;
+import com.pathplanner.lib.config.RobotConfig;
 
 public class SwerveDrive extends Subsystem {
-  private SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain;
+    private SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain;
 
-  private Vision vision;
+    private Vision vision;
   
-  private int gear_index = SwerveDriveConstants.STARTING_GEAR;
-  private boolean stopped = false;
+    private int gear_index = SwerveDriveConstants.STARTING_GEAR;
+    private boolean stopped = false;
 
-  public double speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * SwerveDriveConstants.GEARS[gear_index];
-  public double rotSpeedAdjust = SwerveDriveConstants.MAX_ROT_SPEED;
-  public double autoSpeedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * 0.25; // cap auto performance to 25%
+    public double speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * SwerveDriveConstants.GEARS[gear_index];
+    public double rotSpeedAdjust = SwerveDriveConstants.MAX_ROT_SPEED;
+    public double autoSpeedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * 0.25; // cap auto performance to 25%
   
-  public double rotTarget = 0.0;
-  public Rotation2d orientRotTarget = new Rotation2d();
-  public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
+    public double rotTarget = 0.0;
+    public Rotation2d orientRotTarget = new Rotation2d();
+    public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
 
-  /** Creates a new SwerveDrive. */
-  public SwerveDrive(SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain, Vision vision) {
-    // public SwerveDrive(SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain) {
-    super();
+    /** Creates a new SwerveDrive. */
+    public SwerveDrive(SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain, Vision vision) {
+	// public SwerveDrive(SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain) {
+	super();
 
-    this.swerveDriveTrain = swerveDriveTrain;
-    this.vision = vision;
-  }
+	this.swerveDriveTrain = swerveDriveTrain;
+	this.vision = vision;
 
-  // public void oneModuleTest(SwerveModule module, Translation2d leftStick, Translation2d rightStick){
-  //   // double ang = Math.atan2(rightStick.getY(), rightStick.getX());
-  //   // rightStick.getAngle()
-  //   double speed = Math.sqrt(Math.pow(leftStick.getX(), 2) + Math.pow(leftStick.getY(), 2));
-  //   // System.out.println(ang);
-  //   // module.go(ang);
-  //   // Rotation2d rot = Rotation2d.fromRadians(ang);
-  //   Rotation2d rot = new Rotation2d(rightStick.getX(), rightStick.getY());
-  //   SwerveModuleState state = new SwerveModuleState(speed, rot);
-  //   module.setDesiredState(state);
-  // }
+	RobotConfig config;
+	try{
+	    config = RobotConfig.fromGUISettings();
+	} catch (Exception e) {
+	    // Handle exception as needed
+	    config = null;
+	}
+        // DoubleSupplier a = () -> 1.d;
+        AutoBuilder.configure(
+			      () -> {
+                    var pose = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(new Pose2d());
+                    // pose = new Pose2d(pose.getX(), pose.getY(), pose.getRotation().times(-1));
+                    return pose;//getRotation().times(-1)
+                  }, // Robot pose supplier
+			      swerveDriveTrain::resetPose, // Method to reset odometry (will be called if your auto has a starting pose)
+			      () -> swerveDriveTrain.getState().Speeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
+			      (speeds, feedforwards) -> swerveDriveTrain.setControl(new SwerveRequest.ApplyRobotSpeeds()
+										    .withSpeeds(speeds)
+										    ), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also optionally outputs individual module feedforwards
+			      new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for holonomic drive trains
+							      new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants
+							      new PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants
+							      ),
+			      config, // The robot configuration
+			      () -> {
+				  // 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
 
-  public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
-    if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going:
-      stopModules(); // stop the swerve
+				  var alliance = DriverStation.getAlliance();
+				  if (alliance.isPresent()) {
+				      return alliance.get() == DriverStation.Alliance.Red;
+				  }
+				  return false;
+			      },
+			      this // Reference to this subsystem to set requirements
+			      );
+
+	    }
+
+    // public void oneModuleTest(SwerveModule module, Translation2d leftStick, Translation2d rightStick){
+    //   // double ang = Math.atan2(rightStick.getY(), rightStick.getX());
+    //   // rightStick.getAngle()
+    //   double speed = Math.sqrt(Math.pow(leftStick.getX(), 2) + Math.pow(leftStick.getY(), 2));
+    //   // System.out.println(ang);
+    //   // module.go(ang);
+    //   // Rotation2d rot = Rotation2d.fromRadians(ang);
+    //   Rotation2d rot = new Rotation2d(rightStick.getX(), rightStick.getY());
+    //   SwerveModuleState state = new SwerveModuleState(speed, rot);
+    //   module.setDesiredState(state);
+    // }
+
+    public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
+	if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going:
+	    stopModules(); // stop the swerve
     
-    if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) //if no imput
-      return; // don't bother doing swerve drive math and return early.
+	if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) //if no imput
+	    return; // don't bother doing swerve drive math and return early.
 
-    leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET));
+	leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET));
     
-    if (fieldRelative) {
-      swerveDriveTrain.setControl(new SwerveRequest.FieldCentric()
-        .withVelocityX(leftStick.getX()*speedAdjust)
-        .withVelocityY(leftStick.getY()*speedAdjust)
-        .withRotationalRate(rightStick.getX()*rotSpeedAdjust)
-      );
-      // double rot = 0;
+	if (fieldRelative) {
+	    swerveDriveTrain.setControl(new SwerveRequest.FieldCentric()
+					.withVelocityX(leftStick.getX()*speedAdjust)
+					.withVelocityY(-leftStick.getY()*speedAdjust)
+					.withRotationalRate(rightStick.getX()*rotSpeedAdjust)
+					);
+	    // double rot = 0;
       
-      // ! drift correction
-      // dependant on if the new odomitry system acounts for rotational drift, this may not be needed.
-    //   if (rightStick.getNorm() > 0.05) {
-    //     rotTarget = swerveDriveTrain.getRotation3d().toRotation2d().getDegrees();
-    //     swerveDriveTrain.setControl(new SwerveRequest.FieldCentric()
-    //       .withVelocityX(leftStick.getX()*speedAdjust)
-    //       .withVelocityY(leftStick.getY()*speedAdjust)
-    //       .withRotationalRate(rightStick.getY()*rotSpeedAdjust)
-    //     );
+	    // ! drift correction
+	    // dependant on if the new odomitry system acounts for rotational drift, this may not be needed.
+	    //   if (rightStick.getNorm() > 0.05) {
+	    //     rotTarget = swerveDriveTrain.getRotation3d().toRotation2d().getDegrees();
+	    //     swerveDriveTrain.setControl(new SwerveRequest.FieldCentric()
+	    //       .withVelocityX(leftStick.getX()*speedAdjust)
+	    //       .withVelocityY(leftStick.getY()*speedAdjust)
+	    //       .withRotationalRate(rightStick.getY()*rotSpeedAdjust)
+	    //     );
       
-    //   //  SmartDashboard.putBoolean("drift correction", false);
-    //     stopped = false;
-    //   } else if(leftStick.getNorm() > 0.05) {
-    //     if (!stopped) {
-    //       stopModules();
-    //       stopped = true;
-    //     }
+	    //   //  SmartDashboard.putBoolean("drift correction", false);
+	    //     stopped = false;
+	    //   } else if(leftStick.getNorm() > 0.05) {
+	    //     if (!stopped) {
+	    //       stopModules();
+	    //       stopped = true;
+	    //     }
 
-    //  //   SmartDashboard.putBoolean("drift correction", true);
+	    //  //   SmartDashboard.putBoolean("drift correction", true);
         
-    //     // rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
+	    //     // rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
 
-    //   }
+	    //   }
 
-    //   // Use the left joystick to set speed. Apply a cubic curve and the set max speed.
-    //   Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
-    //   // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00));
+	    //   // Use the left joystick to set speed. Apply a cubic curve and the set max speed.
+	    //   Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
+	    //   // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00));
 
-    //   // Convert field-relative speeds to robot-relative speeds.
-    //   // chassisSpeeds = chassisSpeeds.
-    //   chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), (-1 * rightStick.getX() * rotSpeedAdjust) - rot_correction, gyro.getRotation2d().times(-1));
-    } else {      // Create robot-relative speeds.
-      swerveDriveTrain.setControl(new SwerveRequest.RobotCentric()
-        .withVelocityX(leftStick.getX()*-speedAdjust)
-        .withVelocityY(leftStick.getY()*speedAdjust)
-        .withRotationalRate(rightStick.getX()*rotSpeedAdjust)
-      );
+	    //   // Convert field-relative speeds to robot-relative speeds.
+	    //   // chassisSpeeds = chassisSpeeds.
+	    //   chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), (-1 * rightStick.getX() * rotSpeedAdjust) - rot_correction, gyro.getRotation2d().times(-1));
+	} else {      // Create robot-relative speeds.
+	    swerveDriveTrain.setControl(new SwerveRequest.RobotCentric()
+					.withVelocityX(leftStick.getX()*speedAdjust)
+					.withVelocityY(-leftStick.getY()*speedAdjust)
+					.withRotationalRate(rightStick.getX()*rotSpeedAdjust)
+					);
+	}
     }
-  }
 
-  public void driveWithInputOrientation(Translation2d leftStick, Translation2d rightStick) { // there is no practical reason to have a robot relitive version of this, and no pre provided version 
-    if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going:
-      stopModules(); // stop the swerve
+    public void driveWithInputOrientation(Translation2d leftStick, Translation2d rightStick) { // there is no practical reason to have a robot relitive version of this, and no pre provided version 
+	if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going:
+	    stopModules(); // stop the swerve
   
-    if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) //if no imput
-      return; // don't bother doing swerve drive math and return early.
+	if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) //if no imput
+	    return; // don't bother doing swerve drive math and return early.
 
-    leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET));
+	leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET));
     
-    swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
-      .withVelocityX(leftStick.getX()*speedAdjust)
-      .withVelocityY(leftStick.getY()*speedAdjust)
-      .withTargetDirection(rightStick.getAngle())
-    ); 
-  }
-
-  public boolean rotateToTarget(double angle) {
-    swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
-      .withVelocityX(0)
-      .withVelocityY(0)
-      .withTargetDirection(Rotation2d.fromDegrees(angle))
-    );
-
-    if (Math.abs(angle - getGyroAngle()) < 5.0) {
-      return true;
+	swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
+				    .withVelocityX(leftStick.getX()*speedAdjust)
+				    .withVelocityY(leftStick.getY()*speedAdjust)
+				    .withTargetDirection(rightStick.getAngle())
+				    ); 
     }
 
-    return false;
-  }
+    public boolean rotateToTarget(double angle) {
+	swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
+				    .withVelocityX(0)
+				    .withVelocityY(0)
+				    .withTargetDirection(Rotation2d.fromDegrees(angle))
+				    );
 
-  public void driveWithInputRotation(Translation2d leftStick, Rotation2d rot) {
-    // if (leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going:
-    //   stopModules(); // stop the swerve
-    
-    // if (leftStick.getNorm() < 0.05) //if no imput
-    //   return; // don't bother doing swerve drive math and return early.
+	if (Math.abs(angle - getGyroAngle()) < 5.0) {
+	    return true;
+	}
 
-    leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET));
-    
-    swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
-      .withVelocityX(leftStick.getX()*-speedAdjust)
-      .withVelocityY(leftStick.getY()*speedAdjust)
-      .withTargetDirection(rot)
-    );
-      // double
-  }
-
-  public double getGyroAngle() {
-    return swerveDriveTrain.getRotation3d().getAngle();
-  }
-
-  public void resetGyro() {
-    swerveDriveTrain.tareEverything();
-  }
-
-  public void stopModules() {
-    // swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake());
-  }
-
-  @Override
-  public void periodic() {
-    // This method will be called once per scheduler run\
-    SmartDashboard.putNumber("Gyro", getGyroAngle());
-    SmartDashboard.putNumber("RotTartget", rotTarget);
-
-    double time = Vision.getTime();
-
-    vision.setLastOdomPose(swerveDriveTrain.samplePoseAt(time));
-
-    if(vision.isTag()){
-      swerveDriveTrain.addVisionMeasurement(vision.getPose2d(), time);
+	return false;
     }
 
-    // if(e.isPresent())
-  }
+    public void driveWithInputRotation(Translation2d leftStick, Rotation2d rot) {
+	// if (leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going:
+	//   stopModules(); // stop the swerve
+    
+	// if (leftStick.getNorm() < 0.05) //if no imput
+	//   return; // don't bother doing swerve drive math and return early.
 
-  private void reset_index() {
-    gear_index = SwerveDriveConstants.STARTING_GEAR; // however we wish to initialize the gear (What gear does the robot start in?)
-  }
+	leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET));
+    
+	swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
+				    .withVelocityX(leftStick.getX()*-speedAdjust)
+				    .withVelocityY(leftStick.getY()*speedAdjust)
+				    .withTargetDirection(rot)
+				    );
+	// double
+    }
 
-  public void shiftDown() {
-    if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index
-    int i = gear_index - 1;
-    if (i == -1) i = 0;
-    setPercentOutput(SwerveDriveConstants.GEARS[i]);
-    gear_index = i;
-  }
+    public double getGyroAngle() {
+	return swerveDriveTrain.getRotation3d().getAngle();
+    }
 
-  public void shiftUp() {
-    if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index
-    int i = gear_index + 1;
-    if (i == SwerveDriveConstants.GEARS.length) i = SwerveDriveConstants.GEARS.length - 1;
-    setPercentOutput(SwerveDriveConstants.GEARS[i]);
-    gear_index = i;
-  }
+    public void resetGyro() {
+	swerveDriveTrain.tareEverything();
+    }
 
-  public void setPercentOutput(double speed) {
-    speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed;
-    gear_index = -1;
-  }
+    public void stopModules() {
+	swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake());
+    }
 
-  public void setToSlow() {
-    setPercentOutput(SwerveDriveConstants.SLOW_SPEED);
-    gear_index = 0;
-  }
+    @Override
+    public void periodic() {
+	// This method will be called once per scheduler run\
+	SmartDashboard.putNumber("Gyro", getGyroAngle());
+	SmartDashboard.putNumber("RotTartget", rotTarget);
 
-  public void setToFast() {
-    setPercentOutput(SwerveDriveConstants.FAST_SPEED);
-    gear_index = 1;
-  }
+	double time = Vision.getTime();
 
-  public void setToTurbo() {
-    setPercentOutput(SwerveDriveConstants.TURBO_SPEED);
-    gear_index = 2;
-  }
+	vision.setLastOdomPose(swerveDriveTrain.samplePoseAt(time));
 
-  public void shiftUpRot() {
-    rotSpeedAdjust = SwerveDriveConstants.ROTATION_SPEED;
-  }
+	if(vision.isTag()){
+	    swerveDriveTrain.addVisionMeasurement(vision.getPose2d(), time);
+	}
 
-  public void shiftDownRot() {
-    rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED;
-  }
+	// if(e.isPresent())
+    }
 
-  @Override
-  public String getSubsystemName() {
-    return "Swerve Drive Controller";
-  }
+    private void reset_index() {
+	gear_index = SwerveDriveConstants.STARTING_GEAR; // however we wish to initialize the gear (What gear does the robot start in?)
+    }
 
-  ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems")
-  .getLayout(getSubsystemName(), BuiltInLayouts.kList)
-  .withSize(2, 2);
+    public void shiftDown() {
+	if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index
+	int i = gear_index - 1;
+	if (i == -1) i = 0;
+	setPercentOutput(SwerveDriveConstants.GEARS[i]);
+	gear_index = i;
+    }
 
-  GenericEntry sbGyro = subsystemLayout
-  .add("Gyro angle", 0)
-  .withWidget(BuiltInWidgets.kGyro)
-  .getEntry();
+    public void shiftUp() {
+	if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index
+	int i = gear_index + 1;
+	if (i == SwerveDriveConstants.GEARS.length) i = SwerveDriveConstants.GEARS.length - 1;
+	setPercentOutput(SwerveDriveConstants.GEARS[i]);
+	gear_index = i;
+    }
 
-  GenericEntry sbShiftState = subsystemLayout
-  .add("Shift State", 0)
-  .withWidget(BuiltInWidgets.kNumberBar)
-  .getEntry();
+    public void setPercentOutput(double speed) {
+	speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed;
+	gear_index = -1;
+    }
 
-  @Override
-  public void queryStatus() {
-    sbGyro.setDouble(getGyroAngle());
-    sbShiftState.setDouble(this.speedAdjust);
+    public void setToSlow() {
+	setPercentOutput(SwerveDriveConstants.SLOW_SPEED);
+	gear_index = 0;
+    }
+
+    public void setToFast() {
+	setPercentOutput(SwerveDriveConstants.FAST_SPEED);
+	gear_index = 1;
+    }
+
+    public void setToTurbo() {
+	setPercentOutput(SwerveDriveConstants.TURBO_SPEED);
+	gear_index = 2;
+    }
+
+    public void shiftUpRot() {
+	rotSpeedAdjust = SwerveDriveConstants.ROTATION_SPEED;
+    }
+
+    public void shiftDownRot() {
+	rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED;
+    }
+
+    @Override
+    public String getSubsystemName() {
+	return "Swerve Drive Controller";
+    }
+
+    ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems")
+	.getLayout(getSubsystemName(), BuiltInLayouts.kList)
+	.withSize(2, 2);
+
+    GenericEntry sbGyro = subsystemLayout
+	.add("Gyro angle", 0)
+	.withWidget(BuiltInWidgets.kGyro)
+	.getEntry();
+
+    GenericEntry sbShiftState = subsystemLayout
+	.add("Shift State", 0)
+	.withWidget(BuiltInWidgets.kNumberBar)
+	.getEntry();
+
+    @Override
+    public void queryStatus() {
+	sbGyro.setDouble(getGyroAngle());
+	sbShiftState.setDouble(this.speedAdjust);
   
-    //TODO: Add more status things
-  }
+	//TODO: Add more status things
+    }
 
-  @Override
-  public Status diagnosticStatus() {
-    Status status = new Status();
+    @Override
+    public Status diagnosticStatus() {
+	Status status = new Status();
 
-    status.addReport(ReportLevel.ERROR, "Don't know how to diganose new CTRE swerve systems. please check under the CAN(t) section for more detailed information about the swerves there.");
+	status.addReport(ReportLevel.ERROR, "Don't know how to diganose new CTRE swerve systems. please check under the CAN(t) section for more detailed information about the swerves there.");
     
-    return status;
-  }
+	return status;
+    }
 }