Merge pull request #124 from LakotaRobotics1038/delay-auto-2

LakotaRobotics1038 · Mar 16, 2024 · f242044 · f242044
2 parents 72cb57b + 25ebc6d
commit f242044
Show file tree

Hide file tree

Showing 4 changed files with 36 additions and 1 deletion.
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -10,7 +10,7 @@
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
-
+import edu.wpi.first.wpilibj2.command.WaitCommand;
 import frc.robot.autons.Auton;
 import frc.robot.autons.AutonSelector;
 import frc.robot.commands.LiftUpCommand;

diff --git a/src/main/java/frc/robot/autons/Auton.java b/src/main/java/frc/robot/autons/Auton.java
@@ -16,6 +16,8 @@
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+import frc.robot.autons.AutonSelector.AutonChoices;
 import frc.robot.constants.AutoConstants;
 import frc.robot.constants.DriveConstants;
 import frc.robot.constants.FieldConstants;
@@ -30,6 +32,7 @@ public Auton(Optional<Alliance> alliance) {
         if (alliance.isPresent()) {
             this.alliance = alliance.get();
         }
+        this.addCommands(new WaitCommand(AutonSelector.getInstance().chooseDelay()));
     }
 
     protected void setInitialPose(PathPlannerTrajectory initialTrajectory) {

diff --git a/src/main/java/frc/robot/autons/AutonSelector.java b/src/main/java/frc/robot/autons/AutonSelector.java
@@ -21,6 +21,7 @@ public enum AutonChoices {
 
     // Choosers
     SendableChooser<AutonChoices> autoChooser;
+    SendableChooser<Double> delayChooser;
 
     // Singleton Setup
     private static AutonSelector instance;
@@ -43,6 +44,24 @@ private AutonSelector() {
         this.autoChooser.addOption("Taxi Position 1", AutonChoices.TaxiPos1);
         this.autoChooser.addOption("Taxi Position 2", AutonChoices.TaxiPos2);
         this.autoChooser.addOption("Taxi Position 3", AutonChoices.TaxiPos3);
+
+        this.delayChooser = Dashboard.getInstance().getDelayChooser();
+
+        this.delayChooser.setDefaultOption("0 Seconds", 0.0);
+        this.delayChooser.addOption("1 Second", 1.0);
+        this.delayChooser.addOption("2 Seconds", 2.0);
+        this.delayChooser.addOption("3 Seconds", 3.0);
+        this.delayChooser.addOption("4 Seconds", 4.0);
+        this.delayChooser.addOption("5 Seconds", 5.0);
+        this.delayChooser.addOption("6 Seconds", 6.0);
+        this.delayChooser.addOption("7 Seconds", 7.0);
+        this.delayChooser.addOption("8 Seconds", 8.0);
+        this.delayChooser.addOption("9 Seconds", 9.0);
+        this.delayChooser.addOption("10 Seconds", 10.0);
+        this.delayChooser.addOption("11 Seconds", 11.0);
+        this.delayChooser.addOption("12 Seconds", 12.0);
+        this.delayChooser.addOption("13 Seconds", 13.0);
+        this.delayChooser.addOption("14 Seconds", 14.0);
     }
 
     public Auton chooseAuton() {
@@ -64,4 +83,8 @@ public Auton chooseAuton() {
                 return null;
         }
     }
+
+    public double chooseDelay() {
+        return this.delayChooser.getSelected();
+    }
 }
diff --git a/src/main/java/frc/robot/subsystems/Dashboard.java b/src/main/java/frc/robot/subsystems/Dashboard.java
@@ -27,6 +27,7 @@ public class Dashboard extends SubsystemBase {
 
     // Choosers
     private SendableChooser<AutonChoices> autoChooser = new SendableChooser<>();
+    private SendableChooser<Double> delayChooser = new SendableChooser<>();
 
     // Tabs
     private final ShuffleboardTab driversTab = Shuffleboard.getTab("Drivers");
@@ -79,6 +80,10 @@ private Dashboard() {
                 .withPosition(0, 0)
                 .withSize(2, 1);
 
+        driversTab.add("Delay Choices", delayChooser)
+                .withPosition(0, 1)
+                .withSize(2, 1);
+
         driversTab.addNumber("Gyro", () -> {
             double angle = driveTrain.getHeading();
             angle %= 360;
@@ -178,4 +183,8 @@ public void clearTrajectory() {
     public SendableChooser<AutonChoices> getAutoChooser() {
         return autoChooser;
     }
+
+    public SendableChooser<Double> getDelayChooser() {
+        return delayChooser;
+    }
 }