diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 794d2e2f..3da62345 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -18,6 +18,7 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.Subsystems.SubsystemConstants; import frc.robot.subsystems.SuperStructure; import frc.robot.subsystems.auto.AutoBuilderConfig; @@ -144,8 +145,10 @@ public void disabledExit() { @Override public void autonomousInit() { Shuffleboard.startRecording(); - if (SubsystemConstants.DRIVEBASE_ENABLED && AutoLogic.getSelectedAuto() != null) { - AutoLogic.getSelectedAuto().schedule(); + if (SubsystemConstants.DRIVEBASE_ENABLED) { + Commands.waitSeconds(AutoLogic.autoDelayEntry.get().getDouble()) + .andThen(AutoLogic.auton) + .schedule(); } if (subsystems.climbPivotSubsystem != null) { subsystems.climbPivotSubsystem.moveCompleteFalse(); diff --git a/src/main/java/frc/robot/subsystems/auto/AutoLogic.java b/src/main/java/frc/robot/subsystems/auto/AutoLogic.java index f65e48a9..f948c5e0 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/AutoLogic.java @@ -174,8 +174,8 @@ public static enum StartPosition { new DynamicSendableChooser(); private static SendableChooser gameObjects = new SendableChooser(); private static SendableChooser isVision = new SendableChooser(); - - private static GenericEntry autoDelayEntry; + public static Command auton; + public static GenericEntry autoDelayEntry; /** Registers commands in PathPlanner */ public static void registerCommands() { @@ -216,6 +216,7 @@ public static void initShuffleBoard() { for (StartPosition startPosition : StartPosition.values()) { startPositionChooser.addOption(startPosition.title, startPosition); } + isVision.setDefaultOption("Presets", false); isVision.addOption("Vision", true); gameObjects.setDefaultOption("0", 0); @@ -234,10 +235,16 @@ public static void initShuffleBoard() { tab.addBoolean("Low on time?", () -> AutoAlign.oneSecondLeft()); tab.addDouble("MATCH TIME(TIMER FOR AUTO)", () -> DriverStation.getMatchTime()); autoDelayEntry = tab.add("Auto Delay", 0).withPosition(4, 3).withSize(1, 1).getEntry(); - isVision.onChange((dummyVar) -> AutoLogic.filterAutos(gameObjects.getSelected())); startPositionChooser.onChange((dummyVar) -> AutoLogic.filterAutos(gameObjects.getSelected())); gameObjects.onChange((dummyVar) -> AutoLogic.filterAutos(gameObjects.getSelected())); + availableAutos.onChange( + (dummyVar) -> + auton = + (availableAutos.getSelected() != defaultPath.getAutoName()) + ? AutoBuilder.buildAuto( + namesToAuto.get(availableAutos.getSelected()).getAutoName()) + : defaultPath.getAutoCommand()); filterAutos(gameObjects.getSelected()); }