diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7312fe9e..2a9f5de5 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -22,6 +22,14 @@ * constants are needed, to reduce verbosity. */ public final class Constants { + + /** Constants for the Mailbox system */ + public static class Mailbox { + // It was me, DIO! + /** DIO Port ID for the Mailbox limit switch. */ + public static final int MAILBOX_LIMIT_SWITCH_DIO_PORT = 0; + } + /** Constants for the Pneumatics system. */ public static class MailboxPneumatics { /** The channel on the PCM for the forward direction on the left solenoid. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3f167ee5..62877c65 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -19,14 +19,19 @@ import frc.robot.commands.DeployPneumatics; import frc.robot.commands.DeployUrMom; import frc.robot.commands.EnterXMode; -import frc.robot.commands.RunBelts; import frc.robot.commands.RunIntake; import frc.robot.commands.drive.DriveFieldOriented; import frc.robot.commands.drive.DriveRobotOriented; +import frc.robot.commands.mailbox.DeindexNote; +import frc.robot.commands.mailbox.DeployMailbox; +import frc.robot.commands.mailbox.DeployPneumatics; +import frc.robot.commands.mailbox.FireNoteRoutine; +import frc.robot.commands.mailbox.RunBelts; import frc.robot.subsystems.Drive; import frc.robot.subsystems.Intake; import frc.robot.subsystems.Limelight; import frc.robot.subsystems.UrMom; +import frc.robot.subsystems.mailbox.Mailbox; import frc.robot.subsystems.mailbox.MailboxBelts; import frc.robot.subsystems.mailbox.MailboxPneumatics; @@ -52,6 +57,9 @@ public class RobotContainer { /** Singleton instance of {@link MailboxBelts} for the whole robot. */ public static MailboxBelts mailboxBelts = new MailboxBelts(); + /** Singleton instance of {@link Mailbox} for the whole robot. */ + public static Mailbox mailbox = new Mailbox(); + /** Singleton instance of {@link Intake} for the whole robot. */ public static Intake intake = new Intake(); @@ -73,6 +81,9 @@ public class RobotContainer { private EnterXMode enterXMode = new EnterXMode(); private DeployPneumatics deployPneumatics = new DeployPneumatics(); private RunBelts runBelts = new RunBelts(); + private DeployMailbox deployMailbox = new DeployMailbox(); + private DeindexNote deindexNote = new DeindexNote(); + private FireNoteRoutine fireNote = new FireNoteRoutine(); private RunIntake runIntake = new RunIntake(); private AimWithLimelight aimToAmp = new AimWithLimelight( @@ -113,7 +124,7 @@ private void configureBindings() { driverController.x().onTrue(enterXMode); driverController.a().onTrue(runIntake); - driverController.y().onTrue(deployPneumatics); + driverController.y().onTrue(fireNote); } /** diff --git a/src/main/java/frc/robot/commands/mailbox/DeindexNote.java b/src/main/java/frc/robot/commands/mailbox/DeindexNote.java new file mode 100644 index 00000000..37d45433 --- /dev/null +++ b/src/main/java/frc/robot/commands/mailbox/DeindexNote.java @@ -0,0 +1,60 @@ +// 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.mailbox; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.RobotContainer; +import frc.robot.subsystems.Intake; +import frc.robot.subsystems.mailbox.Mailbox; + +/** Outputs the note from the index belts into the mailbox belts. */ +public class DeindexNote extends Command { + + private Intake intake; + private Mailbox mailbox; + + /** Creates a new DeindexNote. */ + public DeindexNote() { + this.intake = RobotContainer.intake; + this.mailbox = RobotContainer.mailbox; + addRequirements(intake, mailbox); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + /* If the mailbox is fully raised, run the intake. */ + if (mailbox.getLimitSwitch()) { + intake.runIntake(); + } + /* This else isn't neccessary, just advised for safety. If it interferes with anything, feel free to remove it. */ + else { + intake.stop(); + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + intake.stop(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/DeployMailbox.java b/src/main/java/frc/robot/commands/mailbox/DeployMailbox.java similarity index 96% rename from src/main/java/frc/robot/commands/DeployMailbox.java rename to src/main/java/frc/robot/commands/mailbox/DeployMailbox.java index b54b9fcb..8fb4d8d5 100644 --- a/src/main/java/frc/robot/commands/DeployMailbox.java +++ b/src/main/java/frc/robot/commands/mailbox/DeployMailbox.java @@ -9,7 +9,7 @@ * 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; +package frc.robot.commands.mailbox; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; diff --git a/src/main/java/frc/robot/commands/DeployPneumatics.java b/src/main/java/frc/robot/commands/mailbox/DeployPneumatics.java similarity index 97% rename from src/main/java/frc/robot/commands/DeployPneumatics.java rename to src/main/java/frc/robot/commands/mailbox/DeployPneumatics.java index 8c072a71..ba7e9483 100644 --- a/src/main/java/frc/robot/commands/DeployPneumatics.java +++ b/src/main/java/frc/robot/commands/mailbox/DeployPneumatics.java @@ -9,7 +9,7 @@ * 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; +package frc.robot.commands.mailbox; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.RobotContainer; diff --git a/src/main/java/frc/robot/commands/mailbox/FireNoteRoutine.java b/src/main/java/frc/robot/commands/mailbox/FireNoteRoutine.java new file mode 100644 index 00000000..4d32786d --- /dev/null +++ b/src/main/java/frc/robot/commands/mailbox/FireNoteRoutine.java @@ -0,0 +1,25 @@ +// 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.mailbox; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; + +/** + * Scores the note into the amp. Raises the mailbox, runs the mailbox belts, and then runs the index + * belts to send the note into the belts. + */ +public class FireNoteRoutine extends ParallelCommandGroup { + /** Creates a new FireNote. */ + public FireNoteRoutine() { + addCommands(new DeployMailbox(), new DeindexNote()); + } +} diff --git a/src/main/java/frc/robot/commands/RunBelts.java b/src/main/java/frc/robot/commands/mailbox/RunBelts.java similarity index 97% rename from src/main/java/frc/robot/commands/RunBelts.java rename to src/main/java/frc/robot/commands/mailbox/RunBelts.java index f42494b4..68bef281 100644 --- a/src/main/java/frc/robot/commands/RunBelts.java +++ b/src/main/java/frc/robot/commands/mailbox/RunBelts.java @@ -9,7 +9,7 @@ * 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; +package frc.robot.commands.mailbox; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.RobotContainer; diff --git a/src/main/java/frc/robot/subsystems/mailbox/Mailbox.java b/src/main/java/frc/robot/subsystems/mailbox/Mailbox.java index ee925a4d..60d5c747 100644 --- a/src/main/java/frc/robot/subsystems/mailbox/Mailbox.java +++ b/src/main/java/frc/robot/subsystems/mailbox/Mailbox.java @@ -11,12 +11,30 @@ package frc.robot.subsystems.mailbox; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; /** Subsystem for the mailbox that outputs game pieces from our robot. */ public class Mailbox extends SubsystemBase { + + /** Limit switch that detects when the mailbox is raised. */ + private DigitalInput limitSwitch; + /** Creates a new Mailbox. */ - public Mailbox() {} + public Mailbox() { + this.limitSwitch = new DigitalInput(Constants.Mailbox.MAILBOX_LIMIT_SWITCH_DIO_PORT); + } + + /** + * Gets the Mailbox Limit Switch's Value. + * + * @return True if the mailbox is fully raised. False otherwise. + */ + public boolean getLimitSwitch() { + /* Assumes the limit switch is wired to be normally open. */ + return limitSwitch.get(); + } @Override public void periodic() {