From b1b6e6d99a58dbaf58958a29641aa2228ff00056 Mon Sep 17 00:00:00 2001 From: Stephen Just Date: Mon, 18 Apr 2022 10:13:37 -0700 Subject: [PATCH 1/2] Feature/Add auto program for score high after delay This replaces LeftStick (moonshot) on the auto selector. --- .../ScoreHighAfterDelayCommand.java | 70 +++++++++++++++++++ .../OperatorCommandMap.java | 7 +- 2 files changed, 76 insertions(+), 1 deletion(-) create mode 100644 src/main/java/competition/auto_programs/ScoreHighAfterDelayCommand.java diff --git a/src/main/java/competition/auto_programs/ScoreHighAfterDelayCommand.java b/src/main/java/competition/auto_programs/ScoreHighAfterDelayCommand.java new file mode 100644 index 00000000..e4035766 --- /dev/null +++ b/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 swerveToPointProvider, + ShutdownShootingCommandThatEnds shutdownShooting, + PrepareToFireCommandThatEnds prepareforHigh, + ConveyWhileShooterAtSpeedCommand conveyWhenReady, + RotateToVisionTargetCommand visionRotate, + ShooterRPMWithVisionCommand visionRPM + ) { + pf.setPrefix(getName()); + + // Wait for a few seconds + var initialDelayProp = pf.createPersistentProperty("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); + } +} diff --git a/src/main/java/competition/operator_interface/OperatorCommandMap.java b/src/main/java/competition/operator_interface/OperatorCommandMap.java index 55b3a9c4..2f7d8680 100644 --- a/src/main/java/competition/operator_interface/OperatorCommandMap.java +++ b/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 setAutoCommandProvider, Provider setHeadingCommandProvider, Provider 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); From fbbcbcbb4b430ee89b5bab0fd7a91cd6322f3e3e Mon Sep 17 00:00:00 2001 From: Stephen Just Date: Thu, 21 Apr 2022 06:24:11 -0700 Subject: [PATCH 2/2] Move delayed auto prop to top-level for SD --- .../competition/auto_programs/ScoreHighAfterDelayCommand.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/competition/auto_programs/ScoreHighAfterDelayCommand.java b/src/main/java/competition/auto_programs/ScoreHighAfterDelayCommand.java index e4035766..9650dd78 100644 --- a/src/main/java/competition/auto_programs/ScoreHighAfterDelayCommand.java +++ b/src/main/java/competition/auto_programs/ScoreHighAfterDelayCommand.java @@ -30,10 +30,10 @@ public class ScoreHighAfterDelayCommand extends SequentialCommandGroup { RotateToVisionTargetCommand visionRotate, ShooterRPMWithVisionCommand visionRPM ) { - pf.setPrefix(getName()); + pf.setTopLevelPrefix(); // Wait for a few seconds - var initialDelayProp = pf.createPersistentProperty("Initial delay", 5.0); + var initialDelayProp = pf.createPersistentProperty("Delayed auto initial delay", 5.0); var initialDelay = new DelayViaSupplierCommand(() -> initialDelayProp.get()); addCommands(initialDelay);