From bca78e4ba6e985562cc4e492f398b9583fd0b5ec Mon Sep 17 00:00:00 2001 From: Alex Schokking Date: Thu, 3 Apr 2025 21:50:56 -0700 Subject: [PATCH 1/2] add 1s delay to start of key autos --- .../auto_programs/FromCageScoreOneCoralAutoFactory.java | 5 +++++ .../competition/auto_programs/vision/LeftFourCoralAuto.java | 3 +++ .../competition/auto_programs/vision/RightFourCoralAuto.java | 4 +++- 3 files changed, 11 insertions(+), 1 deletion(-) diff --git a/src/main/java/competition/auto_programs/FromCageScoreOneCoralAutoFactory.java b/src/main/java/competition/auto_programs/FromCageScoreOneCoralAutoFactory.java index 02d7c6cd..b603aa5e 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 { @@ -38,6 +41,8 @@ public BaseAutonomousSequentialCommandGroup create(Pose2d startingLocation, var auto = new BaseAutonomousSequentialCommandGroup(autoSelector); auto.setName("FromCageScoreOneCoralAuto"); + auto.addCommands(new WaitCommand(Seconds.of(1))); + var initializeStateCommand = pose.createSetPositionCommand( () -> PoseSubsystem.convertBlueToRedIfNeeded(startingLocation) ) 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, From 28ce37490b5393c6f02409f866013f44047ce4d6 Mon Sep 17 00:00:00 2001 From: Alex Schokking Date: Fri, 4 Apr 2025 09:20:00 -0700 Subject: [PATCH 2/2] move wait to after pose set --- .../auto_programs/FromCageScoreOneCoralAutoFactory.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/competition/auto_programs/FromCageScoreOneCoralAutoFactory.java b/src/main/java/competition/auto_programs/FromCageScoreOneCoralAutoFactory.java index b603aa5e..50201be0 100644 --- a/src/main/java/competition/auto_programs/FromCageScoreOneCoralAutoFactory.java +++ b/src/main/java/competition/auto_programs/FromCageScoreOneCoralAutoFactory.java @@ -41,14 +41,14 @@ public BaseAutonomousSequentialCommandGroup create(Pose2d startingLocation, var auto = new BaseAutonomousSequentialCommandGroup(autoSelector); auto.setName("FromCageScoreOneCoralAuto"); - auto.addCommands(new WaitCommand(Seconds.of(1))); - var initializeStateCommand = pose.createSetPositionCommand( () -> PoseSubsystem.convertBlueToRedIfNeeded(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));