Merge branch 'main' into never-nesting

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

@@ -89,15 +89,6 @@ public static class Controllers {
   /** Constants for the Drivetrain */
   public static class Drive {
 
-    /* TODO: MAKE THIS TOGGLEABLE BY DRIVERS - PROBABLY POST-HEARTLAND */
-    public static enum DrivePerspectiveOptions {
-      RobotOriented,
-      FieldOriented
-    }
-
-    public static DrivePerspectiveOptions currentDrivePerspective =
-        DrivePerspectiveOptions.FieldOriented;
-
     /** Empty translation to prevent creating 2 Translation2ds every time the drive train stops. */
     public static Translation2d EMPTY_TRANSLATION = new Translation2d();
 

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

@@ -104,20 +104,9 @@ private static void configureDefaultKeybinds() {
     operatorController.povDown().whileTrue(RobotContainer.runIntakeReverse);
     operatorController.leftTrigger().whileTrue(RobotContainer.aimToAmp);
     operatorController.rightTrigger().whileTrue(RobotContainer.fireNote);
+    pilotController.leftTrigger().toggleOnTrue(RobotContainer.driveFieldOriented);
 
-    // pilotController.leftTrigger().toggleOnTrue(RobotContainer.driveFieldOriented);
-    pilotController.rightBumper().toggleOnTrue(RobotContainer.enterXMode);
-    switch (Constants.Drive.currentDrivePerspective) {
-      case RobotOriented:
-        pilotController.leftBumper().whileTrue(RobotContainer.driveRobotOrientedSprint);
-        break;
-      case FieldOriented:
-        pilotController.leftBumper().whileTrue(RobotContainer.driveFieldOrientedSprint);
-        break;
-      default:
-        throw new IllegalStateException();
-    }
-    /* TODO: angle / velocity steering toggle w/ right trigger (no issue) and boost on left bumper (issue 86) */
+    /* TODO: angle / velocity steering toggle w/ right trigger (no issue) */
 
     keymapEntry.setString("Default");
   }

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

@@ -33,6 +33,9 @@
 import frc.robot.commands.auto.TaxiAuto;
 import frc.robot.commands.drive.DriveFieldOrientedHeadingSnapping;
 import frc.robot.commands.drive.DriveRobot;
+import frc.robot.commands.drive.SetDrivePerspectiveFieldOriented;
+import frc.robot.commands.drive.SetDrivePerspectiveFieldOrientedHeadingSnapping;
+import frc.robot.commands.drive.SetDrivePerspectiveRobotOriented;
 import frc.robot.commands.drive.ZeroGyro;
 import frc.robot.commands.mailbox.DeindexNote;
 import frc.robot.commands.mailbox.DeployMailbox;
@@ -222,6 +225,22 @@ public class RobotContainer {
   /** Singleton instance of the intake {@link StartCamera} for the whole robot. */
   public static StartCamera startIntakeCamera = new StartCamera(intakeCamera);
 
+  /** Singleton instance of {@link SetDrivePerspectiveFieldOriented} for the whole robot. */
+  public static SetDrivePerspectiveFieldOriented setDrivePerspectiveFieldOriented =
+      new SetDrivePerspectiveFieldOriented();
+
+  /**
+   * Singleton instance of {@link SetDrivePerspectiveFieldOrientedHeadingSnapping} for the whole
+   * robot.
+   */
+  public static SetDrivePerspectiveFieldOrientedHeadingSnapping
+      setDrivePerspectiveFieldOrientedHeadingSnapping =
+          new SetDrivePerspectiveFieldOrientedHeadingSnapping();
+
+  /** Singleton instance of {@link SetDrivePerspectiveRobotOriented} for the whole robot. */
+  public static SetDrivePerspectiveRobotOriented setDrivePerspectiveRobotOriented =
+      new SetDrivePerspectiveRobotOriented();
+
   /* Autos */
   /** Singleton instance of {@link MoveAwayFromAmp} for the whole robot. */
   public static MoveAwayFromAmp moveAwayFromAmp = new MoveAwayFromAmp();
@@ -255,19 +274,18 @@ public RobotContainer() {
     configureAuto();
     Shuffleboard.getTab("Driver").add("Clear PDP sticky faults", clearPDPStickyFaults);
     Shuffleboard.getTab("Driver").add("Zero Gyro", zeroGyro);
+    Shuffleboard.getTab("Driver")
+        .add("Set Drive Perspective to Field Oriented", setDrivePerspectiveFieldOriented);
+    Shuffleboard.getTab("Driver")
+        .add(
+            "Set Drive Perspective to Field Oriented Heading Snapping",
+            setDrivePerspectiveFieldOrientedHeadingSnapping);
+    Shuffleboard.getTab("Driver")
+        .add("Set Drive Perspective Robot Oriented", setDrivePerspectiveRobotOriented);
 
     startIntakeCamera.schedule();
 
-    switch (Constants.Drive.currentDrivePerspective) {
-      case RobotOriented:
-        drive.setDefaultCommand(driveRobotOriented);
-        break;
-      case FieldOriented:
-        drive.setDefaultCommand(driveFieldOriented);
-        break;
-      default:
-        throw new IllegalStateException();
-    }
+    drive.setDefaultCommand(driveFieldOriented);
   }
 
   private void configureAuto() {

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

@@ -0,0 +1,49 @@
+// 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.Controllers;
+import frc.robot.RobotContainer;
+import frc.robot.subsystems.Drive;
+
+/** Sets the current drive perspective to field oriented. */
+public class SetDrivePerspectiveFieldOriented extends Command {
+  /** Creates a new SetDrivePerspectiveFieldOriented. */
+  private Drive drive;
+
+  public SetDrivePerspectiveFieldOriented() {
+    this.drive = RobotContainer.drive;
+    addRequirements(drive);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    drive.setDefaultCommand(RobotContainer.driveFieldOriented);
+    Controllers.pilotController.leftBumper().whileTrue(RobotContainer.driveFieldOrientedSprint);
+  }
+
+  // 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/drive/SetDrivePerspectiveFieldOrientedHeadingSnapping.java

@@ -0,0 +1,49 @@
+// 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.Controllers;
+import frc.robot.RobotContainer;
+import frc.robot.subsystems.Drive;
+
+/** Sets the current drive perspective to field oriented with heading snapping. */
+public class SetDrivePerspectiveFieldOrientedHeadingSnapping extends Command {
+  /** Creates a new SetDrivePerspectiveFieldOriented. */
+  private Drive drive;
+
+  public SetDrivePerspectiveFieldOrientedHeadingSnapping() {
+    this.drive = RobotContainer.drive;
+    addRequirements(drive);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    drive.setDefaultCommand(RobotContainer.driveFieldOrientedHeadingSnapping);
+    Controllers.pilotController.leftBumper().whileTrue(RobotContainer.driveFieldOrientedSprint);
+  }
+
+  // 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/drive/SetDrivePerspectiveRobotOriented.java

@@ -0,0 +1,49 @@
+// 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.Controllers;
+import frc.robot.RobotContainer;
+import frc.robot.subsystems.Drive;
+
+/** Sets the current drive perspective to robot oriented. */
+public class SetDrivePerspectiveRobotOriented extends Command {
+  /** Creates a new SetDrivePerspectiveFieldOriented. */
+  private Drive drive;
+
+  public SetDrivePerspectiveRobotOriented() {
+    this.drive = RobotContainer.drive;
+    addRequirements(drive);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    drive.setDefaultCommand(RobotContainer.driveRobotOriented);
+    Controllers.pilotController.leftBumper().whileTrue(RobotContainer.driveRobotOrientedSprint);
+  }
+
+  // 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;
+  }
+}