feat: dynamic controller switching

simgui.json

@@ -71,8 +71,8 @@
             "open": true
           }
         },
-        "Auto Controllers": {
-          "X": {
+        "Controllers": {
+          "Xbox": {
             "open": true
           },
           "open": true
@@ -87,8 +87,7 @@
         "swerve": {
           "modules": {
             "open": true
-          },
-          "open": true
+          }
         }
       }
     }

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

@@ -46,10 +46,6 @@ public static final class IOConstants {
     public static final int kRightJoystickPort = 2;
     public static final int kButtonBoardPort = 3;
     public static final double kDriveDeadband = 0.05;
-
-    // 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 class Vision {

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

@@ -0,0 +1,154 @@
+package frc.robot;
+
+import edu.wpi.first.wpilibj.Alert;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import frc.robot.utils.FilteredJoystick;
+import java.util.List;
+
+public class Controllers {
+
+  private static final ControllerState DEFAULT_CONTROLLER_STATE =
+      Robot.isSimulation() ? ControllerState.XBOX : ControllerState.JOYSTICKS;
+
+  private final Alert simulationJoysticksAlert =
+      new Alert("Joysticks are connected in simulation", Alert.AlertType.kWarning);
+  private final Alert realXboxAlert =
+      new Alert("Xbox controller is connected to physical robot", Alert.AlertType.kWarning);
+
+  // Driver joysticks
+  public final FilteredJoystick leftJoystick =
+      new FilteredJoystick(Constants.IOConstants.kLeftJoystickPort);
+  public final FilteredJoystick rightJoystick =
+      new FilteredJoystick(Constants.IOConstants.kRightJoystickPort);
+
+  // Operator controller
+  public final CommandXboxController controller =
+      new CommandXboxController(Constants.IOConstants.kControllerPort);
+
+  private ControllerState controllerState = DEFAULT_CONTROLLER_STATE;
+
+  private double lastControllerConsoleWarning = 0.;
+
+  public Controllers() {
+    SmartDashboard.putData("Controllers/Xbox", this.controller.getHID());
+
+    this.switchController(this.controllerState);
+
+    this.controller
+        .start()
+        .onTrue(Commands.runOnce(() -> this.switchController(ControllerState.XBOX)));
+    this.leftJoystick
+        .getButtonNine()
+        .onTrue(Commands.runOnce(() -> this.switchController(ControllerState.JOYSTICKS)));
+    this.rightJoystick
+        .getButtonNine()
+        .onTrue(Commands.runOnce(() -> this.switchController(ControllerState.JOYSTICKS)));
+  }
+
+  private boolean isConnected(ControllerState state) {
+    return switch (state) {
+      case XBOX -> this.isXboxConnected();
+      case JOYSTICKS -> this.isAnyJoystickConnected();
+    };
+  }
+
+  private boolean isXboxConnected() {
+    return this.controller.isConnected();
+  }
+
+  private boolean isAnyJoystickConnected() {
+    return this.leftJoystick.getJoystick().isConnected()
+        || this.rightJoystick.getJoystick().isConnected();
+  }
+
+  public boolean useXboxController() {
+    return this.controllerState != ControllerState.JOYSTICKS;
+  }
+
+  public double getControllerX() {
+    if (this.useXboxController()) {
+      return controller.getLeftX();
+    } else {
+      return leftJoystick.getX();
+    }
+  }
+
+  public double getControllerY() {
+    if (this.useXboxController()) {
+      return controller.getLeftY();
+    } else {
+      return leftJoystick.getY();
+    }
+  }
+
+  public double getControllerRotation() {
+    if (this.useXboxController()) {
+      return -controller.getRightX();
+    } else {
+      return rightJoystick.getX();
+    }
+  }
+
+  public boolean getControllerSlow() {
+    if (this.useXboxController()) {
+      return controller.rightBumper().getAsBoolean();
+    } else {
+      return rightJoystick.getButtonTwo().getAsBoolean();
+    }
+  }
+
+  public double getControllerThrottle() {
+    if (this.useXboxController()) {
+      return 1.0;
+    } else {
+      return leftJoystick.getThrottle();
+    }
+  }
+
+  public void switchController(ControllerState newState) {
+    this.controllerState = newState;
+    System.out.println("Controller: " + this.controllerState);
+    SmartDashboard.putString("Controller", this.controllerState.toString());
+
+    this.simulationJoysticksAlert.set(
+        Robot.isSimulation() && this.controllerState == ControllerState.JOYSTICKS);
+    // TODO: Maybe only with fms?
+    this.realXboxAlert.set(Robot.isReal() && this.controllerState == ControllerState.XBOX);
+  }
+
+  public void updateControllerConnections() {
+    final var time = Timer.getTimestamp();
+    if (this.realXboxAlert.get() && this.lastControllerConsoleWarning < time - 15.) {
+      this.lastControllerConsoleWarning = time;
+      DriverStation.reportWarning(
+          "Oops! Looks like you have the Xbox controller selected for drive on a physical robot.",
+          false);
+    }
+
+    if (!this.isConnected(this.controllerState)) {
+      // Try the default first, then try everything else.
+      if (this.isConnected(DEFAULT_CONTROLLER_STATE)) {
+        this.switchController(DEFAULT_CONTROLLER_STATE);
+      } else {
+        for (final var controller : ControllerState.VALUES) {
+          if (this.isConnected(controller)) {
+            this.switchController(controller);
+            break;
+          }
+        }
+      }
+    }
+  }
+
+  public enum ControllerState {
+    XBOX,
+    JOYSTICKS,
+    ;
+
+    public static final List<ControllerState> VALUES = List.of(values());
+  }
+}

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

@@ -78,10 +78,7 @@ public void robotPeriodic() {
     // block in order for anything in the Command-based framework to work.
     CommandScheduler.getInstance().run();
     SmartDashboard.putData(CommandScheduler.getInstance());
-
-    if (Constants.IOConstants.kTestMode) {
-      System.out.println("Test Mode Enabled\nNot for competition use");
-    }
+    this.m_robotContainer.updateControllerConnections();
   }
 
   /** This function is called once each time the robot enters Disabled mode. */

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

@@ -9,8 +9,8 @@
 import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import frc.robot.Constants.IOConstants;
 import frc.robot.commands.GrabCoralCommand;
@@ -24,9 +24,7 @@
 import frc.robot.subsystems.sim.ElevatorSubsystemSim;
 import frc.robot.subsystems.swervedrive.SwerveSubsystem;
 import frc.robot.utils.FilteredButton;
-import frc.robot.utils.FilteredJoystick;
 import java.io.File;
-import java.util.Optional;
 
 /*
  * This class is where the bulk of the robot should be declared.  Since Command-based is a
@@ -46,15 +44,7 @@ public class RobotContainer {
   private final ChuteSubsystem m_chute = new ChuteSubsystem();
   private final ClimberSubsystem m_climber = new ClimberSubsystem();
 
-  // Driver joysticks
-  private final FilteredJoystick m_leftJoystick =
-      new FilteredJoystick(IOConstants.kLeftJoystickPort);
-  private final FilteredJoystick m_rightJoystick =
-      new FilteredJoystick(IOConstants.kRightJoystickPort);
-
-  // Operator controller
-  private final CommandXboxController m_controller =
-      new CommandXboxController(IOConstants.kControllerPort);
+  private final Controllers controllers = new Controllers();
 
   // Button Board
   private final FilteredButton m_buttons = new FilteredButton(IOConstants.kButtonBoardPort);
@@ -69,32 +59,22 @@ public class RobotContainer {
   public RobotContainer() {
     configureButtonBindings();
 
+    // Set default drive command
+    m_drive.setDefaultCommand(
+        new Drive(
+            m_drive,
+            controllers::getControllerX,
+            controllers::getControllerY,
+            controllers::getControllerRotation,
+            controllers::getControllerSlow,
+            controllers::getControllerThrottle));
+
     m_autoChooser.addRoutine("Test Routine", m_routines::test);
     m_autoChooser.addRoutine("Blue Processor Routine", m_routines::blueProcessor);
     m_autoChooser.addRoutine("Blue Coral Station Routine", m_routines::blueCoralStation);
     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);
-
-    if (IOConstants.kTestMode) {
-      m_drive.setDefaultCommand(
-          new Drive(
-              m_drive,
-              m_controller::getLeftY,
-              m_controller::getLeftX,
-              () -> -m_controller.getRightX(),
-              () -> m_controller.rightBumper().getAsBoolean(),
-              Optional.empty()));
-    } else {
-      m_drive.setDefaultCommand(
-          new Drive(
-              m_drive,
-              m_leftJoystick::getY,
-              m_leftJoystick::getX,
-              m_rightJoystick::getX,
-              () -> m_rightJoystick.getButtonTwo().getAsBoolean(),
-              Optional.of(m_rightJoystick::getThrottle)));
-    }
   }
 
   /**
@@ -104,41 +84,61 @@ public RobotContainer() {
    * {@link JoystickButton}.
    */
   private void configureButtonBindings() {
+    this.controllers.controller.a().onTrue(Commands.runOnce(m_drive::zeroGyro));
 
-    m_controller
+    if (Robot.isSimulation()) {
+      this.controllers
+              .controller
+              .b()
+              .onTrue(Commands.runOnce(() -> ((CoralHandlerSubsystemSim) m_coral).getSimCoral()));
+    }
+
+    this.controllers.controller
         .rightTrigger()
         .onTrue(Commands.runOnce(m_coral::release))
         .onFalse(Commands.runOnce(m_coral::inactive));
 
-    m_controller
+    this.controllers.controller
         .povDown()
         .onTrue(Commands.runOnce(m_climber::down))
         .onFalse(Commands.runOnce(m_climber::inactive));
-    m_controller
+    this.controllers.controller
         .povUp()
         .onTrue(Commands.runOnce(m_climber::up))
         .onFalse(Commands.runOnce(m_climber::inactive));
 
-    // 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));
 
-      // drop chute
-      m_buttons.getChuteSwitch().onTrue(Commands.runOnce(m_chute::drop));
+    // Zero gyro with A button
+    this.controllers.controller.a().onTrue(Commands.runOnce(m_drive::zeroGyro));
 
-      // Zero gyro with A button
-      m_controller.a().onTrue(Commands.runOnce(m_drive::zeroGyro));
+    if (!Robot.isReal()) {
+      this.controllers.controller
+          .b()
+          .onTrue(Commands.runOnce(() -> ((CoralHandlerSubsystemSim) m_coral).getSimCoral()));
+    }
 
-      if (!Robot.isReal()) {
-        m_controller
-            .b()
-            .onTrue(Commands.runOnce(() -> ((CoralHandlerSubsystemSim) m_coral).getSimCoral()));
-      }
+    this.controllers.controller.rightBumper().onTrue(new ReleaseCoralCommand(m_coral));
+    this.controllers.controller.leftBumper().onTrue(new GrabCoralCommand(m_coral));
 
-      m_controller.rightBumper().onTrue(new ReleaseCoralCommand(m_coral));
-      m_controller.leftBumper().onTrue(new GrabCoralCommand(m_coral));
-    }
+    m_buttons.getChuteSwitch().onTrue(Commands.runOnce(m_chute::drop));
+
+    this.controllers.controller.rightBumper().onTrue(new ReleaseCoralCommand(m_coral));
+    this.controllers.controller.leftBumper().onTrue(new GrabCoralCommand(m_coral));
+  }
+
+  public void updateControllerConnections() {
+    this.controllers.updateControllerConnections();
+  }
+
+  /**
+   * Use this to pass the autonomous command to the main {@link Robot} class.
+   *
+   * @return the command to run in autonomous
+   */
+  public Command getAutonomousCommand() {
+    // return autoChooser.getSelected();
+    return null;
   }
 }