Merge branch 'main' into brumble-tweaks

src/main/java/frc/robot/Constants.java

@@ -29,6 +29,9 @@ 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;
+
+    /** The delay between raising the mailbox and firing the note. In seconds. */
+    public static final double FIRE_NOTE_DELAY_TIME = 0.25;
   }
 
   /** Constants for the Pneumatics system. */

src/main/java/frc/robot/Controllers.java

@@ -62,6 +62,7 @@ public final class Controllers {
   public static Supplier<Boolean> headingSnappingLeftSupplier =
       () -> rawPilotController.getPOV() == 270;
 
+  /** Shuffleboard (NT) entry for the keymap selector */
   public static GenericEntry keymapEntry =
       Shuffleboard.getTab("Driver").add("Keymap", "Default").getEntry();
 

src/main/java/frc/robot/RobotContainer.java

@@ -32,6 +32,7 @@
 import frc.robot.commands.auto.TaxiAuto;
 import frc.robot.commands.drive.DriveFieldOrientedHeadingSnapping;
 import frc.robot.commands.drive.DriveRobot;
+import frc.robot.commands.drive.ZeroGyro;
 import frc.robot.commands.mailbox.DeindexNote;
 import frc.robot.commands.mailbox.DeployMailbox;
 import frc.robot.commands.mailbox.DeployPneumatics;
@@ -226,6 +227,9 @@ public class RobotContainer {
   /** Singleton instance of {@link ClearPDPStickyFaults} for the whole robot. */
   public static ClearPDPStickyFaults clearPDPStickyFaults = new ClearPDPStickyFaults();
 
+  /** Singleton instance of {@link ZeroGyro} for the whole robot. */
+  public static ZeroGyro zeroGyro = new ZeroGyro();
+
   /*
    * ***********************
    * * OTHER INSTANCE VARS *
@@ -242,6 +246,7 @@ public RobotContainer() {
     configureBindings();
     configureAuto();
     Shuffleboard.getTab("Driver").add("Clear PDP sticky faults", clearPDPStickyFaults);
+    Shuffleboard.getTab("Driver").add("Zero Gyro", zeroGyro);
 
     drive.setDefaultCommand(driveRobotOriented);
   }

src/main/java/frc/robot/commands/drive/DriveFieldOrientedHeadingSnapping.java

@@ -21,7 +21,7 @@
 
 /**
  * Drives robot in field oriented mode with shortcuts to snap to field relative angles in increments
- * of 90º using dpad
+ * of 90 degrees using dpad
  */
 public class DriveFieldOrientedHeadingSnapping extends Command {
   private Drive drive;

src/main/java/frc/robot/commands/drive/ZeroGyro.java

@@ -0,0 +1,48 @@
+// 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.drive;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.RobotContainer;
+import frc.robot.subsystems.Drive;
+
+/** Zeroes the robot's NavX gyro. See {@link Drive#zeroGyro} for more information. */
+public class ZeroGyro extends Command {
+  private Drive drive;
+
+  /** Creates a new ZeroGyro. */
+  public ZeroGyro() {
+    // Use addRequirements() here to declare subsystem dependencies.
+    this.drive = RobotContainer.drive;
+    addRequirements(drive);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    drive.zeroGyro();
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {}
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return true;
+  }
+}

src/main/java/frc/robot/commands/mailbox/FireNoteRoutine.java

@@ -12,6 +12,8 @@
 package frc.robot.commands.mailbox;
 
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+import frc.robot.Constants;
 
 /**
  * Scores the note into the amp. Raises the mailbox, runs the mailbox belts, and then runs the index
@@ -20,6 +22,9 @@
 public class FireNoteRoutine extends ParallelCommandGroup {
   /** Creates a new FireNote. */
   public FireNoteRoutine() {
-    addCommands(new DeployMailbox(), new DeindexNote());
+    addCommands(
+        new DeployMailbox(),
+        new WaitCommand(Constants.Mailbox.FIRE_NOTE_DELAY_TIME),
+        new DeindexNote());
   }
 }

src/main/java/frc/robot/commands/mailbox/FireNoteRoutineNoLimitSwitch.java

@@ -17,7 +17,11 @@
 /**
  * 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 without a limit switch.
+ *
+ * @deprecated This class does not work consistently when there is an intake limit switch present,
+ *     which will be present for the foreseeable future.
  */
+@Deprecated
 public class FireNoteRoutineNoLimitSwitch extends ParallelCommandGroup {
   /** Creates a new FireNoteRoutineNoLimitSwitch. */
   public FireNoteRoutineNoLimitSwitch() {

src/main/java/frc/robot/subsystems/Drive.java

@@ -110,6 +110,16 @@ public void enterXMode() {
     drive.lockPose();
   }
 
+  /**
+   * Zeroes the NavX gyro. Mostly used for resetting the angle for field-oriented drive (for now).
+   *
+   * <p>Somewhat notably, this will also reset odometry to the same position it's currently at, but
+   * facing towards zero.
+   */
+  public void zeroGyro() {
+    drive.zeroGyro();
+  }
+
   /**
    * Gets the maximum speed the robot chassis can acheive in m/s.
    *