feat!: Auto alignment

simgui.json

@@ -13,9 +13,9 @@
       "/SmartDashboard/IMU": "Alerts",
       "/SmartDashboard/JSON": "Alerts",
       "/SmartDashboard/Motors": "Alerts",
-      "/SmartDashboard/PathPlanner": "Alerts",
       "/SmartDashboard/Scheduler": "Scheduler",
-      "/SmartDashboard/Swerve Drive": "Alerts"
+      "/SmartDashboard/Swerve Drive": "Alerts",
+      "/SmartDashboard/VisionSystemSim-Vision/Sim Field": "Field2d"
     },
     "windows": {
       "/SmartDashboard/Field": {
@@ -51,7 +51,6 @@
           "width": 0.30000001192092896
         },
         "bottom": 1638,
-        "builtin": "2025 Reefscape",
         "height": 8.051901817321777,
         "left": 534,
         "right": 3466,
@@ -71,8 +70,8 @@
             "open": true
           }
         },
-        "Auto Controllers": {
-          "X": {
+        "Controllers": {
+          "Xbox": {
             "open": true
           },
           "open": true
@@ -87,8 +86,7 @@
         "swerve": {
           "modules": {
             "open": true
-          },
-          "open": true
+          }
         }
       }
     }

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

@@ -49,7 +49,7 @@ public static final class IOConstants {
 
     // When test mode is enabled, the operator controller is used for driving and testing
     // This should always be false on the main branch
-    public static final boolean kTestMode = false;
+    public static final boolean kTestMode = Robot.isSimulation();
   }
 
   public static final class Vision {

src/main/java/frc/robot/Robot.java

@@ -54,6 +54,10 @@ public void robotInit() {
       DriverStation.silenceJoystickConnectionWarning(true);
       SimulatedArena.getInstance().resetFieldForAuto();
     }
+
+    if (Constants.IOConstants.kTestMode) {
+      System.out.println("Test Mode Enabled\nNot for competition use");
+    }
   }
 
   public static Robot getInstance() {
@@ -80,14 +84,10 @@ public void robotPeriodic() {
     SmartDashboard.putData(CommandScheduler.getInstance());
 
     if (Constants.IOConstants.kTestMode) {
-      System.out.println("Test Mode Enabled\nNot for competition use");
+      //      System.out.println("Test Mode Enabled\nNot for competition use");
     }
   }
 
-  /** This function is called once each time the robot enters Disabled mode. */
-  @Override
-  public void disabledInit() {}
-
   @Override
   public void disabledPeriodic() {
     if (m_autonomousCommand != null) {

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

@@ -6,6 +6,7 @@
 
 import choreo.auto.AutoChooser;
 import choreo.auto.AutoFactory;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -16,17 +17,20 @@
 import frc.robot.commands.GrabCoralCommand;
 import frc.robot.commands.ReleaseCoralCommand;
 import frc.robot.commands.drive.Drive;
+import frc.robot.commands.drive.MoveToPose;
 import frc.robot.subsystems.ChuteSubsystem;
 import frc.robot.subsystems.ClimberSubsystem;
 import frc.robot.subsystems.CoralHandlerSubsystem;
 import frc.robot.subsystems.ElevatorSubsystem;
 import frc.robot.subsystems.sim.CoralHandlerSubsystemSim;
 import frc.robot.subsystems.sim.ElevatorSubsystemSim;
 import frc.robot.subsystems.swervedrive.SwerveSubsystem;
+import frc.robot.utils.AprilTags;
 import frc.robot.utils.FilteredButton;
 import frc.robot.utils.FilteredJoystick;
 import java.io.File;
 import java.util.Optional;
+import java.util.Set;
 
 /*
  * This class is where the bulk of the robot should be declared.  Since Command-based is a
@@ -75,13 +79,18 @@ public RobotContainer() {
     m_autoChooser.addRoutine("Blue Reef K Routine", m_routines::blueCoralToReefK);
     m_autoChooser.addRoutine("Blue Test Full Routine", m_routines::blueTestFull);
     SmartDashboard.putData("Auto Chooser", m_autoChooser);
+    SmartDashboard.putData("Xbox Controller Debug", m_controller.getHID());
+
+    if (Robot.isSimulation()) {
+      DriverStation.silenceJoystickConnectionWarning(true);
+    }
 
     if (IOConstants.kTestMode) {
       m_drive.setDefaultCommand(
           new Drive(
               m_drive,
-              m_controller::getLeftY,
-              m_controller::getLeftX,
+              () -> -m_controller.getLeftY(),
+              () -> -m_controller.getLeftX(),
               () -> -m_controller.getRightX(),
               () -> m_controller.rightBumper().getAsBoolean(),
               Optional.empty()));
@@ -121,10 +130,10 @@ private void configureButtonBindings() {
 
     // Test mode allows everything to be run on a single controller
     // Test mode should not be enabled in competition
+
     if (IOConstants.kTestMode) {
       m_controller.a().onTrue(Commands.runOnce(m_drive::zeroGyro));
     } else {
-
       // drop chute
       m_buttons.getChuteSwitch().onTrue(Commands.runOnce(m_chute::drop));
 
@@ -140,5 +149,36 @@ private void configureButtonBindings() {
       m_controller.rightBumper().onTrue(new ReleaseCoralCommand(m_coral));
       m_controller.leftBumper().onTrue(new GrabCoralCommand(m_coral));
     }
+
+    m_controller
+        .rightStick()
+        .whileTrue(
+            Commands.defer(
+                () ->
+                    MoveToPose.tagRelative(
+                            this.m_drive,
+                            AprilTags.findReefTagForAlignment(this.m_drive.getPose()),
+                            AprilTags.REEF_ALIGN_OFFSET)
+                        .withDriveInputs(
+                            m_controller::getLeftX,
+                            m_controller::getLeftY,
+                            () -> -m_controller.getRightX(),
+                            0.5),
+                Set.of(this.m_drive)));
+    m_controller
+        .leftStick()
+        .whileTrue(
+            Commands.defer(
+                () ->
+                    MoveToPose.tagRelative(
+                            this.m_drive,
+                            AprilTags.findStationTagForAlignment(this.m_drive.getPose()),
+                            AprilTags.STATION_ALIGN_OFFSET)
+                        .withDriveInputs(
+                            m_controller::getLeftX,
+                            m_controller::getLeftY,
+                            () -> -m_controller.getRightX(),
+                            0.5),
+                Set.of(this.m_drive)));
   }
 }