diff --git a/simgui.json b/simgui.json index 7a24c87..49a3807 100644 --- a/simgui.json +++ b/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 + } } } } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ccf67b1..6c87808 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/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 { diff --git a/src/main/java/frc/robot/Controllers.java b/src/main/java/frc/robot/Controllers.java new file mode 100644 index 0000000..ad8583e --- /dev/null +++ b/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 VALUES = List.of(values()); + } +} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 694b0b4..ecab716 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/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. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5622f4e..e08af73 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/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; } } diff --git a/src/main/java/frc/robot/commands/drive/Drive.java b/src/main/java/frc/robot/commands/drive/Drive.java index e9b6914..df6c1e1 100644 --- a/src/main/java/frc/robot/commands/drive/Drive.java +++ b/src/main/java/frc/robot/commands/drive/Drive.java @@ -3,7 +3,6 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants.DriveConstants; import frc.robot.subsystems.swervedrive.SwerveSubsystem; -import java.util.Optional; import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; import swervelib.SwerveInputStream; @@ -15,7 +14,7 @@ public class Drive extends Command { final DoubleSupplier m_y; final DoubleSupplier m_rotation; final BooleanSupplier m_slow; - final Optional m_throttle; + final DoubleSupplier m_throttle; SwerveInputStream driveInput; public Drive( @@ -24,7 +23,7 @@ public Drive( DoubleSupplier y, DoubleSupplier rotation, BooleanSupplier slow, - Optional throttle) { + DoubleSupplier throttle) { m_drive = drive; m_x = x; m_y = y; @@ -45,20 +44,20 @@ public void initialize() { * (m_slow.getAsBoolean() ? DriveConstants.kDrivingSpeeds[1] : DriveConstants.kDrivingSpeeds[0]) - * m_throttle.orElse(() -> 1.0).getAsDouble(), + * m_throttle.getAsDouble(), () -> m_y.getAsDouble() * (m_slow.getAsBoolean() ? DriveConstants.kDrivingSpeeds[1] : DriveConstants.kDrivingSpeeds[0]) - * m_throttle.orElse(() -> 1.0).getAsDouble()) + * m_throttle.getAsDouble()) .withControllerRotationAxis( () -> m_rotation.getAsDouble() * (m_slow.getAsBoolean() ? DriveConstants.kRotationSpeeds[1] : DriveConstants.kRotationSpeeds[0]) - * m_throttle.orElse(() -> 1.0).getAsDouble()) + * m_throttle.getAsDouble()) .deadband(0.1) .scaleTranslation(0.8) .allianceRelativeControl(true); diff --git a/src/main/java/frc/robot/utils/FilteredJoystick.java b/src/main/java/frc/robot/utils/FilteredJoystick.java index 78503a2..f70523c 100644 --- a/src/main/java/frc/robot/utils/FilteredJoystick.java +++ b/src/main/java/frc/robot/utils/FilteredJoystick.java @@ -6,7 +6,7 @@ import frc.robot.Constants; public class FilteredJoystick { - private Joystick joystick; + private final Joystick joystick; /** * Filter class for the joysticks @@ -17,6 +17,10 @@ public FilteredJoystick(int port) { joystick = new Joystick(port); } + public Joystick getJoystick() { + return joystick; + } + /** * Returns the x-value of the joystick *