From 275327ad24b34a5471a6d8826b343087d221829f Mon Sep 17 00:00:00 2001
From: PGgit08 <peter.mty8@gmail.com>
Date: Fri, 17 Jan 2025 22:18:24 -0500
Subject: [PATCH] added driver perspective for field relative chassis speeds

---
 simgui-ds.json                                    |  5 -----
 src/main/java/frc/robot/subsystems/Swerve.java    | 15 +++++++++++++++
 .../java/frc/robot/utils/VisionPoseEstimator.java |  2 +-
 3 files changed, 16 insertions(+), 6 deletions(-)

diff --git a/simgui-ds.json b/simgui-ds.json
index b0c3fd2..ec0f8fc 100644
--- a/simgui-ds.json
+++ b/simgui-ds.json
@@ -1,9 +1,4 @@
 {
-  "FMS": {
-    "window": {
-      "visible": false
-    }
-  },
   "System Joysticks": {
     "window": {
       "visible": false
diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java
index 6b30b0d..836fc64 100644
--- a/src/main/java/frc/robot/subsystems/Swerve.java
+++ b/src/main/java/frc/robot/subsystems/Swerve.java
@@ -28,6 +28,8 @@
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.Notifier;
 import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -160,6 +162,8 @@ public class Swerve extends TunerSwerveDrivetrain implements Subsystem, SelfChec
 
   private final VisionSystemSim _visionSystemSim;
 
+  private boolean _hasAppliedDriverPerspective;
+
   /**
    * Creates a new CommandSwerveDrivetrain.
    *
@@ -476,6 +480,17 @@ private void updateVisionPoseEstimates() {
   public void periodic() {
     updateVisionPoseEstimates();
 
+    if (!_hasAppliedDriverPerspective || DriverStation.isDisabled()) {
+      DriverStation.getAlliance()
+          .ifPresent(
+              allianceColor -> {
+                setOperatorPerspectiveForward(
+                    allianceColor == Alliance.Red ? Rotation2d.k180deg : Rotation2d.kZero);
+
+                _hasAppliedDriverPerspective = true;
+              });
+    }
+
     DogLog.log(
         "Swerve/Accepted Estimates",
         _acceptedEstimates.stream().map(VisionPoseEstimate::pose).toArray(Pose3d[]::new));
diff --git a/src/main/java/frc/robot/utils/VisionPoseEstimator.java b/src/main/java/frc/robot/utils/VisionPoseEstimator.java
index fcf9f6f..6c953d3 100644
--- a/src/main/java/frc/robot/utils/VisionPoseEstimator.java
+++ b/src/main/java/frc/robot/utils/VisionPoseEstimator.java
@@ -210,7 +210,7 @@ public VisionPoseEstimator(
     if (Robot.isSimulation()) {
       var cameraProps = new SimCameraProperties();
 
-      _cameraSim = new PhotonCameraSim(_camera, cameraProps);
+      _cameraSim = new PhotonCameraSim(_camera, cameraProps, fieldLayout);
     } else {
       _cameraSim = null;
     }