diff --git a/src/main/java/org/curtinfrc/frc2025/Constants.java b/src/main/java/org/curtinfrc/frc2025/Constants.java index af9c4579..36862caf 100644 --- a/src/main/java/org/curtinfrc/frc2025/Constants.java +++ b/src/main/java/org/curtinfrc/frc2025/Constants.java @@ -15,7 +15,7 @@ * (log replay from a file). */ public final class Constants { - public static final RobotType robotType = RobotType.COMPBOT; + public static final RobotType robotType = RobotType.SIMBOT; public static final double ROBOT_X = 0.705; public static final double ROBOT_Y = 0.730; public static final double FIELD_LENGTH = aprilTagLayout.getFieldLength(); diff --git a/src/main/java/org/curtinfrc/frc2025/Robot.java b/src/main/java/org/curtinfrc/frc2025/Robot.java index 33243d71..d85ed17d 100644 --- a/src/main/java/org/curtinfrc/frc2025/Robot.java +++ b/src/main/java/org/curtinfrc/frc2025/Robot.java @@ -20,7 +20,6 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.ScheduleCommand; import edu.wpi.first.wpilibj2.command.Subsystem; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @@ -69,6 +68,7 @@ import org.curtinfrc.frc2025.subsystems.vision.VisionIOPhotonVisionSim; import org.curtinfrc.frc2025.util.AutoChooser; import org.curtinfrc.frc2025.util.ButtonBoard; +import org.curtinfrc.frc2025.util.ContextfulXboxController; import org.curtinfrc.frc2025.util.PhoenixUtil; import org.curtinfrc.frc2025.util.VirtualSubsystem; import org.littletonrobotics.junction.AutoLogOutput; @@ -96,8 +96,17 @@ public class Robot extends LoggedRobot { private Ejector ejector; private Climber climber; + @AutoLogOutput(key = "Robot/Overridden") + private boolean overridden = false; + + @AutoLogOutput(key = "Robot/Overide") + private final Trigger override = new Trigger(() -> overridden); + // Controller - private final CommandXboxController controller = new CommandXboxController(0); + private final ContextfulXboxController controller = + new ContextfulXboxController(0, override.negate()); + private final ContextfulXboxController manualController = + new ContextfulXboxController(0, override); private final Alert controllerDisconnected = new Alert("Driver controller disconnected!", AlertType.kError); private final ButtonBoard board = new ButtonBoard(1); @@ -112,12 +121,6 @@ public class Robot extends LoggedRobot { private List l1Setpoints; private List algaeSetpoints; - @AutoLogOutput(key = "Robot/Overridden") - private boolean overridden = false; - - @AutoLogOutput(key = "Robot/Overide") - private final Trigger override = new Trigger(() -> overridden); - public Robot() { // Record metadata Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME); @@ -344,7 +347,6 @@ public Robot() { controller .rightBumper() .or(controller.leftBumper()) - .and(override.negate()) .whileTrue( elevator .goToSetpoint(ElevatorSetpoints.L2, intake.backSensor.negate()) @@ -352,7 +354,6 @@ public Robot() { controller .rightTrigger() .or(controller.leftTrigger()) - .and(override.negate()) .whileTrue( elevator .goToSetpoint(ElevatorSetpoints.L3, intake.backSensor.negate()) @@ -361,7 +362,6 @@ public Robot() { controller .rightBumper() .or(controller.rightTrigger()) - .and(override.negate()) .whileTrue( drive .autoAlignWithOverride( @@ -371,30 +371,25 @@ public Robot() { () -> controller.getRightX()) .until(ejector.backSensor.negate())); - controller.leftBumper().or(controller.leftTrigger()).and(override).whileTrue(ejector.eject(30)); - controller + manualController.leftBumper().or(controller.leftTrigger()).whileTrue(ejector.eject(30)); + manualController .leftBumper() - .and(override) .whileTrue( elevator.goToSetpoint(ElevatorSetpoints.AlgaePopLow, intake.backSensor.negate())); - controller + manualController .leftTrigger() - .and(override) .whileTrue( elevator.goToSetpoint(ElevatorSetpoints.AlgaePopHigh, intake.backSensor.negate())); - controller + manualController .rightBumper() - .and(override) .whileTrue(elevator.goToSetpoint(ElevatorSetpoints.L2, intake.backSensor.negate())); - controller + manualController .rightTrigger() - .and(override) .whileTrue(elevator.goToSetpoint(ElevatorSetpoints.L3, intake.backSensor.negate())); controller .leftBumper() .or(controller.leftTrigger()) - .and(override.negate()) .whileTrue( drive .autoAlignWithOverride( @@ -446,7 +441,6 @@ public Robot() { .or(controller.rightTrigger()) .or(controller.leftTrigger()) .negate()) - .and(override.negate()) .whileTrue( Commands.parallel( drive.autoAlignWithOverride( @@ -501,7 +495,8 @@ public Robot() { .or(controller.rightTrigger()) .or(controller.leftTrigger())) .whileTrue(ejector.eject(15).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); - controller.povUp().whileTrue(intake.intake(-4)); + + controller.povUpRaw().whileTrue(intake.intake(-4)); intake .backSensor @@ -537,7 +532,7 @@ public Robot() { // Reset gyro to 0° when B button is pressed controller - .y() + .yRaw() .onTrue( Commands.runOnce( () -> @@ -546,7 +541,7 @@ public Robot() { drive) .ignoringDisable(true)); controller - .x() + .xRaw() .onTrue( Commands.sequence( climber.disengage(), @@ -554,7 +549,7 @@ public Robot() { elevator.goToSetpoint(ElevatorSetpoints.climbPrep, intake.backSensor.negate()))); controller // climb attempt - .a() + .aRaw() .and(() -> climber.climberDeployed) .onTrue( elevator @@ -567,7 +562,7 @@ public Robot() { elevator.goToSetpoint( ElevatorSetpoints.climbPrep, intake.backSensor.negate())))); - controller.b().onTrue(Commands.runOnce(() -> overridden = !overridden)); + controller.bRaw().onTrue(Commands.runOnce(() -> overridden = !overridden)); new Trigger(this::isEnabled).onTrue(climber.disengage()); diff --git a/src/main/java/org/curtinfrc/frc2025/util/ContextfulXboxController.java b/src/main/java/org/curtinfrc/frc2025/util/ContextfulXboxController.java new file mode 100644 index 00000000..7dd8a840 --- /dev/null +++ b/src/main/java/org/curtinfrc/frc2025/util/ContextfulXboxController.java @@ -0,0 +1,287 @@ +package org.curtinfrc.frc2025.util; + +import edu.wpi.first.wpilibj.GenericHID.RumbleType; +import edu.wpi.first.wpilibj.event.EventLoop; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import java.util.function.BooleanSupplier; + +/* All the Triggers check for the context as they should + * activate based on the controller state. None of the methods that return a + * (non-trigger) value don't check the context as they return values from the + * physical controller which isn't dependent on the context. + */ +public class ContextfulXboxController { + + private final CommandXboxController xboxController; + private final BooleanSupplier context; + + public ContextfulXboxController(int port, BooleanSupplier context) { + xboxController = new CommandXboxController(port); + this.context = context; + } + + public Trigger a() { + return xboxController.a().and(context); + } + + public Trigger aRaw() { + return xboxController.a(); + } + + public Trigger b() { + return xboxController.b().and(context); + } + + public Trigger bRaw() { + return xboxController.b(); + } + + public Trigger x() { + return xboxController.x().and(context); + } + + public Trigger xRaw() { + return xboxController.x(); + } + + public Trigger y() { + return xboxController.y().and(context); + } + + public Trigger yRaw() { + return xboxController.y(); + } + + public Trigger leftBumper() { + return xboxController.leftBumper().and(context); + } + + public Trigger leftBumperRaw() { + return xboxController.leftBumper(); + } + + public Trigger rightBumper() { + return xboxController.rightBumper().and(context); + } + + public Trigger rightBumperRaw() { + return xboxController.rightBumper(); + } + + public Trigger back() { + return xboxController.back().and(context); + } + + public Trigger backRaw() { + return xboxController.back(); + } + + public Trigger start() { + return xboxController.start().and(context); + } + + public Trigger startRaw() { + return xboxController.start(); + } + + public Trigger leftStick() { + return xboxController.leftStick().and(context); + } + + public Trigger leftStickRaw() { + return xboxController.leftStick(); + } + + public Trigger rightStick() { + return xboxController.rightStick().and(context); + } + + public Trigger rightStickRaw() { + return xboxController.rightStick(); + } + + public Trigger leftTrigger() { + return xboxController.leftTrigger().and(context); + } + + public Trigger leftTriggerRaw() { + return xboxController.leftTrigger(); + } + + public Trigger rightTrigger() { + return xboxController.rightTrigger().and(context); + } + + public Trigger rightTriggerRaw() { + return xboxController.rightTrigger(); + } + + public Trigger povUp() { + return xboxController.povUp().and(context); + } + + public Trigger povUpRaw() { + return xboxController.povUp(); + } + + public Trigger povUpRight() { + return xboxController.povUpRight().and(context); + } + + public Trigger povRightRaw() { + return xboxController.povRight(); + } + + public Trigger povRight() { + return xboxController.povRight().and(context); + } + + public Trigger povDownRight() { + return xboxController.povDownRight().and(context); + } + + public Trigger povDownRightRaw() { + return xboxController.povDownRight(); + } + + public Trigger povDown() { + return xboxController.povDown().and(context); + } + + public Trigger povDownRaw() { + return xboxController.povDown(); + } + + public Trigger povDownLeft() { + return xboxController.povDownLeft().and(context); + } + + public Trigger povDownLeftRaw() { + return xboxController.povDownLeft(); + } + + public Trigger povLeft() { + return xboxController.povLeft().and(context); + } + + public Trigger povLeftRaw() { + return xboxController.povLeft(); + } + + public Trigger povUpLeft() { + return xboxController.povUpLeft().and(context); + } + + public Trigger povUpLeftRaw() { + return xboxController.povUpLeft(); + } + + public Trigger pov(int angle) { + return xboxController.pov(angle).and(context); + } + + public Trigger povRaw(int angle) { + return xboxController.pov(angle); + } + + public Trigger pov(int pov, int angle, EventLoop loop) { + return xboxController.pov(pov, angle, loop).and(context); + } + + public Trigger povRaw(int pov, int angle, EventLoop loop) { + return xboxController.pov(pov, angle, loop); + } + + public Trigger povCenter() { + return xboxController.povCenter().and(context); + } + + public Trigger povCenterRaw() { + return xboxController.povCenter(); + } + + public Trigger axisLessThan(int axis, double threshold) { + return xboxController.axisLessThan(axis, threshold).and(context); + } + + public Trigger axisLessThanRaw(int axis, double threshold) { + return xboxController.axisLessThan(axis, threshold); + } + + public Trigger axisLessThan(int axis, double threshold, EventLoop loop) { + return xboxController.axisLessThan(axis, threshold, loop).and(context); + } + + public Trigger axisLessThanRaw(int axis, double threshold, EventLoop loop) { + return xboxController.axisLessThan(axis, threshold, loop); + } + + public Trigger axisGreaterThan(int axis, double threshold) { + return xboxController.axisGreaterThan(axis, threshold).and(context); + } + + public Trigger axisGreaterThanRaw(int axis, double threshold) { + return xboxController.axisGreaterThan(axis, threshold); + } + + public Trigger axisGreaterThan(int axis, double threshold, EventLoop loop) { + return xboxController.axisGreaterThan(axis, threshold, loop).and(context); + } + + public Trigger axisGreaterThanRaw(int axis, double threshold, EventLoop loop) { + return xboxController.axisGreaterThan(axis, threshold, loop); + } + + public Trigger axisMagnitudeGreaterThan(int axis, double threshold) { + return xboxController.axisMagnitudeGreaterThan(axis, threshold).and(context); + } + + public Trigger axisMagnitudeGreaterThanRaw(int axis, double threshold) { + return xboxController.axisMagnitudeGreaterThan(axis, threshold); + } + + public Trigger axisMagnitudeGreaterThan(int axis, double threshold, EventLoop loop) { + return xboxController.axisMagnitudeGreaterThan(axis, threshold, loop).and(context); + } + + public Trigger axisMagnitudeGreaterThanRaw(int axis, double threshold, EventLoop loop) { + return xboxController.axisMagnitudeGreaterThan(axis, threshold, loop); + } + + public void setRumble(RumbleType type, double value) { + xboxController.setRumble(type, value); + } + + public double getLeftX() { + return xboxController.getLeftX(); + } + + public double getRightX() { + return xboxController.getRightX(); + } + + public double getLeftY() { + return xboxController.getLeftY(); + } + + public double getRightY() { + return xboxController.getRightY(); + } + + public double getLeftTriggerAxis() { + return xboxController.getLeftTriggerAxis(); + } + + public double getRightTriggerAxis() { + return xboxController.getRightTriggerAxis(); + } + + public boolean isConnected() { + return xboxController.isConnected(); + } + + public double getRawAxis(int axis) { + return xboxController.getRawAxis(axis); + } +}