From 4ef5d0e245246b3ba0254e4678aa6f845e4cdda5 Mon Sep 17 00:00:00 2001
From: JohnGilb <johngilb@gmail.com>
Date: Mon, 19 Feb 2024 22:06:58 -0800
Subject: [PATCH] Fix some bugs related to red alliance start

---
 .../competition/auto_programs/FromMidShootCollectShoot.java  | 2 +-
 src/main/java/competition/subsystems/pose/PoseSubsystem.java | 5 +++++
 2 files changed, 6 insertions(+), 1 deletion(-)

diff --git a/src/main/java/competition/auto_programs/FromMidShootCollectShoot.java b/src/main/java/competition/auto_programs/FromMidShootCollectShoot.java
index 13a93ae0..0200dba2 100644
--- 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
index eb7f4313..977976fb 100644
--- 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);