Merge pull request #138 from frc937/zero-gyro

Zero gyro

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/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/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.
    *