From 38a7052bc7d8b606ee31fb1852d47f485d5e2f9d Mon Sep 17 00:00:00 2001 From: tk2217 Date: Sat, 15 Mar 2025 14:49:11 -0500 Subject: [PATCH 01/12] dynamic controller switching --- src/main/java/frc/robot/Constants.java | 4 - src/main/java/frc/robot/Robot.java | 5 +- src/main/java/frc/robot/RobotContainer.java | 155 ++++++++++++------ .../frc/robot/utils/FilteredJoystick.java | 6 +- 4 files changed, 113 insertions(+), 57 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 47b11f23..d8252605 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -60,10 +60,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/Robot.java b/src/main/java/frc/robot/Robot.java index e088aca1..f795ee33 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -77,10 +77,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 d3527c98..3ce7e722 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,9 +7,11 @@ import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.IOConstants; import frc.robot.commands.drive.Drive; import frc.robot.subsystems.CoralHandlerSubsystem; @@ -51,30 +53,22 @@ public class RobotContainer { // Button Board private final FilteredButton m_buttonBoard = new FilteredButton(IOConstants.kButtonBoardPort); + private ControllerState controllerState = ControllerState.UNKNOWN; + /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { + this.updateControllerConnections(); configureButtonBindings(); // Set default drive command - if (IOConstants.kTestMode) { - m_drive.setDefaultCommand( - new Drive( - m_drive, - m_operatorController::getRightX, - m_operatorController::getLeftY, - () -> -m_operatorController.getRightX(), - () -> m_operatorController.rightBumper().getAsBoolean(), - Optional.empty())); - } else { - m_drive.setDefaultCommand( - new Drive( - m_drive, - m_driverLeftJoystick::getX, - m_driverLeftJoystick::getY, - m_driverRightJoystick::getX, - m_driverRightJoystick::getButtonTwo, - Optional.of(m_driverLeftJoystick::getThrottle))); - } + m_drive.setDefaultCommand(new Drive( + m_drive, + this::getControllerX, + this::getControllerY, + this::getContollerRotation, + this::getControllerSlow, + Optional.of(this::getControllerThrottle) + )); } /** @@ -84,39 +78,37 @@ public RobotContainer() { * {@link JoystickButton}. */ private void configureButtonBindings() { + m_operatorController.start().onTrue(Commands.runOnce(() -> this.controllerState = ControllerState.XBOX)); + new Trigger(CommandScheduler.getInstance().getDefaultButtonLoop(), m_driverLeftJoystick::getButtonNine) + .onTrue(Commands.runOnce(() -> this.controllerState = ControllerState.JOYSTICKS)); + new Trigger(CommandScheduler.getInstance().getDefaultButtonLoop(), m_driverRightJoystick::getButtonNine) + .onTrue(Commands.runOnce(() -> this.controllerState = ControllerState.JOYSTICKS)); - // Test mode allows everything to be run on a single controller - // Test mode should not be enabled in competition - if (IOConstants.kTestMode) { - - } else { - - // Zero gyro with A button - m_operatorController.a().onTrue(Commands.runOnce(m_drive::zeroGyro)); + // Zero gyro with A button + m_operatorController.a().onTrue(Commands.runOnce(m_drive::zeroGyro)); - if (!Robot.isReal()) { - m_operatorController - .b() - .onTrue(Commands.runOnce(() -> ((CoralHandlerSubsystemSim) m_coral).getSimCoral())); - } - - m_operatorController - .x() - .onTrue(Commands.runOnce(() -> m_elevator.setState(ElevatorSubsystem.ElevatorState.L2))); - m_operatorController - .y() - .onTrue( - Commands.runOnce(() -> m_elevator.setState(ElevatorSubsystem.ElevatorState.DOWN))); - - m_operatorController - .rightBumper() - .onTrue(Commands.runOnce(m_coral::grab)) - .onFalse(Commands.runOnce(m_coral::idle)); + if (!Robot.isReal()) { m_operatorController - .leftBumper() - .onTrue(Commands.runOnce(m_coral::release)) - .onFalse(Commands.runOnce(m_coral::idle)); + .b() + .onTrue(Commands.runOnce(() -> ((CoralHandlerSubsystemSim) m_coral).getSimCoral())); } + + m_operatorController + .x() + .onTrue(Commands.runOnce(() -> m_elevator.setState(ElevatorSubsystem.ElevatorState.L2))); + m_operatorController + .y() + .onTrue( + Commands.runOnce(() -> m_elevator.setState(ElevatorSubsystem.ElevatorState.DOWN))); + + m_operatorController + .rightBumper() + .onTrue(Commands.runOnce(m_coral::grab)) + .onFalse(Commands.runOnce(m_coral::idle)); + m_operatorController + .leftBumper() + .onTrue(Commands.runOnce(m_coral::release)) + .onFalse(Commands.runOnce(m_coral::idle)); } /** @@ -128,4 +120,71 @@ public Command getAutonomousCommand() { // return autoChooser.getSelected(); return null; } + + public boolean isXboxController() { + return this.controllerState != ControllerState.JOYSTICKS; + } + + public double getControllerX() { + if (this.isXboxController()) { + return m_operatorController.getLeftX(); + } else { + return m_driverLeftJoystick.getX(); + } + } + + public double getControllerY() { + if (this.isXboxController()) { + return m_operatorController.getLeftY(); + } else { + return m_driverLeftJoystick.getY(); + } + } + + public double getContollerRotation() { + if (this.isXboxController()) { + return -m_operatorController.getRightX(); + } else { + return m_driverRightJoystick.getX(); + } + } + + public boolean getControllerSlow() { + if (this.isXboxController()) { + return m_operatorController.rightBumper().getAsBoolean(); + } else { + return m_driverRightJoystick.getButtonTwo(); + } + } + + public double getControllerThrottle() { + if (this.isXboxController()) { + return 1.0; + } else { + return m_driverLeftJoystick.getThrottle(); + } + } + + private boolean isAnyJoystickConnected() { + return this.m_driverLeftJoystick.getJoystick().isConnected() || this.m_driverRightJoystick.getJoystick().isConnected(); + } + + public void updateControllerConnections() { + if (this.controllerState == ControllerState.XBOX && !this.m_operatorController.isConnected()) { + this.controllerState = ControllerState.UNKNOWN; + } else if (this.controllerState == ControllerState.JOYSTICKS && !this.isAnyJoystickConnected()) { + this.controllerState = ControllerState.UNKNOWN; + } + + if (this.controllerState == ControllerState.UNKNOWN) { + if (this.m_operatorController.isConnected()) this.controllerState = ControllerState.XBOX; + else if (this.isAnyJoystickConnected()) this.controllerState = ControllerState.JOYSTICKS; + } + } + + private enum ControllerState { + UNKNOWN, + XBOX, + JOYSTICKS, + } } diff --git a/src/main/java/frc/robot/utils/FilteredJoystick.java b/src/main/java/frc/robot/utils/FilteredJoystick.java index f878fbd8..056cf0f4 100644 --- a/src/main/java/frc/robot/utils/FilteredJoystick.java +++ b/src/main/java/frc/robot/utils/FilteredJoystick.java @@ -5,7 +5,7 @@ import frc.robot.Constants; public class FilteredJoystick { - private Joystick joystick; + private final Joystick joystick; /** * Filter class for the joysticks @@ -16,6 +16,10 @@ public FilteredJoystick(int port) { joystick = new Joystick(port); } + public Joystick getJoystick() { + return joystick; + } + /** * Returns the x-value of the joystick * From aebfe1e23c829340b6c4ad5393ac9083d6be4bd0 Mon Sep 17 00:00:00 2001 From: tk2217 Date: Sat, 15 Mar 2025 14:57:18 -0500 Subject: [PATCH 02/12] debug --- src/main/java/frc/robot/RobotContainer.java | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ac13157a..b162b14e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -8,6 +8,7 @@ import choreo.auto.AutoFactory; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -70,6 +71,7 @@ public class RobotContainer { /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { + this.setControllerState(ControllerState.UNKNOWN); this.updateControllerConnections(); configureButtonBindings(); @@ -189,16 +191,22 @@ private boolean isAnyJoystickConnected() { return this.m_leftJoystick.getJoystick().isConnected() || this.m_rightJoystick.getJoystick().isConnected(); } + private void setControllerState(ControllerState newState) { + this.controllerState = newState; + System.out.println("Controller state now " + this.controllerState); + SmartDashboard.putString("Controller State", this.controllerState.toString()); + } + public void updateControllerConnections() { if (this.controllerState == ControllerState.XBOX && !this.m_controller.isConnected()) { - this.controllerState = ControllerState.UNKNOWN; + this.setControllerState(ControllerState.UNKNOWN); } else if (this.controllerState == ControllerState.JOYSTICKS && !this.isAnyJoystickConnected()) { - this.controllerState = ControllerState.UNKNOWN; + this.setControllerState(ControllerState.UNKNOWN); } if (this.controllerState == ControllerState.UNKNOWN) { - if (this.m_controller.isConnected()) this.controllerState = ControllerState.XBOX; - else if (this.isAnyJoystickConnected()) this.controllerState = ControllerState.JOYSTICKS; + if (this.m_controller.isConnected()) this.setControllerState(ControllerState.XBOX); + else if (this.isAnyJoystickConnected()) this.setControllerState(ControllerState.JOYSTICKS); } } From 1b44d1acb1e78238956c1a0a8559b0b24b564375 Mon Sep 17 00:00:00 2001 From: Sam948-byte Date: Sat, 15 Mar 2025 16:48:36 -0500 Subject: [PATCH 03/12] lint --- src/main/java/frc/robot/RobotContainer.java | 56 +++++++++++---------- 1 file changed, 29 insertions(+), 27 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b162b14e..10ecae7e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -8,14 +8,11 @@ import choreo.auto.AutoFactory; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandScheduler; 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 edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.IOConstants; import frc.robot.commands.drive.Drive; import frc.robot.subsystems.AlgaeHandlerSubsystem; @@ -76,14 +73,14 @@ public RobotContainer() { configureButtonBindings(); // Set default drive command - m_drive.setDefaultCommand(new Drive( + m_drive.setDefaultCommand( + new Drive( m_drive, this::getControllerX, this::getControllerY, this::getContollerRotation, this::getControllerSlow, - Optional.of(this::getControllerThrottle) - )); + Optional.of(this::getControllerThrottle))); m_autoChooser.addRoutine("Test Routine", m_routines::test); m_autoChooser.addRoutine("Blue Processor Routine", m_routines::blueProcessor); @@ -100,37 +97,40 @@ public RobotContainer() { * {@link JoystickButton}. */ private void configureButtonBindings() { - m_controller.start().onTrue(Commands.runOnce(() -> this.controllerState = ControllerState.XBOX)); - m_leftJoystick.getButtonNine() - .onTrue(Commands.runOnce(() -> this.controllerState = ControllerState.JOYSTICKS)); - m_leftJoystick.getButtonNine() - .onTrue(Commands.runOnce(() -> this.controllerState = ControllerState.JOYSTICKS)); + m_controller + .start() + .onTrue(Commands.runOnce(() -> this.controllerState = ControllerState.XBOX)); + m_leftJoystick + .getButtonNine() + .onTrue(Commands.runOnce(() -> this.controllerState = ControllerState.JOYSTICKS)); + m_leftJoystick + .getButtonNine() + .onTrue(Commands.runOnce(() -> this.controllerState = ControllerState.JOYSTICKS)); // Zero gyro with A button m_controller.a().onTrue(Commands.runOnce(m_drive::zeroGyro)); if (!Robot.isReal()) { m_controller - .b() - .onTrue(Commands.runOnce(() -> ((CoralHandlerSubsystemSim) m_coral).getSimCoral())); + .b() + .onTrue(Commands.runOnce(() -> ((CoralHandlerSubsystemSim) m_coral).getSimCoral())); } m_controller - .x() - .onTrue(Commands.runOnce(() -> m_elevator.setState(ElevatorSubsystem.ElevatorState.L2))); + .x() + .onTrue(Commands.runOnce(() -> m_elevator.setState(ElevatorSubsystem.ElevatorState.L2))); m_controller - .y() - .onTrue( - Commands.runOnce(() -> m_elevator.setState(ElevatorSubsystem.ElevatorState.DOWN))); + .y() + .onTrue(Commands.runOnce(() -> m_elevator.setState(ElevatorSubsystem.ElevatorState.DOWN))); m_controller - .rightBumper() - .onTrue(Commands.runOnce(m_coral::grab)) - .onFalse(Commands.runOnce(m_coral::idle)); + .rightBumper() + .onTrue(Commands.runOnce(m_coral::grab)) + .onFalse(Commands.runOnce(m_coral::idle)); m_controller - .leftBumper() - .onTrue(Commands.runOnce(m_coral::release)) - .onFalse(Commands.runOnce(m_coral::idle)); + .leftBumper() + .onTrue(Commands.runOnce(m_coral::release)) + .onFalse(Commands.runOnce(m_coral::idle)); } /** @@ -188,7 +188,8 @@ public double getControllerThrottle() { } private boolean isAnyJoystickConnected() { - return this.m_leftJoystick.getJoystick().isConnected() || this.m_rightJoystick.getJoystick().isConnected(); + return this.m_leftJoystick.getJoystick().isConnected() + || this.m_rightJoystick.getJoystick().isConnected(); } private void setControllerState(ControllerState newState) { @@ -200,7 +201,8 @@ private void setControllerState(ControllerState newState) { public void updateControllerConnections() { if (this.controllerState == ControllerState.XBOX && !this.m_controller.isConnected()) { this.setControllerState(ControllerState.UNKNOWN); - } else if (this.controllerState == ControllerState.JOYSTICKS && !this.isAnyJoystickConnected()) { + } else if (this.controllerState == ControllerState.JOYSTICKS + && !this.isAnyJoystickConnected()) { this.setControllerState(ControllerState.UNKNOWN); } From 6ae1ce63f84e80f6204cc01a3957a1118facca61 Mon Sep 17 00:00:00 2001 From: tk2217 Date: Mon, 17 Mar 2025 18:24:36 -0500 Subject: [PATCH 04/12] , --- simgui.json | 7 +++---- src/main/java/frc/robot/RobotContainer.java | 21 ++++++++++++--------- 2 files changed, 15 insertions(+), 13 deletions(-) diff --git a/simgui.json b/simgui.json index 8254285b..1a6e4546 100644 --- a/simgui.json +++ b/simgui.json @@ -70,8 +70,8 @@ "open": true } }, - "Auto Controllers": { - "X": { + "Controllers": { + "Xbox": { "open": true }, "open": true @@ -86,8 +86,7 @@ "swerve": { "modules": { "open": true - }, - "open": true + } } } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 10ecae7e..3516484c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -64,11 +64,11 @@ public class RobotContainer { m_drive::getPose, m_drive::resetOdometry, m_drive::followTrajectory, true, m_drive); private final Routines m_routines = new Routines(m_autoFactory); - private ControllerState controllerState = ControllerState.UNKNOWN; + private ControllerState controllerState = Robot.isSimulation() ? ControllerState.XBOX : ControllerState.JOYSTICKS; /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { - this.setControllerState(ControllerState.UNKNOWN); + this.setControllerState(this.controllerState); this.updateControllerConnections(); configureButtonBindings(); @@ -88,6 +88,8 @@ 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("Controllers/Xbox", m_controller.getHID()); } /** @@ -99,13 +101,13 @@ public RobotContainer() { private void configureButtonBindings() { m_controller .start() - .onTrue(Commands.runOnce(() -> this.controllerState = ControllerState.XBOX)); + .onTrue(Commands.runOnce(() -> this.setControllerState(ControllerState.XBOX))); m_leftJoystick .getButtonNine() - .onTrue(Commands.runOnce(() -> this.controllerState = ControllerState.JOYSTICKS)); + .onTrue(Commands.runOnce(() -> this.setControllerState(ControllerState.JOYSTICKS))); m_leftJoystick .getButtonNine() - .onTrue(Commands.runOnce(() -> this.controllerState = ControllerState.JOYSTICKS)); + .onTrue(Commands.runOnce(() -> this.setControllerState(ControllerState.JOYSTICKS))); // Zero gyro with A button m_controller.a().onTrue(Commands.runOnce(m_drive::zeroGyro)); @@ -194,19 +196,19 @@ private boolean isAnyJoystickConnected() { private void setControllerState(ControllerState newState) { this.controllerState = newState; - System.out.println("Controller state now " + this.controllerState); + System.out.println("Controller state: " + this.controllerState); SmartDashboard.putString("Controller State", this.controllerState.toString()); } public void updateControllerConnections() { if (this.controllerState == ControllerState.XBOX && !this.m_controller.isConnected()) { - this.setControllerState(ControllerState.UNKNOWN); + this.setControllerState(ControllerState.DISCONNECTED); } else if (this.controllerState == ControllerState.JOYSTICKS && !this.isAnyJoystickConnected()) { - this.setControllerState(ControllerState.UNKNOWN); + this.setControllerState(ControllerState.DISCONNECTED); } - if (this.controllerState == ControllerState.UNKNOWN) { + if (this.controllerState == ControllerState.DISCONNECTED) { if (this.m_controller.isConnected()) this.setControllerState(ControllerState.XBOX); else if (this.isAnyJoystickConnected()) this.setControllerState(ControllerState.JOYSTICKS); } @@ -214,6 +216,7 @@ public void updateControllerConnections() { private enum ControllerState { UNKNOWN, + DISCONNECTED, XBOX, JOYSTICKS, } From 144d1cb24f3b329f2b5b4c4548ce11d782da0184 Mon Sep 17 00:00:00 2001 From: samfreund Date: Tue, 18 Mar 2025 11:33:46 -0500 Subject: [PATCH 05/12] istg pls run linting --- src/main/java/frc/robot/RobotContainer.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3516484c..b3e890fc 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -64,7 +64,8 @@ public class RobotContainer { m_drive::getPose, m_drive::resetOdometry, m_drive::followTrajectory, true, m_drive); private final Routines m_routines = new Routines(m_autoFactory); - private ControllerState controllerState = Robot.isSimulation() ? ControllerState.XBOX : ControllerState.JOYSTICKS; + private ControllerState controllerState = + Robot.isSimulation() ? ControllerState.XBOX : ControllerState.JOYSTICKS; /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { From c7754b38b529aa4d08e1db11c1297cb2f0167456 Mon Sep 17 00:00:00 2001 From: tk2217 Date: Tue, 18 Mar 2025 17:13:01 -0500 Subject: [PATCH 06/12] more --- src/main/java/frc/robot/Controllers.java | 104 ++++++++++++++++ src/main/java/frc/robot/RobotContainer.java | 129 +++----------------- 2 files changed, 123 insertions(+), 110 deletions(-) create mode 100644 src/main/java/frc/robot/Controllers.java diff --git a/src/main/java/frc/robot/Controllers.java b/src/main/java/frc/robot/Controllers.java new file mode 100644 index 00000000..763d6092 --- /dev/null +++ b/src/main/java/frc/robot/Controllers.java @@ -0,0 +1,104 @@ +package frc.robot; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.utils.FilteredJoystick; + +public class Controllers { + + // 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 = Robot.isSimulation() ? ControllerState.XBOX : ControllerState.JOYSTICKS; + + public Controllers() { + SmartDashboard.putData("Controllers/Xbox", this.controller.getHID()); + + this.switchController(this.controllerState); + } + + private boolean isCurrentConnected() { + return switch (this.controllerState) { + 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()); + } + + public void updateControllerConnections() { + if (!this.isCurrentConnected()) { + if (this.controller.isConnected()) this.switchController(ControllerState.XBOX); + else if (this.isAnyJoystickConnected()) this.switchController(ControllerState.JOYSTICKS); + } + } + + public enum ControllerState { + XBOX, + JOYSTICKS, + } +} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3516484c..58ffbf1f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -11,7 +11,6 @@ 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.drive.Drive; @@ -23,7 +22,6 @@ 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; @@ -45,15 +43,7 @@ public class RobotContainer { private final ChuteSubsystem m_chute = new ChuteSubsystem(); private final AlgaeHandlerSubsystem m_algae = new AlgaeHandlerSubsystem(); - // 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); @@ -64,23 +54,20 @@ public class RobotContainer { m_drive::getPose, m_drive::resetOdometry, m_drive::followTrajectory, true, m_drive); private final Routines m_routines = new Routines(m_autoFactory); - private ControllerState controllerState = Robot.isSimulation() ? ControllerState.XBOX : ControllerState.JOYSTICKS; /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { - this.setControllerState(this.controllerState); - this.updateControllerConnections(); configureButtonBindings(); // Set default drive command m_drive.setDefaultCommand( new Drive( m_drive, - this::getControllerX, - this::getControllerY, - this::getContollerRotation, - this::getControllerSlow, - Optional.of(this::getControllerThrottle))); + controllers::getControllerX, + controllers::getControllerY, + controllers::getControllerRotation, + controllers::getControllerSlow, + Optional.of(controllers::getControllerThrottle))); m_autoChooser.addRoutine("Test Routine", m_routines::test); m_autoChooser.addRoutine("Blue Processor Routine", m_routines::blueProcessor); @@ -88,8 +75,6 @@ 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("Controllers/Xbox", m_controller.getHID()); } /** @@ -99,37 +84,37 @@ public RobotContainer() { * {@link JoystickButton}. */ private void configureButtonBindings() { - m_controller + this.controllers.controller .start() - .onTrue(Commands.runOnce(() -> this.setControllerState(ControllerState.XBOX))); - m_leftJoystick + .onTrue(Commands.runOnce(() -> this.controllers.switchController(Controllers.ControllerState.XBOX))); + this.controllers.leftJoystick .getButtonNine() - .onTrue(Commands.runOnce(() -> this.setControllerState(ControllerState.JOYSTICKS))); - m_leftJoystick + .onTrue(Commands.runOnce(() -> this.controllers.switchController(Controllers.ControllerState.JOYSTICKS))); + this.controllers.rightJoystick .getButtonNine() - .onTrue(Commands.runOnce(() -> this.setControllerState(ControllerState.JOYSTICKS))); + .onTrue(Commands.runOnce(() -> this.controllers.switchController(Controllers.ControllerState.JOYSTICKS))); // Zero gyro with A button - m_controller.a().onTrue(Commands.runOnce(m_drive::zeroGyro)); + this.controllers.controller.a().onTrue(Commands.runOnce(m_drive::zeroGyro)); - if (!Robot.isReal()) { - m_controller + if (Robot.isSimulation()) { + this.controllers.controller .b() .onTrue(Commands.runOnce(() -> ((CoralHandlerSubsystemSim) m_coral).getSimCoral())); } - m_controller + this.controllers.controller .x() .onTrue(Commands.runOnce(() -> m_elevator.setState(ElevatorSubsystem.ElevatorState.L2))); - m_controller + this.controllers.controller .y() .onTrue(Commands.runOnce(() -> m_elevator.setState(ElevatorSubsystem.ElevatorState.DOWN))); - m_controller + this.controllers.controller .rightBumper() .onTrue(Commands.runOnce(m_coral::grab)) .onFalse(Commands.runOnce(m_coral::idle)); - m_controller + this.controllers.controller .leftBumper() .onTrue(Commands.runOnce(m_coral::release)) .onFalse(Commands.runOnce(m_coral::idle)); @@ -144,80 +129,4 @@ public Command getAutonomousCommand() { // return autoChooser.getSelected(); return null; } - - public boolean isXboxController() { - return this.controllerState != ControllerState.JOYSTICKS; - } - - public double getControllerX() { - if (this.isXboxController()) { - return m_controller.getLeftX(); - } else { - return m_leftJoystick.getX(); - } - } - - public double getControllerY() { - if (this.isXboxController()) { - return m_controller.getLeftY(); - } else { - return m_leftJoystick.getY(); - } - } - - public double getContollerRotation() { - if (this.isXboxController()) { - return -m_controller.getRightX(); - } else { - return m_rightJoystick.getX(); - } - } - - public boolean getControllerSlow() { - if (this.isXboxController()) { - return m_controller.rightBumper().getAsBoolean(); - } else { - return m_rightJoystick.getButtonTwo().getAsBoolean(); - } - } - - public double getControllerThrottle() { - if (this.isXboxController()) { - return 1.0; - } else { - return m_leftJoystick.getThrottle(); - } - } - - private boolean isAnyJoystickConnected() { - return this.m_leftJoystick.getJoystick().isConnected() - || this.m_rightJoystick.getJoystick().isConnected(); - } - - private void setControllerState(ControllerState newState) { - this.controllerState = newState; - System.out.println("Controller state: " + this.controllerState); - SmartDashboard.putString("Controller State", this.controllerState.toString()); - } - - public void updateControllerConnections() { - if (this.controllerState == ControllerState.XBOX && !this.m_controller.isConnected()) { - this.setControllerState(ControllerState.DISCONNECTED); - } else if (this.controllerState == ControllerState.JOYSTICKS - && !this.isAnyJoystickConnected()) { - this.setControllerState(ControllerState.DISCONNECTED); - } - - if (this.controllerState == ControllerState.DISCONNECTED) { - if (this.m_controller.isConnected()) this.setControllerState(ControllerState.XBOX); - else if (this.isAnyJoystickConnected()) this.setControllerState(ControllerState.JOYSTICKS); - } - } - - private enum ControllerState { - UNKNOWN, - DISCONNECTED, - XBOX, - JOYSTICKS, - } } From 8ff6077ebdf25a7be92f4be4fd750058a2c89ce3 Mon Sep 17 00:00:00 2001 From: tk2217 Date: Wed, 19 Mar 2025 12:47:48 -0500 Subject: [PATCH 07/12] more stuff --- src/main/java/frc/robot/Controllers.java | 52 ++++++++++++++++--- src/main/java/frc/robot/RobotContainer.java | 16 ++---- .../java/frc/robot/commands/drive/Drive.java | 10 ++-- 3 files changed, 56 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc/robot/Controllers.java b/src/main/java/frc/robot/Controllers.java index 763d6092..4fe8d4c9 100644 --- a/src/main/java/frc/robot/Controllers.java +++ b/src/main/java/frc/robot/Controllers.java @@ -1,11 +1,21 @@ package frc.robot; +import edu.wpi.first.wpilibj.Alert; +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); @@ -16,16 +26,28 @@ public class Controllers { public final CommandXboxController controller = new CommandXboxController(Constants.IOConstants.kControllerPort); - private ControllerState controllerState = Robot.isSimulation() ? ControllerState.XBOX : ControllerState.JOYSTICKS; + 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 isCurrentConnected() { - return switch (this.controllerState) { + private boolean isConnected(ControllerState state) { + return switch (state) { case XBOX -> this.isXboxConnected(); case JOYSTICKS -> this.isAnyJoystickConnected(); }; @@ -88,17 +110,35 @@ 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); + this.realXboxAlert.set(Robot.isReal() && this.controllerState == ControllerState.XBOX); } public void updateControllerConnections() { - if (!this.isCurrentConnected()) { - if (this.controller.isConnected()) this.switchController(ControllerState.XBOX); - else if (this.isAnyJoystickConnected()) this.switchController(ControllerState.JOYSTICKS); + final var time = Timer.getTimestamp(); + if (this.realXboxAlert.get() && this.lastControllerConsoleWarning < time - 15.) { + this.lastControllerConsoleWarning = time; + System.err.println("Oops! Looks like you have the Xbox controller selected for drive on a physical robot."); + } + + 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); + } + } } } public enum ControllerState { XBOX, JOYSTICKS, + ; + + public static final List VALUES = List.of(values()); } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b7b6a3a9..4f827923 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -66,7 +66,7 @@ public RobotContainer() { controllers::getControllerY, controllers::getControllerRotation, controllers::getControllerSlow, - Optional.of(controllers::getControllerThrottle))); + controllers::getControllerThrottle)); m_autoChooser.addRoutine("Test Routine", m_routines::test); m_autoChooser.addRoutine("Blue Processor Routine", m_routines::blueProcessor); @@ -83,16 +83,6 @@ public RobotContainer() { * {@link JoystickButton}. */ private void configureButtonBindings() { - this.controllers.controller - .start() - .onTrue(Commands.runOnce(() -> this.controllers.switchController(Controllers.ControllerState.XBOX))); - this.controllers.leftJoystick - .getButtonNine() - .onTrue(Commands.runOnce(() -> this.controllers.switchController(Controllers.ControllerState.JOYSTICKS))); - this.controllers.rightJoystick - .getButtonNine() - .onTrue(Commands.runOnce(() -> this.controllers.switchController(Controllers.ControllerState.JOYSTICKS))); - // Zero gyro with A button this.controllers.controller.a().onTrue(Commands.runOnce(m_drive::zeroGyro)); @@ -119,6 +109,10 @@ private void configureButtonBindings() { .onFalse(Commands.runOnce(m_coral::idle)); } + public void updateControllerConnections() { + this.controllers.updateControllerConnections(); + } + /** * Use this to pass the autonomous command to the main {@link Robot} class. * diff --git a/src/main/java/frc/robot/commands/drive/Drive.java b/src/main/java/frc/robot/commands/drive/Drive.java index bccf2193..c15c217c 100644 --- a/src/main/java/frc/robot/commands/drive/Drive.java +++ b/src/main/java/frc/robot/commands/drive/Drive.java @@ -15,7 +15,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 +24,7 @@ public Drive( DoubleSupplier y, DoubleSupplier rotation, BooleanSupplier slow, - Optional throttle) { + DoubleSupplier throttle) { m_drive = drive; m_x = x; m_y = y; @@ -45,20 +45,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); From 7ad9e110a88dd3374c5f5b114090cbbbcfb289d2 Mon Sep 17 00:00:00 2001 From: tk2217 Date: Wed, 19 Mar 2025 12:52:38 -0500 Subject: [PATCH 08/12] logic fix --- src/main/java/frc/robot/Controllers.java | 6 +++++- src/main/java/frc/robot/RobotContainer.java | 1 - 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Controllers.java b/src/main/java/frc/robot/Controllers.java index 4fe8d4c9..a8e3bdda 100644 --- a/src/main/java/frc/robot/Controllers.java +++ b/src/main/java/frc/robot/Controllers.java @@ -112,6 +112,7 @@ public void switchController(ControllerState newState) { 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); } @@ -128,7 +129,10 @@ public void updateControllerConnections() { this.switchController(DEFAULT_CONTROLLER_STATE); } else { for (final var controller : ControllerState.VALUES) { - if (this.isConnected(controller)) this.switchController(controller); + if (this.isConnected(controller)) { + this.switchController(controller); + break; + } } } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4f827923..bbc8efa1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -23,7 +23,6 @@ import frc.robot.subsystems.swervedrive.SwerveSubsystem; import frc.robot.utils.FilteredButton; 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 From b608c37b15bc155fbd74ecd2d1c93050d8e7f575 Mon Sep 17 00:00:00 2001 From: tk2217 Date: Wed, 19 Mar 2025 12:52:55 -0500 Subject: [PATCH 09/12] spotless --- src/main/java/frc/robot/Controllers.java | 244 +++++++++--------- src/main/java/frc/robot/RobotContainer.java | 15 +- .../java/frc/robot/commands/drive/Drive.java | 1 - 3 files changed, 134 insertions(+), 126 deletions(-) diff --git a/src/main/java/frc/robot/Controllers.java b/src/main/java/frc/robot/Controllers.java index a8e3bdda..a6fe5f07 100644 --- a/src/main/java/frc/robot/Controllers.java +++ b/src/main/java/frc/robot/Controllers.java @@ -6,143 +6,147 @@ 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 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(); } + } - private boolean isConnected(ControllerState state) { - return switch (state) { - case XBOX -> this.isXboxConnected(); - case JOYSTICKS -> this.isAnyJoystickConnected(); - }; + public double getControllerY() { + if (this.useXboxController()) { + return controller.getLeftY(); + } else { + return leftJoystick.getY(); } + } - private boolean isXboxConnected() { - return this.controller.isConnected(); + public double getControllerRotation() { + if (this.useXboxController()) { + return -controller.getRightX(); + } else { + return rightJoystick.getX(); } + } - private boolean isAnyJoystickConnected() { - return this.leftJoystick.getJoystick().isConnected() - || this.rightJoystick.getJoystick().isConnected(); + public boolean getControllerSlow() { + if (this.useXboxController()) { + return controller.rightBumper().getAsBoolean(); + } else { + return rightJoystick.getButtonTwo().getAsBoolean(); } + } - public boolean useXboxController() { - return this.controllerState != ControllerState.JOYSTICKS; + public double getControllerThrottle() { + if (this.useXboxController()) { + return 1.0; + } else { + return leftJoystick.getThrottle(); } - - public double getControllerX() { - if (this.useXboxController()) { - return controller.getLeftX(); - } else { - return leftJoystick.getX(); - } + } + + 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; + System.err.println( + "Oops! Looks like you have the Xbox controller selected for drive on a physical robot."); } - public double getControllerY() { - if (this.useXboxController()) { - return controller.getLeftY(); - } else { - return leftJoystick.getY(); + 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 double getControllerRotation() { - if (this.useXboxController()) { - return -controller.getRightX(); - } else { - return rightJoystick.getX(); - } - } + public enum ControllerState { + XBOX, + JOYSTICKS, + ; - 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; - System.err.println("Oops! Looks like you have the Xbox controller selected for drive on a physical robot."); - } - - 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()); - } + public static final List VALUES = List.of(values()); + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index bbc8efa1..df9d39b8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -86,23 +86,28 @@ private void configureButtonBindings() { this.controllers.controller.a().onTrue(Commands.runOnce(m_drive::zeroGyro)); if (Robot.isSimulation()) { - this.controllers.controller + this.controllers + .controller .b() .onTrue(Commands.runOnce(() -> ((CoralHandlerSubsystemSim) m_coral).getSimCoral())); } - this.controllers.controller + this.controllers + .controller .x() .onTrue(Commands.runOnce(() -> m_elevator.setState(ElevatorSubsystem.ElevatorState.L2))); - this.controllers.controller + this.controllers + .controller .y() .onTrue(Commands.runOnce(() -> m_elevator.setState(ElevatorSubsystem.ElevatorState.DOWN))); - this.controllers.controller + this.controllers + .controller .rightBumper() .onTrue(Commands.runOnce(m_coral::grab)) .onFalse(Commands.runOnce(m_coral::idle)); - this.controllers.controller + this.controllers + .controller .leftBumper() .onTrue(Commands.runOnce(m_coral::release)) .onFalse(Commands.runOnce(m_coral::idle)); diff --git a/src/main/java/frc/robot/commands/drive/Drive.java b/src/main/java/frc/robot/commands/drive/Drive.java index c15c217c..6535691e 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; From 50c94366bc73ea4e65f557c5056725a8bbdd85d6 Mon Sep 17 00:00:00 2001 From: samfreund Date: Wed, 19 Mar 2025 14:10:52 -0500 Subject: [PATCH 10/12] istg owen run the linting --- src/main/java/frc/robot/RobotContainer.java | 18 ++++++------------ 1 file changed, 6 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 11988d1b..ab4606b7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -85,22 +85,16 @@ private void configureButtonBindings() { this.controllers.controller.a().onTrue(Commands.runOnce(m_drive::zeroGyro)); if (Robot.isSimulation()) { - this.controllers - .controller - .b() - .onTrue(Commands.runOnce(() -> ((CoralHandlerSubsystemSim) m_coral).getSimCoral())); + this.controllers + .controller + .b() + .onTrue(Commands.runOnce(() -> ((CoralHandlerSubsystemSim) m_coral).getSimCoral())); } 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)); + this.controllers.controller.rightBumper().onTrue(new ReleaseCoralCommand(m_coral)); + this.controllers.controller.leftBumper().onTrue(new GrabCoralCommand(m_coral)); } public void updateControllerConnections() { From 4521584ae39467d5481f23da6447884cc70cc3af Mon Sep 17 00:00:00 2001 From: tk2217 Date: Wed, 19 Mar 2025 14:14:36 -0500 Subject: [PATCH 11/12] warning method --- src/main/java/frc/robot/Controllers.java | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Controllers.java b/src/main/java/frc/robot/Controllers.java index a6fe5f07..9980e5d8 100644 --- a/src/main/java/frc/robot/Controllers.java +++ b/src/main/java/frc/robot/Controllers.java @@ -1,6 +1,7 @@ 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; @@ -123,8 +124,8 @@ public void updateControllerConnections() { final var time = Timer.getTimestamp(); if (this.realXboxAlert.get() && this.lastControllerConsoleWarning < time - 15.) { this.lastControllerConsoleWarning = time; - System.err.println( - "Oops! Looks like you have the Xbox controller selected for drive on a physical robot."); + DriverStation.reportWarning( + "Oops! Looks like you have the Xbox controller selected for drive on a physical robot.", false); } if (!this.isConnected(this.controllerState)) { From 63faa92771a9dc0fae676aea8f854daf4a59a9b5 Mon Sep 17 00:00:00 2001 From: tk2217 Date: Wed, 19 Mar 2025 14:19:28 -0500 Subject: [PATCH 12/12] spotless --- src/main/java/frc/robot/Controllers.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Controllers.java b/src/main/java/frc/robot/Controllers.java index 9980e5d8..ad8583e0 100644 --- a/src/main/java/frc/robot/Controllers.java +++ b/src/main/java/frc/robot/Controllers.java @@ -125,7 +125,8 @@ public void updateControllerConnections() { 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); + "Oops! Looks like you have the Xbox controller selected for drive on a physical robot.", + false); } if (!this.isConnected(this.controllerState)) {