Auto behavior is better with hokey fix, coming out of auto into teleop has ok field oriented, resetting heading is weird.

src/main/java/frc/robot/RobotContainer.java

@@ -17,6 +17,7 @@ import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
@@ -53,6 +54,7 @@ public class RobotContainer {
     private CommandXboxController operator;
 
     private Trigger isTeleopTrigger;
+    private Trigger isEnabledTrigger;
 
     private SendableChooser<Command> autoChooser;
 
@@ -65,6 +67,7 @@ public class RobotContainer {
     private int speakerTag;
 
     private boolean setAmpAngle;
+    private boolean invertXYDrive;
 
     public RobotContainer() {
         /*try {
@@ -107,18 +110,7 @@ public class RobotContainer {
         operator = new CommandXboxController(OIConstants.kSecondaryXboxUSB);
 
         isTeleopTrigger = new Trigger(DriverStation::isTeleopEnabled);
-
+        isEnabledTrigger = new Trigger(DriverStation::isEnabled);
-        Optional<DriverStation.Alliance> alliance = DriverStation.getAlliance();
-
-        if (alliance.isEmpty() || alliance.get() == DriverStation.Alliance.Red) {
-            ampTagID = 5;
-            sourceTagID = 9;
-            speakerTag = 4; 
-        } else {
-            ampTagID = 6;
-            sourceTagID = 1;
-            speakerTag = 8;
-        }
 
         setAmpAngle = false;
 
@@ -135,10 +127,30 @@ public class RobotContainer {
     private void configureBindings() {
         isTeleopTrigger.onTrue(new InstantCommand(() -> Shuffleboard.selectTab(kTeleopTabName)));
 
+        isEnabledTrigger.onTrue(
+            new InstantCommand(
+                () -> {
+                    Optional<DriverStation.Alliance> alliance = DriverStation.getAlliance();
+
+                    if (alliance.isEmpty() || alliance.get() == DriverStation.Alliance.Red) {
+                        ampTagID = 5;
+                        sourceTagID = 9;
+                        speakerTag = 4;
+                        invertXYDrive = true;
+                    } else {
+                        ampTagID = 6;
+                        sourceTagID = 1;
+                        speakerTag = 8;
+                        invertXYDrive = false;
+                    }
+                }
+            )
+        );
+
         drivetrain.setDefaultCommand(
             drivetrain.teleopCommand(
-                () -> { return -driver.getLeftY(); }, 
+                () -> { return driver.getLeftY() * (invertXYDrive ? -1 : 1); }, 
-                () -> { return -driver.getLeftX(); }, 
+                () -> { return driver.getLeftX() * (invertXYDrive ? -1 : 1); }, 
                 () -> { return -driver.getRightX(); }, 
                 OIConstants.kTeleopDriveDeadband
             )

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

@@ -299,7 +299,7 @@ public class Drivetrain extends SubsystemBase {
     }
 
     public void driveWithChassisSpeeds(ChassisSpeeds speeds) {
-        //speeds.omegaRadiansPerSecond = -speeds.omegaRadiansPerSecond;
+        speeds.omegaRadiansPerSecond = -speeds.omegaRadiansPerSecond;
         SwerveModuleState[] states = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(speeds);
 
         m_frontLeft.setDesiredState(states[0]);