feat: untested but theoretically sound auto speedups

flamingchickens1540 · Apr 1, 2024 · 0f32b45 · 0f32b45
1 parent c793979
commit 0f32b45
Showing 1 changed file with 8 additions and 12 deletions.
diff --git a/src/main/java/org/team1540/robot2024/util/auto/AutoCommand.java b/src/main/java/org/team1540/robot2024/util/auto/AutoCommand.java
@@ -96,20 +96,16 @@ protected Command createSegmentSequence(Drivetrain drivetrain, Shooter shooter,
                         new InstantCommand(shooter::zeroPivotToCancoder)
                 ).onlyIf(()->shouldZeroCancoder),
                 drivetrain.commandCopyVisionPose().onlyIf(()->shouldResetOdometry),
-                Commands.parallel(
-                        Commands.sequence(
-//                                Commands.waitUntil(()->drivetrain.getPose().getRotation().minus(drivetrain.getTargetPose().getRotation()).getDegrees()<10).onlyIf(()->shouldRealignYaw),
-                                new ParallelDeadlineGroup(
-                                        Commands.sequence(
-                                                Commands.waitSeconds(extraPreShotWait),
-                                                Commands.waitUntil(()->!indexer.isNoteStaged()),
-                                                Commands.waitSeconds(0.2)
-                                        ).withTimeout(1+extraPreShotWait),
-                                        Commands.waitSeconds(extraPreShotWait).andThen(IntakeAndFeed.withDefaults(indexer))
-                                )
+                Commands.deadline(
+                        Commands.deadline(
+                                Commands.sequence(
+                                        Commands.waitSeconds(extraPreShotWait),
+                                        Commands.waitUntil(()->!indexer.isNoteStaged()),
+                                        Commands.waitSeconds(0.1)
+                                ).withTimeout(1+extraPreShotWait),
+                                Commands.waitSeconds(extraPreShotWait).andThen(IntakeAndFeed.withDefaults(indexer))
                         ),
                         new DriveWithTargetingCommand(drivetrain, null).withTimeout(0.4).onlyIf(()->shouldRealignYaw)
-
                 )
         );
     }