diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 33a9060..af380a3 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -126,6 +126,39 @@ public static class Auto { /** Time in seconds that we delay our taxi in the delayed taxi auto. */ public static final double TAXI_DELAY_TIME = 10.0; + + /** Time in seconds that we wait for FireNoteRoutine to end. */ + public static final double DUMP_NOTE_WAIT_TIME = 4.0; + + /** Time in seconds that we wait for PickupFromCenter to end. */ + public static final double PICKUP_CENTER_WAIT_TIME = 5.0; + + /** Time in seconds that we wait for TaxiLongAuto to end. */ + public static final double TAXI_LONG_WAIT_TIME = 2.0; + + /** Constants for the DriveToAmpBlue auto */ + public static class DriveToAmpBlue { + /** The time for the robot to drive left towards the amp. */ + public static final double LEFT_WAIT_TIME = 1; + + /** The time for the robot to drive forwards towards the amp. */ + public static final double FORWARD_WAIT_TIME = 0.5; + } + + /** Constants for the DriveToAmpRed auto */ + public static class DriveToAmpRed { + /** The time for the robot to drive left towards the amp. */ + public static final double LEFT_WAIT_TIME = 1; + + /** The time for the robot to drive forwards towards the amp. */ + public static final double FORWARD_WAIT_TIME = 0.5; + } + + /** Constants for the auto that drives bot to center of field. */ + public static class DriveToCenter { + /** The time for the robot to drive to the center. */ + public static final double DRIVE_CENTER_WAIT_TIME = 5.0; + } } /** Constants for the Intake System */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f4918db..3efc3e2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,6 +13,8 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; @@ -64,6 +66,7 @@ import frc.robot.subsystems.mailbox.Mailbox; import frc.robot.subsystems.mailbox.MailboxBelts; import frc.robot.subsystems.mailbox.MailboxPneumatics; +import java.util.Optional; /** Singleton class that contains all the robot's subsystems, commands, and button bindings. */ @SuppressWarnings("unused") @@ -387,4 +390,18 @@ private void configureBindings() { public Command getAutonomousCommand() { return autoChooser.getSelected(); } + + /** + * Returns true if the robot is on the red alliance. False otherwise. + * + * @return True if the robot is on the red alliance. False otherwise. + */ + public static boolean isRedAlliance() { + Optional alliance = DriverStation.getAlliance(); + if (!alliance.isEmpty()) { + return alliance.get() == Alliance.Red; + } else { + return false; + } + } } diff --git a/src/main/java/frc/robot/commands/SeekTargetWithLimelight.java b/src/main/java/frc/robot/commands/SeekTargetWithLimelight.java index 81e4154..923d150 100644 --- a/src/main/java/frc/robot/commands/SeekTargetWithLimelight.java +++ b/src/main/java/frc/robot/commands/SeekTargetWithLimelight.java @@ -11,7 +11,6 @@ package frc.robot.commands; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants; import frc.robot.RobotContainer; @@ -52,8 +51,7 @@ public void initialize() { this.oldPipelineNumber = limelight.getLimelightPipeline(); limelight.setLimelightPipeline(seekingPipelineNumber); - var alliance = DriverStation.getAlliance(); - if (alliance.isPresent() ? alliance.get() == DriverStation.Alliance.Red : false) { + if (RobotContainer.isRedAlliance()) { this.rotationRadiansPerSecond = rotationRadiansPerSecond * -1; } } diff --git a/src/main/java/frc/robot/commands/auto/DriveToAmpBlue.java b/src/main/java/frc/robot/commands/auto/DriveToAmpBlue.java index 49b4348..30c4b38 100644 --- a/src/main/java/frc/robot/commands/auto/DriveToAmpBlue.java +++ b/src/main/java/frc/robot/commands/auto/DriveToAmpBlue.java @@ -14,11 +14,9 @@ import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.Constants; import frc.robot.commands.drive.DriveFieldOrientedHeadingSnapping; -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html /** Auto that manually drives bot to blue amp. Made due to faulty limelight at comp. */ public class DriveToAmpBlue extends SequentialCommandGroup { @@ -26,7 +24,7 @@ public class DriveToAmpBlue extends SequentialCommandGroup { public DriveToAmpBlue() { super( new ParallelDeadlineGroup( - new WaitCommand(1), + new WaitCommand(Constants.Auto.DriveToAmpBlue.LEFT_WAIT_TIME), new DriveFieldOrientedHeadingSnapping( () -> 0.0, () -> 0.0, @@ -36,7 +34,7 @@ public DriveToAmpBlue() { () -> true, () -> false)), new ParallelDeadlineGroup( - new WaitCommand(0.5), + new WaitCommand(Constants.Auto.DriveToAmpBlue.FORWARD_WAIT_TIME), new DriveFieldOrientedHeadingSnapping( () -> 0.0, () -> 1.0, diff --git a/src/main/java/frc/robot/commands/auto/DriveToAmpRed.java b/src/main/java/frc/robot/commands/auto/DriveToAmpRed.java index 75f9a6a..8b4cc19 100644 --- a/src/main/java/frc/robot/commands/auto/DriveToAmpRed.java +++ b/src/main/java/frc/robot/commands/auto/DriveToAmpRed.java @@ -14,11 +14,9 @@ import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.Constants; import frc.robot.commands.drive.DriveFieldOrientedHeadingSnapping; -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html /** Auto that manually drives bot to red amp. Made due to faulty limelight at comp. */ public class DriveToAmpRed extends SequentialCommandGroup { @@ -26,7 +24,7 @@ public class DriveToAmpRed extends SequentialCommandGroup { public DriveToAmpRed() { super( new ParallelDeadlineGroup( - new WaitCommand(1), + new WaitCommand(Constants.Auto.DriveToAmpRed.LEFT_WAIT_TIME), new DriveFieldOrientedHeadingSnapping( () -> 0.0, () -> 0.0, @@ -36,7 +34,7 @@ public DriveToAmpRed() { () -> false, () -> true)), new ParallelDeadlineGroup( - new WaitCommand(0.5), + new WaitCommand(Constants.Auto.DriveToAmpRed.FORWARD_WAIT_TIME), new DriveFieldOrientedHeadingSnapping( () -> 0.0, () -> -1.0, @@ -44,6 +42,6 @@ public DriveToAmpRed() { () -> false, () -> false, () -> false, - () -> false))); /* TODO: CONSTANTS */ + () -> false))); } } diff --git a/src/main/java/frc/robot/commands/auto/DriveToCenterAuto.java b/src/main/java/frc/robot/commands/auto/DriveToCenterAuto.java index d6aac6c..3db0b1d 100644 --- a/src/main/java/frc/robot/commands/auto/DriveToCenterAuto.java +++ b/src/main/java/frc/robot/commands/auto/DriveToCenterAuto.java @@ -14,20 +14,16 @@ import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.Constants; import frc.robot.commands.drive.DriveFieldOrientedHeadingSnapping; -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html /** Auto that drives bot to center of field. */ public class DriveToCenterAuto extends ParallelCommandGroup { /** Creates a new PickUpFromCenterAuto. */ public DriveToCenterAuto() { - // Add your commands in the addCommands() call, e.g. - // addCommands(new FooCommand(), new BarCommand()); addCommands( new ParallelDeadlineGroup( - new WaitCommand(5), + new WaitCommand(Constants.Auto.DriveToCenter.DRIVE_CENTER_WAIT_TIME), new DriveFieldOrientedHeadingSnapping( () -> 1.0, () -> 0.0, diff --git a/src/main/java/frc/robot/commands/auto/DumpNote.java b/src/main/java/frc/robot/commands/auto/DumpNote.java index 1475b47..8977202 100644 --- a/src/main/java/frc/robot/commands/auto/DumpNote.java +++ b/src/main/java/frc/robot/commands/auto/DumpNote.java @@ -13,18 +13,13 @@ import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.Constants; import frc.robot.commands.mailbox.FireNoteRoutine; -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html /** Auto that dumps the preloaded note in the robot after 4 seconds. */ public class DumpNote extends ParallelDeadlineGroup { /** Creates a new DumpNote. */ public DumpNote() { - // Add the deadline command in the super() call. Add other commands using - // addCommands(). - super(new WaitCommand(4), new FireNoteRoutine()); - // addCommands(new FooCommand(), new BarCommand()); + super(new WaitCommand(Constants.Auto.DUMP_NOTE_WAIT_TIME), new FireNoteRoutine()); } } diff --git a/src/main/java/frc/robot/commands/auto/OnePieceAutoButItWorksISwear.java b/src/main/java/frc/robot/commands/auto/OnePieceAutoButItWorksISwear.java index 0367ff2..35bae2e 100644 --- a/src/main/java/frc/robot/commands/auto/OnePieceAutoButItWorksISwear.java +++ b/src/main/java/frc/robot/commands/auto/OnePieceAutoButItWorksISwear.java @@ -11,12 +11,9 @@ package frc.robot.commands.auto; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.RobotContainer; -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html // One piece - the anime thing /** * Auto that manually drives to the amp and offloads the note. Made due to faulty limelight at comp. @@ -24,10 +21,8 @@ public class OnePieceAutoButItWorksISwear extends SequentialCommandGroup { /** Creates a new OnePieceAutoButItWorksISwear. */ public OnePieceAutoButItWorksISwear() { - // Add your commands in the addCommands() call, e.g. - // addCommands(new FooCommand(), new BarCommand()); - var alliance = DriverStation.getAlliance(); - if (alliance.isPresent() ? alliance.get() == DriverStation.Alliance.Red : false) { + + if (RobotContainer.isRedAlliance()) { addCommands(new DriveToAmpRed(), new DumpNote()); } else { addCommands(new DriveToAmpBlue(), new DumpNote()); diff --git a/src/main/java/frc/robot/commands/auto/PickUpFromCenterAuto.java b/src/main/java/frc/robot/commands/auto/PickUpFromCenterAuto.java index 613871c..ed09b49 100644 --- a/src/main/java/frc/robot/commands/auto/PickUpFromCenterAuto.java +++ b/src/main/java/frc/robot/commands/auto/PickUpFromCenterAuto.java @@ -14,22 +14,19 @@ import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.Constants; import frc.robot.commands.RunIntake; import frc.robot.commands.drive.DriveFieldOrientedHeadingSnapping; -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html /** Auto that drives the bot to a note and intakes it. */ public class PickUpFromCenterAuto extends ParallelCommandGroup { /** Creates a new PickUpFromCenterAuto. */ public PickUpFromCenterAuto() { - // Add your commands in the addCommands() call, e.g. - // addCommands(new FooCommand(), new BarCommand()); + addCommands( new RunIntake(), new ParallelDeadlineGroup( - new WaitCommand(5), + new WaitCommand(Constants.Auto.PICKUP_CENTER_WAIT_TIME), new DriveFieldOrientedHeadingSnapping( () -> 1.0, () -> 0.0, diff --git a/src/main/java/frc/robot/commands/auto/TaxiLongAuto.java b/src/main/java/frc/robot/commands/auto/TaxiLongAuto.java index 7c132bd..8822079 100644 --- a/src/main/java/frc/robot/commands/auto/TaxiLongAuto.java +++ b/src/main/java/frc/robot/commands/auto/TaxiLongAuto.java @@ -23,7 +23,7 @@ public class TaxiLongAuto extends ParallelDeadlineGroup { /** Creates a new TaxiAuto. */ public TaxiLongAuto() { - super(new WaitCommand(2)); + super(new WaitCommand(Constants.Auto.TAXI_LONG_WAIT_TIME)); addCommands( new DriveAutoRobotOriented( new Translation2d(Constants.Auto.TAXI_AUTO_METERS_PER_SECOND, 0), 0)); diff --git a/src/main/java/frc/robot/commands/lightstrip/EnabledLight.java b/src/main/java/frc/robot/commands/lightstrip/EnabledLight.java index 5f3bbe7..bc1af95 100644 --- a/src/main/java/frc/robot/commands/lightstrip/EnabledLight.java +++ b/src/main/java/frc/robot/commands/lightstrip/EnabledLight.java @@ -11,13 +11,10 @@ package frc.robot.commands.lightstrip; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants; import frc.robot.RobotContainer; import frc.robot.subsystems.AddressableLightStrip; -import java.util.Optional; /** Activates the light strip when the robot is enabled. */ public class EnabledLight extends Command { @@ -32,16 +29,13 @@ public EnabledLight() { // Called when the command is initially scheduled. @Override public void initialize() { - Optional alliance = DriverStation.getAlliance(); - if (alliance.isPresent()) { - if (alliance.get() == Alliance.Red) { - robotLights.setStripColor(Constants.LightStrips.Colors.ENABLE_COLOR_RED_ALLIANCE); - } else { - robotLights.setStripColor(Constants.LightStrips.Colors.ENABLE_COLOR_BLUE_ALLIANCE); - } + + if (RobotContainer.isRedAlliance()) { + robotLights.setStripColor(Constants.LightStrips.Colors.ENABLE_COLOR_RED_ALLIANCE); } else { - robotLights.setStripColor(Constants.LightStrips.Colors.ENABLE_COLOR_NO_ALLIANCE); + robotLights.setStripColor(Constants.LightStrips.Colors.ENABLE_COLOR_BLUE_ALLIANCE); } + robotLights.flush(); robotLights.startLights(); } diff --git a/src/main/java/frc/robot/subsystems/Drive.java b/src/main/java/frc/robot/subsystems/Drive.java index fea140c..1c21d49 100644 --- a/src/main/java/frc/robot/subsystems/Drive.java +++ b/src/main/java/frc/robot/subsystems/Drive.java @@ -19,12 +19,12 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.GenericEntry; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; +import frc.robot.RobotContainer; import java.io.File; import java.io.IOException; import swervelib.SwerveDrive; @@ -79,11 +79,7 @@ public Drive() { /* This will flip the path being followed to the red side of the field. */ /* THE ORIGIN WILL REMAIN ON THE BLUE SIDE */ - var alliance = DriverStation.getAlliance(); - if (alliance.isPresent()) { - return alliance.get() == DriverStation.Alliance.Red; - } - return false; + return RobotContainer.isRedAlliance(); }, this /* Reference to this subsystem to set requirements */); }