Fix some bugs related to red alliance start

Team488 · Feb 20, 2024 · 4ef5d0e · 4ef5d0e
1 parent 2fc74f8
commit 4ef5d0e
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 1 deletion.
diff --git a/src/main/java/competition/auto_programs/FromMidShootCollectShoot.java b/src/main/java/competition/auto_programs/FromMidShootCollectShoot.java
@@ -38,7 +38,7 @@ public FromMidShootCollectShoot(
         this.autoSelector = autoSelector;
         // Force our location
         var startInFrontOfSpeaker = pose.createSetPositionCommand(
-                PoseSubsystem.SubwooferCentralScoringLocation);
+                () -> PoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.SubwooferCentralScoringLocation));
         this.addCommands(startInFrontOfSpeaker);
 
         // Shoot the pre-loaded note from the subwoofer

diff --git a/src/main/java/competition/subsystems/pose/PoseSubsystem.java b/src/main/java/competition/subsystems/pose/PoseSubsystem.java
@@ -30,6 +30,7 @@
 
 
 import java.util.Optional;
+import java.util.function.Supplier;
 
 
 @Singleton
@@ -220,6 +221,10 @@ public Command createSetPositionCommand(Pose2d pose) {
         return Commands.runOnce(() -> setCurrentPosition(pose));
     }
 
+    public Command createSetPositionCommand(Supplier<Pose2d> poseSupplier) {
+        return Commands.runOnce(() -> setCurrentPosition(poseSupplier.get()));
+    }
+
     private void improveFusedOdometryUsingPhotonLib(Pose2d recentPosition) {
         var photonEstimatedPoses = vision.getPhotonVisionEstimatedPoses(recentPosition);