fix: don't reset field orientation on scoring if odometry isn't updated

flamingchickens1540 · Oct 17, 2023 · 586927b · 586927b
1 parent a5ab15c
commit 586927b
Showing 1 changed file with 6 additions and 1 deletion.
diff --git a/src/main/java/org/team1540/robot2023/commands/drivetrain/Drivetrain.java b/src/main/java/org/team1540/robot2023/commands/drivetrain/Drivetrain.java
@@ -122,7 +122,12 @@ public boolean updateWithScoringApriltags() {
 
     public Command updateOdometryAnd(Command cmd) {
         return new ParallelCommandGroup(
-                new InstantCommand(this::updateWithScoringApriltags).andThen(new InstantCommand(this::zeroFieldOrientation)),
+                new InstantCommand(() -> {
+                    boolean isGood = this.updateWithScoringApriltags();
+                    if (isGood) {
+                        this.zeroFieldOrientation();
+                    }
+                }),
                 cmd
         );
     }