Feature/Add auto program for score high after delay

src/main/java/competition/auto_programs/ScoreHighAfterDelayCommand.java

@@ -0,0 +1,70 @@
+package competition.auto_programs;
+
+import com.google.inject.Inject;
+import com.google.inject.Provider;
+
+import competition.commandgroups.PrepareToFireCommandThatEnds;
+import competition.commandgroups.ShutdownShootingCommandThatEnds;
+import competition.subsystems.conveyer.commands.ConveyWhileShooterAtSpeedCommand;
+import competition.subsystems.drive.commands.RotateToVisionTargetCommand;
+import competition.subsystems.drive.commands.SwerveToPointCommand;
+import competition.subsystems.shooterwheel.ShooterWheelSubsystem.Target;
+import competition.subsystems.shooterwheel.ShooterWheelSubsystem.TargetRPM;
+import competition.subsystems.vision.commands.ShooterRPMWithVisionCommand;
+import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+import xbot.common.command.DelayViaSupplierCommand;
+import xbot.common.math.XYPair;
+import xbot.common.properties.PropertyFactory;
+
+public class ScoreHighAfterDelayCommand extends SequentialCommandGroup {
+
+    @Inject
+    ScoreHighAfterDelayCommand(
+        PropertyFactory pf,
+        Provider<SwerveToPointCommand> swerveToPointProvider,
+        ShutdownShootingCommandThatEnds shutdownShooting,
+        PrepareToFireCommandThatEnds prepareforHigh,
+        ConveyWhileShooterAtSpeedCommand conveyWhenReady,
+        RotateToVisionTargetCommand visionRotate,
+        ShooterRPMWithVisionCommand visionRPM
+    ) {
+        pf.setTopLevelPrefix();
+
+        // Wait for a few seconds
+        var initialDelayProp = pf.createPersistentProperty("Delayed auto initial delay", 5.0);
+        var initialDelay = new DelayViaSupplierCommand(() -> initialDelayProp.get());
+        addCommands(initialDelay);
+
+        // Get out of the tarmac area
+        var escape = swerveToPointProvider.get();
+        escape.setRobotRelativeMotion();
+        escape.setMaxPower(0.5);
+        escape.setTargetPosition(new XYPair(0, -60), 90);
+        addCommands(new ParallelRaceGroup(
+            escape,
+            new WaitCommand(3)
+        ));
+
+        prepareforHigh.setTargetRPM(TargetRPM.DistanceShot);
+
+        this.addCommands(prepareforHigh);
+
+        SequentialCommandGroup visionAdjustAndShoot = new SequentialCommandGroup(
+            visionRotate,
+            conveyWhenReady
+        );
+
+        visionRPM.setTarget(Target.High);
+
+        ParallelRaceGroup shotWithVisionAdjustedRPM = new ParallelRaceGroup(
+            visionAdjustAndShoot,
+            visionRPM,
+            new WaitCommand(5)
+        );
+        this.addCommands(shotWithVisionAdjustedRPM);
+
+        this.addCommands(shutdownShooting);
+    }
+}

src/main/java/competition/operator_interface/OperatorCommandMap.java

@@ -11,6 +11,7 @@
 import competition.auto_programs.GoCollectComebackCommand;
 import competition.auto_programs.MoonshotCommand;
 import competition.auto_programs.SCSFromOneRobotAwayCommand;
+import competition.auto_programs.ScoreHighAfterDelayCommand;
 import competition.auto_programs.ShootCollectShootCommand;
 import competition.auto_programs.ShootFarThenEscapeCommand;
 import competition.auto_programs.ShootRecklesslyThenEscapeCommand;
@@ -351,6 +352,7 @@ public void setupAutonomousCommands(
             ShootFarThenEscapeCommand shootFarThenEscape,
             CollectThenHighScoreCommand highScore,
             MoonshotCommand moonshot,
+            ScoreHighAfterDelayCommand highAfterDelay,
             Provider<SetAutonomousCommand> setAutoCommandProvider,
             Provider<SetRobotHeadingCommand> setHeadingCommandProvider,
             Provider<SetPoseCommand> setPoseCommandProvider,
@@ -377,6 +379,8 @@ public void setupAutonomousCommands(
         setHighScore.setAutoCommand(highScore);
         SetAutonomousCommand setMoonshot = setAutoCommandProvider.get();
         setMoonshot.setAutoCommand(moonshot);
+        SetAutonomousCommand setHighAfterDelay = setAutoCommandProvider.get();
+        setHighAfterDelay.setAutoCommand(highAfterDelay);
 
         setDoNothing.includeOnSmartDashboard("AutoPrograms/DoNothing");
         setDriveFiveFeet.includeOnSmartDashboard("AutoPrograms/DriveFiveFeet");
@@ -389,12 +393,13 @@ public void setupAutonomousCommands(
         setShootFarThenEscapeCommand.includeOnSmartDashboard("AutoPrograms/ShootFarThenEscape");
         setHighScore.includeOnSmartDashboard("AutoPrograms/HighScore");
         setMoonshot.includeOnSmartDashboard("AutoPrograms/Moonshot");
+        setHighAfterDelay.includeOnSmartDashboard("AutoPrograms/HighAfterDelay");
 
         operatorInterface.autoGamepad.getPovIfAvailable(0).whenPressed(setDoNothing);
         operatorInterface.autoGamepad.getPovIfAvailable(90).whenPressed(setDriveFiveFeet);
         operatorInterface.autoGamepad.getPovIfAvailable(180).whenPressed(setShootThenEscape);
         operatorInterface.autoGamepad.getPovIfAvailable(270).whenPressed(setHighScore);
-        operatorInterface.autoGamepad.getifAvailable(XboxButton.LeftStick).whenPressed(moonshot);
+        operatorInterface.autoGamepad.getifAvailable(XboxButton.LeftStick).whenPressed(setHighAfterDelay);
         operatorInterface.autoGamepad.getifAvailable(XboxButton.RightStick).whenPressed(setShootCollectShoot);
         operatorInterface.autoGamepad.getifAvailable(XboxButton.Back).whenPressed(setScsFromOneRobotAwayCommand);
         operatorInterface.autoGamepad.getifAvailable(XboxButton.Start).whenPressed(setShootFarThenEscapeCommand);