AUTO rotate

src/main/java/frc4388/robot/RobotContainer.java

@@ -8,6 +8,8 @@
 package frc4388.robot;
 
 import java.io.File;
+
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.GenericHID;
@@ -104,11 +106,24 @@ public class RobotContainer {
 
         DriverStation.silenceJoystickConnectionWarning(true);
 
-                m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
+        m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
-            m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(),
+
-            // m_robotSwerveDrive.driveWithInput(new Translation2d(.4, 0),
+            // IF the driver is holding the aim button, aim the robot towards the hub
-                                            getDeadbandedDriverController().getRight(),
+            if(m_driverXbox.getRightTriggerAxis() > 0.5) {
-                                true);
+                // Aim 
+                Translation2d shootTarget = new Translation2d();
+                // new Rotation2()
+                Rotation2d ang = m_robotSwerveDrive.getPose2d().getTranslation().minus(shootTarget).getAngle();
+                m_robotSwerveDrive.driveFieldAngle(
+                    getDeadbandedDriverController().getLeft(),
+                    ang);
+            } else {
+                // Drive normally
+                m_robotSwerveDrive.driveWithInput(
+                    getDeadbandedDriverController().getLeft(),
+                    getDeadbandedDriverController().getRight(),true);
+            }
+
         }, m_robotSwerveDrive)
         .withName("SwerveDrive DefaultCommand"));
         m_robotSwerveDrive.setToSlow();
@@ -131,13 +146,24 @@ public class RobotContainer {
             .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro()));
 
             
-        new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
+        // new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
-            .onTrue(new RotTo45(m_robotSwerveDrive));
+        //     .onTrue(new RotTo45(m_robotSwerveDrive));
 
             
         new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
             .onTrue(new InstantCommand(() -> {m_robotSwerveDrive.softStop();}, m_robotSwerveDrive)); 
 
+        new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
+            .onTrue(new InstantCommand(()  -> m_robotSwerveDrive.shiftUp()));
+        
+        new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
+            .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
+
+        new JoystickButton(getDeadbandedDriverController(), XboxController.START_BUTTON)
+            .onTrue(new InstantCommand(()  -> m_robotSwerveDrive.activateLuigiMode()));
+        
+        new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON)
+            .onTrue(new InstantCommand(()  -> m_robotSwerveDrive.deactivateLuigiMode()));
     }
 
     /**

src/main/java/frc4388/robot/constants/BuildConstants.java

@@ -7,12 +7,12 @@ public final class BuildConstants {
   public static final String MAVEN_GROUP = "";
   public static final String MAVEN_NAME = "2026KPopRobotHunters";
   public static final String VERSION = "unspecified";
-  public static final int GIT_REVISION = 3;
+  public static final int GIT_REVISION = 7;
-  public static final String GIT_SHA = "8dbb9d5a9099f417617ec2245e275a42b6788116";
+  public static final String GIT_SHA = "24fdd610c9256b4599b7d29ebb1a3acd14cc38b7";
-  public static final String GIT_DATE = "2026-01-10 16:52:43 MST";
+  public static final String GIT_DATE = "2026-01-13 18:19:47 MST";
   public static final String GIT_BRANCH = "master";
-  public static final String BUILD_DATE = "2026-01-12 16:53:00 MST";
+  public static final String BUILD_DATE = "2026-01-13 18:58:23 MST";
-  public static final long BUILD_UNIX_TIME = 1768261980081L;
+  public static final long BUILD_UNIX_TIME = 1768355903336L;
   public static final int DIRTY = 1;
 
   private BuildConstants(){}

src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java

@@ -236,6 +236,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
     }
 
     public void driveRelativeAngle(Translation2d leftStick, Rotation2d heading) {
+        
         leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
         leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis);
         var ctrl = new SwerveRequest.FieldCentricFacingAngle()
@@ -250,6 +251,32 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
         io.setControl(ctrl);
     }
 
+    public void driveFieldAngle(Translation2d leftStick, Rotation2d heading) {
+        if (leftStick.getNorm() < 0.05) // 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.
+
+
+        leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
+        leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis);
+
+
+        var ctrl = new SwerveRequest.FieldCentricFacingAngle()
+            .withVelocityX(leftStick.getX() * speedAdjust)
+            .withVelocityY(leftStick.getY() * speedAdjust)
+            .withTargetDirection(heading);
+        ctrl.HeadingController.setPID(
+            SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kP,
+            SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kI,
+            SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kD
+        );
+        io.setControl(ctrl);
+        // SmartDashboard.putBoolean("drift correction", true);
+        
+    }
+
     public void driveRelativeLockedAngle(Translation2d leftStick, Rotation2d heading) {
         leftStick = leftStick.rotateBy(heading);