diff --git a/src/main/java/competition/auto_programs/AutoCommandFactory.java b/src/main/java/competition/auto_programs/AutoCommandFactory.java index 529546ef..0f6fb758 100644 --- a/src/main/java/competition/auto_programs/AutoCommandFactory.java +++ b/src/main/java/competition/auto_programs/AutoCommandFactory.java @@ -130,11 +130,13 @@ public Command statusMessage(String message) { /** * Drives to the neutral zone ball pit, deploys the intake, drives across to collect balls. */ - public Command collectFromNeutralZone() { + public Command collectFromNeutralZone(Command collectorWaitCommand) { var group = new SequentialCommandGroup(); group.setName("CollectFromNeutralZone"); - group.addCommands(driveToNeutralZoneProvider.get()); + group.addCommands(new ParallelDeadlineGroup( + driveToNeutralZoneProvider.get(), + collectorWaitCommand.andThen(collectorIntakeProvider.get()))); group.addCommands(new ParallelDeadlineGroup( firstDriveForCollectionCommandProvider.get(), @@ -146,11 +148,13 @@ public Command collectFromNeutralZone() { /** * Drives to the neutral zone ball pit, deploys the intake, drives across to collect balls. */ - public Command collectFromNeutralZoneSecond() { + public Command collectFromNeutralZoneSecond(Command collectorWaitCommand) { var group = new SequentialCommandGroup(); group.setName("CollectFromNeutralZoneSecond"); - group.addCommands(driveToNeutralZoneSecondTimeProvider.get()); + group.addCommands(new ParallelDeadlineGroup( + driveToNeutralZoneSecondTimeProvider.get() + collectorWaitCommand.andThen(collectorIntakeProvider.get()))); group.addCommands(new ParallelDeadlineGroup( secondDriveForCollectionCommandProvider.get(), @@ -172,7 +176,6 @@ public Command driveToAllianceAndShoot(Command shootingDeadline) { prepareToShoot.setPresetLocation(TrajectoriesCalculation.PresetShootingDistance.TRENCH); group.addCommands(new ParallelDeadlineGroup( driveFromNeutralZoneToAllianceProvider.get(), - collectorStopProvider.get(), prepareToShoot)); var continuousPrepare = continuousPrepareToShootProvider.get(); diff --git a/src/main/java/competition/auto_programs/CollectAndShootTwiceCommand.java b/src/main/java/competition/auto_programs/CollectAndShootTwiceCommand.java index 3c9310a1..0adcf5c7 100644 --- a/src/main/java/competition/auto_programs/CollectAndShootTwiceCommand.java +++ b/src/main/java/competition/auto_programs/CollectAndShootTwiceCommand.java @@ -19,16 +19,17 @@ public CollectAndShootTwiceCommand( pf.setPrefix(this.getName()); var firstShotTimeout = pf.createPersistentProperty("First shot timeout seconds", 3.0); var secondShotTimeout = pf.createPersistentProperty("Second shot timeout seconds", 10.0); + var collectorWait = pf.createPersistentProperty("Collector run wait seconds", 1.0); addCommands( auto.extendIntake(), - auto.collectFromNeutralZone() + auto.collectFromNeutralZone(new WaitForDurationCommand(collectorWait::get)) .alongWith(auto.statusMessage("Driving to neutral zone and back")), auto.driveToAllianceAndShoot( auto.waitForShootingDone().raceWith(new WaitForDurationCommand(firstShotTimeout::get))) .alongWith(auto.statusMessage("Driving to alliance and shoot")), auto.stopShooting().alongWith(auto.extendIntake()), - auto.collectFromNeutralZoneSecond() + auto.collectFromNeutralZoneSecond(new WaitForDurationCommand(collectorWait::get)) .alongWith(auto.statusMessage("Driving to neutral zone and back the second time")), auto.driveToAllianceAndShoot(new WaitForDurationCommand(secondShotTimeout::get)) .alongWith(auto.statusMessage("Driving to alliance and shoot")));