diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 1063cf1..68cfa6d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -124,6 +124,9 @@ public static class Auto { /** The amount of time that we want to run the fire note command in auto. */ public static final double FIRE_NOTE_FOR_TIME = 4.0; + /** Time in seconds that we delay our taxi in the delayed taxi auto. */ + public static final double TAXI_DELAY_TIME = 10.0; + /** Constants for the DriveToAmpBlue auto */ public static class DriveToAmpBlue { /** The time for the robot to drive left towards the amp. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e303247..4232d5f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -31,6 +31,7 @@ import frc.robot.commands.RunIntake; import frc.robot.commands.RunIntakeReverse; import frc.robot.commands.StartCamera; +import frc.robot.commands.auto.DelayedTaxiAuto; import frc.robot.commands.auto.DriveToCenterAuto; import frc.robot.commands.auto.MoveAwayFromAmp; import frc.robot.commands.auto.OnePieceAuto; @@ -285,6 +286,9 @@ public class RobotContainer { /** Singleton instance of {@link TaxiAuto} for the whole robot. */ public static TaxiAuto taxiAuto = new TaxiAuto(); + /** Singleton instance of {@link DelayedTaxiAuto} for the whole robot. */ + public static DelayedTaxiAuto delayedTaxiAuto = new DelayedTaxiAuto(); + /** Singleton instance of {@link ClearPDPStickyFaults} for the whole robot. */ public static ClearPDPStickyFaults clearPDPStickyFaults = new ClearPDPStickyFaults(); @@ -359,6 +363,8 @@ private void configureAuto() { autoChooser.addOption("NO INTAKE drive to center", driveToCenterAuto); + autoChooser.addOption("Taxi with 10 second delay", delayedTaxiAuto); + Shuffleboard.getTab("Driver").add("Choose Auto Routine", autoChooser); } diff --git a/src/main/java/frc/robot/commands/auto/DelayedTaxiAuto.java b/src/main/java/frc/robot/commands/auto/DelayedTaxiAuto.java new file mode 100644 index 0000000..2676f41 --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/DelayedTaxiAuto.java @@ -0,0 +1,24 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +/* + * Asimov's Laws: + * The First Law: A robot may not injure a human being or, through inaction, allow a human being to come to harm. + * The Second Law: A robot must obey the orders given it by human beings except where such orders would conflict with the First Law. + * The Third Law: A robot must protect its own existence as long as such protection does not conflict with the First or Second Law. + */ + +package frc.robot.commands.auto; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.Constants; + +/** Taxi auto that waits a bit before it taxis. */ +public class DelayedTaxiAuto extends SequentialCommandGroup { + /** Creates a new DelayedTaxiAuto. */ + public DelayedTaxiAuto() { + addCommands(new WaitCommand(Constants.Auto.TAXI_DELAY_TIME), new TaxiAuto()); + } +}