x /= i^2

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

@@ -65,6 +65,7 @@ import frc4388.robot.subsystems.SwerveDrive;
 import frc4388.utility.DeferredBlock;
 import frc4388.utility.ReefPositionHelper;
 import frc4388.utility.Subsystem;
+import frc4388.utility.TimesNegativeOne;
 import frc4388.utility.ReefPositionHelper.Side;
 import frc4388.utility.configurable.ConfigurableString;
 
@@ -292,7 +293,10 @@ public class RobotContainer {
 
         configureButtonBindings();        
         configureVirtualButtonBindings();
-        new DeferredBlock(() -> m_robotSwerveDrive.resetGyro());
+        new DeferredBlock(() -> { // Called on robot enable
+            TimesNegativeOne.update();
+            m_robotSwerveDrive.resetGyro();
+        });
         DriverStation.silenceJoystickConnectionWarning(true);
         // CameraServer.startAutomaticCapture();
 
@@ -462,7 +466,6 @@ public class RobotContainer {
         new Trigger(() -> getDeadbandedOperatorController().getPOV() == 180)
             .onTrue(new InstantCommand(() -> AutoConstants.ELEVATOR_OFFSET_TRIM.stepDown()));
 
-
         new Trigger(() -> getDeadbandedOperatorController().getPOV() == 90)
             .onTrue(new InstantCommand(() -> AutoConstants.ARM_OFFSET_TRIM.stepUp()));
 

src/main/java/frc4388/robot/commands/GotoLastApril.java

@@ -15,6 +15,7 @@ import frc4388.robot.subsystems.SwerveDrive;
 import frc4388.robot.subsystems.Vision;
 import frc4388.utility.Gains;
 import frc4388.utility.ReefPositionHelper;
+import frc4388.utility.TimesNegativeOne;
 import frc4388.utility.ReefPositionHelper.Side;
 import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame;
 import frc4388.utility.UtilityStructs.AutoRecordingFrame;
@@ -75,18 +76,9 @@ public class GotoLastApril extends Command {
 
     @Override
     public void execute() {
-        
+        xerr = TimesNegativeOne.invert(targetpos.getX() - vision.getPose2d().getX(), TimesNegativeOne.XAxis);
-        if (alliance == Alliance.Red) {
+        yerr = TimesNegativeOne.invert(targetpos.getX() - vision.getPose2d().getX(), TimesNegativeOne.YAxis);
-            xerr = -(targetpos.getX() - vision.getPose2d().getX());
-            yerr = (targetpos.getY() - vision.getPose2d().getY());
-        }
-        else{
-            
-            xerr = targetpos.getX() - vision.getPose2d().getX();
-            yerr = targetpos.getY() - vision.getPose2d().getY();
-        }
 
-            
         roterr = targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees();
 
         // SmartDashboard.putNumber("PID X Error", xerr);

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

@@ -35,9 +35,8 @@ import frc4388.robot.commands.GotoLastApril;
 import frc4388.robot.commands.LidarAlign;
 import frc4388.utility.Status;
 import frc4388.utility.Subsystem;
+import frc4388.utility.TimesNegativeOne;
 import frc4388.utility.Status.ReportLevel;
-import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.DriverStation.Alliance;
 
 import com.pathplanner.lib.controllers.PPHolonomicDriveController;
 import com.pathplanner.lib.config.PIDConstants;
@@ -103,11 +102,11 @@ public class SwerveDrive extends Subsystem {
                     // 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();
+                    // var alliance = DriverStation.getAlliance();
-                    if (alliance.isPresent()) {
+                    // if (alliance.isPresent()) {
-                        return alliance.get() == DriverStation.Alliance.Red;
+                    //     return alliance.get() == DriverStation.Alliance.Red;
-                    }
+                    // }
-                    return false;
+                    return TimesNegativeOne.isRed;
                 },
                 this // Reference to this subsystem to set requirements
         );
@@ -141,18 +140,8 @@ public class SwerveDrive extends Subsystem {
             return; // don't bother doing swerve drive math and return early.
 
         leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET));
-
+        leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis);
-        Optional<Alliance> alliance = DriverStation.getAlliance();
+        leftStick = TimesNegativeOne.invert(rightStick, TimesNegativeOne.RotAxis);    
-        
-        if(!alliance.isEmpty()){
-            if (alliance.get() == Alliance.Red) 
-                leftStick = new Translation2d(-leftStick.getX(), leftStick.getY());
-            else 
-                leftStick = new Translation2d(leftStick.getX(), -leftStick.getY());
-            // if (alliance.get() != Alliance.Red) leftStick = new Translation2d(leftStick.getX(), -leftStick.getY());
-        }
-        if (SwerveDriveConstants.INVERT_ROTATION) rightStick.times(-1);
-
 
         
         stopped = false;

src/main/java/frc4388/utility/Alliance.java

@@ -0,0 +1,5 @@
+package frc4388.utility;
+
+public class Alliance {
+
+}

src/main/java/frc4388/utility/ReefPositionHelper.java

@@ -63,14 +63,11 @@ public class ReefPositionHelper {
      * Function to find closest tag location based on side
      */
     public static Pose2d getNearestTag(Pose2d position) {
-        Optional<Alliance> ally = DriverStation.getAlliance();
+
-        if (!ally.isPresent()) 
+        if(TimesNegativeOne.isRed)
-            return new Pose2d();
-        if (ally.get() == Alliance.Red) 
             return getNearestTag(RED_TAGS, position);
-        if (ally.get() == Alliance.Blue) 
+        else
-            return getNearestTag(BLUE_TAGS, position);   
+            return getNearestTag(BLUE_TAGS, position);
-        return new Pose2d();
     }
 
     public static Pose2d getNearestPosition(Pose2d position, Side side, double xtrim, double ydistance) {

src/main/java/frc4388/utility/TimesNegativeOne.java

@@ -0,0 +1,46 @@
+package frc4388.utility;
+
+import java.util.Optional;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import frc4388.robot.Constants.SwerveDriveConstants;
+
+// Class that holds weather the drivers sticks should be inverted
+public class TimesNegativeOne {
+
+    public static boolean XAxis = SwerveDriveConstants.INVERT_X;
+    public static boolean YAxis = SwerveDriveConstants.INVERT_Y;
+    public static boolean RotAxis = SwerveDriveConstants.INVERT_ROTATION;
+    public static boolean isRed = false;
+
+    private static boolean isRed() {
+        Optional<Alliance> alliance = DriverStation.getAlliance();
+        if(alliance.isEmpty()) return false;
+        return (alliance.get() == Alliance.Red);
+    }
+
+    public static void update(){
+        isRed = isRed();
+        XAxis = SwerveDriveConstants.INVERT_X ^ isRed;
+        YAxis = SwerveDriveConstants.INVERT_Y ^ isRed;
+        RotAxis = SwerveDriveConstants.INVERT_ROTATION;
+    }
+
+    public static double invert(double num, boolean invert){
+        return invert ? -num : num;
+    }
+
+    public static Translation2d invert(Translation2d stick, boolean invertXY){
+        if(invertXY) return stick.times(-1);
+        else return stick;
+    }
+
+    public static Translation2d invert(Translation2d stick, boolean invertX, boolean invertY){
+        return new Translation2d(
+            invert(stick.getX(), invertX), 
+            invert(stick.getY(), invertY)
+        );
+    }
+}