From 8f03d1432616a2a5a3be907900da4c37b2b0369f Mon Sep 17 00:00:00 2001
From: willitcode <91231142+willitcode@users.noreply.github.com>
Date: Tue, 12 Mar 2024 23:33:50 -0500
Subject: [PATCH 1/4] added method to zero gyro

---
 src/main/java/frc/robot/subsystems/Drive.java | 10 ++++++++++
 1 file changed, 10 insertions(+)

diff --git a/src/main/java/frc/robot/subsystems/Drive.java b/src/main/java/frc/robot/subsystems/Drive.java
index 70878e4e..363c37b9 100644
--- a/src/main/java/frc/robot/subsystems/Drive.java
+++ b/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.
    *

From ea71ca1a0dc55006268ea087ecd576eef06fc955 Mon Sep 17 00:00:00 2001
From: willitcode <91231142+willitcode@users.noreply.github.com>
Date: Tue, 12 Mar 2024 23:33:59 -0500
Subject: [PATCH 2/4] add zero gyro command

---
 .../frc/robot/commands/drive/ZeroGyro.java    | 47 +++++++++++++++++++
 1 file changed, 47 insertions(+)
 create mode 100644 src/main/java/frc/robot/commands/drive/ZeroGyro.java

diff --git a/src/main/java/frc/robot/commands/drive/ZeroGyro.java b/src/main/java/frc/robot/commands/drive/ZeroGyro.java
new file mode 100644
index 00000000..6d83a4b6
--- /dev/null
+++ b/src/main/java/frc/robot/commands/drive/ZeroGyro.java
@@ -0,0 +1,47 @@
+// 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;
+
+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;
+  }
+}

From d756ee1c85cf8fdf62d4c95018b752f3a8d9147a Mon Sep 17 00:00:00 2001
From: willitcode <91231142+willitcode@users.noreply.github.com>
Date: Tue, 12 Mar 2024 23:36:23 -0500
Subject: [PATCH 3/4] send zero gyro command to Shuffleboard

---
 src/main/java/frc/robot/RobotContainer.java | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index c9f39c72..4a7d7a52 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/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);
   }

From cfab0124971174cdc358c3761f375f603efc4c86 Mon Sep 17 00:00:00 2001
From: willitcode <91231142+willitcode@users.noreply.github.com>
Date: Tue, 12 Mar 2024 23:44:30 -0500
Subject: [PATCH 4/4] ZeroGyro command JavaDoc

---
 src/main/java/frc/robot/commands/drive/ZeroGyro.java | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/main/java/frc/robot/commands/drive/ZeroGyro.java b/src/main/java/frc/robot/commands/drive/ZeroGyro.java
index 6d83a4b6..9410e3c1 100644
--- a/src/main/java/frc/robot/commands/drive/ZeroGyro.java
+++ b/src/main/java/frc/robot/commands/drive/ZeroGyro.java
@@ -15,6 +15,7 @@
 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;