added driver perspective for field relative chassis speeds

Team334 · Jan 18, 2025 · 275327a · 275327a
1 parent 2e00a2d
commit 275327a
Show file tree

Hide file tree

Showing 3 changed files with 16 additions and 6 deletions.
diff --git 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
@@ -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
@@ -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;
     }