diff --git a/src/main/java/frc/robot/autons/Auton.java b/src/main/java/frc/robot/autons/Auton.java index 6731b08c..243df2b2 100644 --- a/src/main/java/frc/robot/autons/Auton.java +++ b/src/main/java/frc/robot/autons/Auton.java @@ -16,9 +16,11 @@ 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.constants.AutoConstants; import frc.robot.constants.DriveConstants; import frc.robot.constants.FieldConstants; +import frc.robot.subsystems.Dashboard; import frc.robot.subsystems.DriveTrain; public abstract class Auton extends SequentialCommandGroup { @@ -30,6 +32,7 @@ public Auton(Optional alliance) { if (alliance.isPresent()) { this.alliance = alliance.get(); } + this.addCommands(new WaitCommand(Dashboard.getInstance().getAutoDelay())); } 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 index ba2c9cd4..7eb10938 100644 --- a/src/main/java/frc/robot/autons/AutonSelector.java +++ b/src/main/java/frc/robot/autons/AutonSelector.java @@ -12,7 +12,6 @@ public class AutonSelector { public enum AutonChoices { NoAuto, AmpAuto, - AmpAutoWait, AmpAutoAcquire, TaxiPos1, TaxiPos2, @@ -38,7 +37,6 @@ private AutonSelector() { this.autoChooser.setDefaultOption("No Auto", AutonChoices.NoAuto); this.autoChooser.addOption("Score 2 In Amp Position 1", AutonChoices.AmpAuto); - this.autoChooser.addOption("Wait Score in amp position 1", AutonChoices.AmpAutoWait); this.autoChooser.addOption("Score 2 in amp and acquire Pos 1", AutonChoices.AmpAutoAcquire); this.autoChooser.addOption("Taxi Position 1", AutonChoices.TaxiPos1); this.autoChooser.addOption("Taxi Position 2", AutonChoices.TaxiPos2); @@ -50,8 +48,6 @@ public Auton chooseAuton() { switch (this.autoChooser.getSelected()) { case AmpAuto: return new ScoreInAmp(alliance); - case AmpAutoWait: - return new ScoreInAmpWait(alliance); case AmpAutoAcquire: return new ScoreInAmpAcquire(alliance); case TaxiPos1: diff --git a/src/main/java/frc/robot/autons/ScoreInAmpWait.java b/src/main/java/frc/robot/autons/ScoreInAmpWait.java deleted file mode 100644 index 99663bd0..00000000 --- a/src/main/java/frc/robot/autons/ScoreInAmpWait.java +++ /dev/null @@ -1,38 +0,0 @@ -package frc.robot.autons; - -import java.util.Optional; - -import com.pathplanner.lib.commands.FollowPathCommand; - -import frc.robot.subsystems.Acquisition; -import frc.robot.subsystems.Dashboard; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import frc.robot.commands.AcquisitionRunCommand; -import frc.robot.commands.ScoreNoteCommand; -import frc.robot.commands.ScoringElevatorPositionCommand; -import frc.robot.commands.ScoringElevatorPositionCommand.FinishActions; -import frc.robot.constants.ScoringConstants.ScoringLocation; -import frc.robot.subsystems.ScoringElevator.ElevatorSetpoints; - -public class ScoreInAmpWait extends Auton { - - private Acquisition acquisition = Acquisition.getInstance(); - - public ScoreInAmpWait(Optional alliance) { - super(alliance); - - Dashboard.getInstance().setTrajectory(Trajectories.getFromPosition1ToAmpTrajectory()); - this.setInitialPose(Trajectories.getFromPosition1ToAmpTrajectory()); - // try .concatenate a return and final trajectory later - - super.addCommands( - new WaitCommand(5), - followPathCommand(Paths.pathFromPosition1ToAmp), - new ScoringElevatorPositionCommand(ElevatorSetpoints.Amp, FinishActions.NoDisable), - new ScoreNoteCommand(ScoringLocation.Amp, 1.5), - new ScoringElevatorPositionCommand(ElevatorSetpoints.Ground), - followPathCommand(Paths.pathFromAmpToMidline)); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Dashboard.java b/src/main/java/frc/robot/subsystems/Dashboard.java index 3cab7bcd..2aa80231 100644 --- a/src/main/java/frc/robot/subsystems/Dashboard.java +++ b/src/main/java/frc/robot/subsystems/Dashboard.java @@ -7,6 +7,7 @@ import edu.wpi.first.cscore.HttpCamera; import edu.wpi.first.cscore.VideoSource.ConnectionStrategy; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.trajectory.Trajectory; import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.networktables.NetworkTableInstance; @@ -51,6 +52,10 @@ public enum Cameras { .getEntry(); // Drivers Tab Inputs + private GenericEntry autonDelay = driversTab.add("Auton Delay", 0) + .withPosition(0, 1) + .withSize(2, 1) + .getEntry(); private GenericEntry feedAmpSpeed = driversTab.add("Feed Amp Speed", ScoringConstants.feedAmpSpeed) .withPosition(0, 2) .getEntry(); @@ -178,4 +183,13 @@ public void clearTrajectory() { public SendableChooser getAutoChooser() { return autoChooser; } + + /** + * Gets the number of seconds to delay the start of auton + * + * @return delay in seconds clamped between 0 and 10 seconds + */ + public double getAutoDelay() { + return MathUtil.clamp(autonDelay.getDouble(0), 0, 10); + } }