From 5f15633044b076bbb539dde4867acf4035bdb2dc Mon Sep 17 00:00:00 2001
From: Bradley Bickford <bradleybickford@gmail.com>
Date: Sun, 10 Mar 2024 12:27:22 -0400
Subject: [PATCH] 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   | 40 ++++++++++++-------
 .../java/frc/robot/subsystems/Drivetrain.java |  2 +-
 2 files changed, 27 insertions(+), 15 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index bb3c0a7..1cb3e44 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/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);
-
-        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;
-        }
+        isEnabledTrigger = new Trigger(DriverStation::isEnabled);
 
         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.getLeftX(); }, 
+                () -> { return driver.getLeftY() * (invertXYDrive ? -1 : 1); }, 
+                () -> { return driver.getLeftX() * (invertXYDrive ? -1 : 1); }, 
                 () -> { return -driver.getRightX(); }, 
                 OIConstants.kTeleopDriveDeadband
             )
diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index a35c824..0975eed 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/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]);