diff --git a/src/main/java/competition/auto_programs/FromCageScoreOneCoralAutoFactory.java b/src/main/java/competition/auto_programs/FromCageScoreOneCoralAutoFactory.java index 02d7c6cd..50201be0 100644 --- a/src/main/java/competition/auto_programs/FromCageScoreOneCoralAutoFactory.java +++ b/src/main/java/competition/auto_programs/FromCageScoreOneCoralAutoFactory.java @@ -7,8 +7,11 @@ import competition.subsystems.pose.PoseSubsystem; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.WaitCommand; import xbot.common.subsystems.autonomous.AutonomousCommandSelector; +import static edu.wpi.first.units.Units.Seconds; + import javax.inject.Inject; public class FromCageScoreOneCoralAutoFactory { @@ -44,6 +47,8 @@ public BaseAutonomousSequentialCommandGroup create(Pose2d startingLocation, .alongWith(new InstantCommand(() -> simulator.resetPosition(PoseSubsystem.convertBlueToRedIfNeeded(startingLocation)))); auto.addCommands(initializeStateCommand); + auto.addCommands(new WaitCommand(Seconds.of(1))); + var driveAndScore = driveToFaceAndScoreCommandGroupFact.create(targetReefFace, targetBranch, targetLevel) .alongWith( auto.getDriveAndScoreStatusMessageCommand(targetReefFace, targetBranch, targetLevel)); diff --git a/src/main/java/competition/auto_programs/vision/LeftFourCoralAuto.java b/src/main/java/competition/auto_programs/vision/LeftFourCoralAuto.java index f65c9d3c..b58c53b6 100644 --- a/src/main/java/competition/auto_programs/vision/LeftFourCoralAuto.java +++ b/src/main/java/competition/auto_programs/vision/LeftFourCoralAuto.java @@ -14,6 +14,8 @@ import javax.inject.Inject; import javax.inject.Provider; +import static edu.wpi.first.units.Units.Seconds; + import edu.wpi.first.wpilibj2.command.WaitCommand; import xbot.common.subsystems.autonomous.AutonomousCommandSelector; @@ -26,6 +28,7 @@ public LeftFourCoralAuto( Provider pathDriveToLocationAndIntakeUntilCollectedProvider) { super(autoSelector); + this.addCommands(new WaitCommand(Seconds.of(1))); // Score 1 getDriveAndScoreStatusMessageCommand(Landmarks.ReefFace.FAR_LEFT, diff --git a/src/main/java/competition/auto_programs/vision/RightFourCoralAuto.java b/src/main/java/competition/auto_programs/vision/RightFourCoralAuto.java index 3cc99a21..720eefac 100644 --- a/src/main/java/competition/auto_programs/vision/RightFourCoralAuto.java +++ b/src/main/java/competition/auto_programs/vision/RightFourCoralAuto.java @@ -13,6 +13,8 @@ import javax.inject.Inject; import javax.inject.Provider; +import static edu.wpi.first.units.Units.Seconds; + import edu.wpi.first.wpilibj2.command.WaitCommand; import xbot.common.subsystems.autonomous.AutonomousCommandSelector; @@ -25,7 +27,7 @@ public RightFourCoralAuto( Provider pathDriveToLocationAndIntakeUntilCollectedProvider) { super(autoSelector); - + this.addCommands(new WaitCommand(Seconds.of(1))); // Score 1 getDriveAndScoreStatusMessageCommand(Landmarks.ReefFace.FAR_RIGHT,