Flip to red properly on autos

frc868 · Mar 7, 2024 · 121fcf4 · 121fcf4
1 parent 33ef0cf
commit 121fcf4
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 2 deletions.
diff --git a/src/main/java/com/techhounds/houndutil/houndauto/AutoManager.java b/src/main/java/com/techhounds/houndutil/houndauto/AutoManager.java
@@ -17,6 +17,7 @@
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -131,6 +132,9 @@ public void periodicUpdate() {
     public void displayPaths(List<PathPlannerPath> paths) {
         ArrayList<Pose2d> poses = new ArrayList<Pose2d>();
         for (PathPlannerPath path : paths) {
+            if (DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red) {
+                path = path.flipPath();
+            }
             PathPlannerTrajectory trajectory = path.getTrajectory(new ChassisSpeeds(),
                     path.getStartingDifferentialPose().getRotation());
 
@@ -167,9 +171,9 @@ public void runSelectedRoutine() {
             timer.start();
             currentCommand = baseCommand
                     .beforeStarting(() -> resetOdometryConsumer.accept(getSelectedRoutine().getInitialPose()))
+                    .andThen(Commands.runOnce(timer::stop))
                     .withName("auto");
             currentCommand.schedule();
-            new Trigger(currentCommand::isFinished).onTrue(Commands.runOnce(timer::stop).ignoringDisable(true));
         }
     }
 

diff --git a/src/main/java/com/techhounds/houndutil/houndauto/AutoRoutine.java b/src/main/java/com/techhounds/houndutil/houndauto/AutoRoutine.java
@@ -4,6 +4,8 @@
 import com.pathplanner.lib.path.PathPlannerPath;
 
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.Command;
 
 public class AutoRoutine {
@@ -45,7 +47,10 @@ public List<PathPlannerPath> getPathPlannerPaths() {
 
     public Pose2d getInitialPose() {
         if (pathPlannerPaths != null) {
-            return initialPose;
+            if (DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red)
+                return Reflector.reflectPose2d(initialPose, 16.54);
+            else
+                return initialPose;
         } else {
             return new Pose2d();
         }