From: iefomit Date: Fri, 6 Mar 2026 01:22:22 +0000 (-0800) Subject: add emulation support X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=917832ead8bfd9f869c81c8f98be3264f940b613;p=FRC2026.git add emulation support --- diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index bbe567d..06f6323 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -25,6 +25,7 @@ import frc.robot.constants.Constants; import frc.robot.controls.BaseDriverConfig; import frc.robot.controls.Operator; import frc.robot.controls.PS5ControllerDriverConfig; +import frc.robot.controls.PS5XboxModeDriverConfig; import frc.robot.subsystems.Climb.LinearClimb; import frc.robot.subsystems.Intake.Intake; import frc.robot.subsystems.drivetrain.Drivetrain; @@ -110,7 +111,7 @@ public class RobotContainer { case Vertigo: // AKA "French Toast" drive = new Drivetrain(vision, new GyroIOPigeon2()); - driver = new PS5ControllerDriverConfig(drive, shooter, turret, hood, intake, spindexer, linearClimb); + driver = new PS5XboxModeDriverConfig(drive, shooter, turret, hood, intake, spindexer, linearClimb); operator = new Operator(drive); // Detected objects need access to the drivetrain diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java index 818e4c6..e8919a1 100644 --- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java +++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java @@ -181,12 +181,6 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig { hood.stopCalibrating(); })); } - - // rumble test - controller.get(PS5Button.TOUCHPAD).onTrue(new SequentialCommandGroup( - new InstantCommand(() -> controller.rumbleOn()), - new WaitCommand(0.5), - new InstantCommand(() -> controller.rumbleOff()))); } @Override @@ -224,10 +218,12 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig { return false; } + // doesn't work on ps5 controllers, use PS5XboxModeDriverConfig instead public void startRumble() { controller.rumbleOn(); } + // doesn't work on ps5 controllers, use PS5XboxModeDriverConfig instead public void endRumble() { controller.rumbleOff(); } diff --git a/src/main/java/frc/robot/controls/PS5XboxModeDriverConfig.java b/src/main/java/frc/robot/controls/PS5XboxModeDriverConfig.java new file mode 100644 index 0000000..adb696e --- /dev/null +++ b/src/main/java/frc/robot/controls/PS5XboxModeDriverConfig.java @@ -0,0 +1,266 @@ +package frc.robot.controls; + +import java.util.function.BooleanSupplier; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.FunctionalCommand; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.Robot; +import frc.robot.commands.gpm.AutoShootCommand; +import frc.robot.commands.gpm.ClimbDriveCommand; +import frc.robot.commands.gpm.ReverseMotors; +import frc.robot.constants.Constants; +import frc.robot.subsystems.Climb.LinearClimb; +import frc.robot.subsystems.drivetrain.Drivetrain; +import frc.robot.subsystems.hood.Hood; +import frc.robot.subsystems.Intake.Intake; +import frc.robot.subsystems.shooter.Shooter; +import frc.robot.subsystems.spindexer.Spindexer; +import frc.robot.subsystems.turret.Turret; +import lib.controllers.GameController; +import lib.controllers.GameController.Axis; +import lib.controllers.GameController.Button; +import lib.controllers.GameController.DPad; + +/** + * Driver config for PS5 controllers using Xbox 360 emulation mode. + * This lets SCUF and other PS5 controllers work with WPILib rumble. + * + * Setup: + * - download DSX (https://dualsensex.com/download/) + * - install ViGEmBus driver (if app doesn't auto prompt) + * - in dsx, set "controller emulation" to Xbox 360 + * - ensure rumble is enabled in dsx settings + * - once code is depoloyed, change controller to "Xbox 360" controller in driverstation + */ + +public class PS5XboxModeDriverConfig extends BaseDriverConfig { + private final GameController controller = new GameController(Constants.DRIVER_JOY); + private final BooleanSupplier slowModeSupplier = () -> false; + private boolean intakeBoolean = true; + private Command autoShoot = null; + private Command reverseMotors = null; + private Shooter shooter; + private Turret turret; + private Hood hood; + private Intake intake; + private Spindexer spindexer; + private LinearClimb climb; + + // PS5 button aliases + private final Button CROSS = Button.A; + private final Button CIRCLE = Button.B; + private final Button SQUARE = Button.X; + private final Button TRIANGLE = Button.Y; + private final Button LB = Button.LB; + private final Button RB = Button.RB; + private final Button CREATE = Button.BACK; + private final Button OPTIONS = Button.START; + private final Button LEFT_JOY = Button.LEFT_JOY; + private final Button RIGHT_JOY = Button.RIGHT_JOY; + + // PS5 trigger buttons + private final BooleanSupplier LEFT_TRIGGER_BUTTON = controller.LEFT_TRIGGER_BUTTON; + private final BooleanSupplier RIGHT_TRIGGER_BUTTON = controller.RIGHT_TRIGGER_BUTTON; + + // PS5 axis aliases + private final Axis LEFT_X = Axis.LEFT_X; + private final Axis LEFT_Y = Axis.LEFT_Y; + private final Axis RIGHT_X = Axis.RIGHT_X; + private final Axis RIGHT_Y = Axis.RIGHT_Y; + private final Axis LEFT_TRIGGER = Axis.LEFT_TRIGGER; + private final Axis RIGHT_TRIGGER = Axis.RIGHT_TRIGGER; + + public PS5XboxModeDriverConfig( + Drivetrain drive, + Shooter shooter, + Turret turret, + Hood hood, + Intake intake, + Spindexer spindexer, + LinearClimb climb) { + super(drive); + this.shooter = shooter; + this.turret = turret; + this.hood = hood; + this.intake = intake; + this.spindexer = spindexer; + this.climb = climb; + } + + public void configureControls() { + // Reset the yaw. Mainly useful for testing/driver practice + controller.get(CREATE).onTrue(new InstantCommand(() -> getDrivetrain().setYaw( + new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI)))); + + // Cancel commands + controller.get(RB).onTrue(new InstantCommand(() -> { + getDrivetrain().setIsAlign(false); + getDrivetrain().setDesiredPose(() -> null); + CommandScheduler.getInstance().cancelAll(); + })); + + // Align wheels + controller.get(DPad.RIGHT).onTrue(new FunctionalCommand( + () -> getDrivetrain().setStateDeadband(false), + getDrivetrain()::alignWheels, + interrupted -> getDrivetrain().setStateDeadband(true), + () -> false, getDrivetrain()).withTimeout(2)); + + // Trench align + controller.get(DPad.LEFT).onTrue(new InstantCommand(() -> { + getDrivetrain().setTrenchAssist(true); + getDrivetrain().setTrenchAlign(true); + })) + .onFalse(new InstantCommand(() -> { + getDrivetrain().setTrenchAssist(false); + getDrivetrain().setTrenchAlign(false); + })); + + // Reverse motors + if (intake != null && spindexer != null && shooter != null) { + controller.get(CIRCLE).onTrue(new InstantCommand(() -> { + reverseMotors = new ReverseMotors(intake, spindexer); + reverseMotors.schedule(); + })).onFalse(new InstantCommand(() -> { + if (reverseMotors != null) { + reverseMotors.cancel(); + } + })); + } + + // Intake + if (intake != null) { + // Toggle intake + controller.get(RIGHT_TRIGGER_BUTTON).onTrue(new InstantCommand(() -> { + if (intakeBoolean) { + intake.extend(); + intake.spinStart(); + intakeBoolean = false; + } else { + intake.intermediateExtend(); + intake.spinStop(); + intakeBoolean = true; + } + })); + + // Retract if hold for 3 seconds + controller.get(RIGHT_TRIGGER_BUTTON).debounce(3.0).onTrue(new InstantCommand(() -> { + intake.retract(); + intakeBoolean = true; + })); + + // Calibration + controller.get(LEFT_JOY).onTrue(new InstantCommand(() -> { + intake.calibrate(); + })).onFalse(new InstantCommand(() -> { + intake.stopCalibrating(); + })); + } + + // Spindexer + if (spindexer != null) { + // Will only run if we are not calling default shoot command + controller.get(LEFT_TRIGGER_BUTTON).onTrue(new InstantCommand(() -> spindexer.maxSpindexer())) + .onFalse(new InstantCommand(() -> spindexer.stopSpindexer())); + } + + // Auto shoot + if (turret != null && hood != null && shooter != null) { + controller.get(SQUARE).onTrue( + new InstantCommand(() -> { + if (autoShoot != null && autoShoot.isScheduled()) { + autoShoot.cancel(); + } else { + autoShoot = new AutoShootCommand(turret, getDrivetrain(), hood, shooter, spindexer); + CommandScheduler.getInstance().schedule(autoShoot); + } + })); + } + + // Climb + if (climb != null) { + // Calibration + controller.get(OPTIONS).onTrue(new InstantCommand(() -> { + climb.hardstopCalibration(); + })).onFalse(new InstantCommand(() -> { + climb.stopCalibrating(); + })); + + // Climb retract + controller.get(CROSS).onTrue(new InstantCommand(() -> { + climb.retract(); + })); + + // Drive to climb position and rumble + controller.get(TRIANGLE).onTrue(new SequentialCommandGroup( + new ClimbDriveCommand(climb, getDrivetrain()), + new InstantCommand(() -> this.startRumble()), + new WaitCommand(1), + new InstantCommand(() -> this.endRumble()))); + } + + // Hood + if (hood != null) { + controller.get(LEFT_JOY).onTrue(new InstantCommand(() -> { + hood.calibrate(); + })).onFalse(new InstantCommand(() -> { + hood.stopCalibrating(); + })); + } + + // Rumble test + controller.get(RIGHT_JOY).onTrue(new SequentialCommandGroup( + new InstantCommand(() -> controller.setRumble(GameController.RumbleStatus.RUMBLE_ON)), + new WaitCommand(0.5), + new InstantCommand(() -> controller.setRumble(GameController.RumbleStatus.RUMBLE_OFF)))); + } + + @Override + public double getRawSideTranslation() { + return controller.get(LEFT_X); + } + + @Override + public double getRawForwardTranslation() { + return controller.get(LEFT_Y); + } + + @Override + public double getRawRotation() { + return controller.get(RIGHT_X); + } + + @Override + public double getRawHeadingAngle() { + return Math.atan2(controller.get(RIGHT_X), -controller.get(RIGHT_Y)) - Math.PI / 2; + } + + @Override + public double getRawHeadingMagnitude() { + return Math.hypot(controller.get(RIGHT_X), controller.get(RIGHT_Y)); + } + + @Override + public boolean getIsSlowMode() { + return slowModeSupplier.getAsBoolean(); + } + + @Override + public boolean getIsAlign() { + return false; + } + + public void startRumble() { + controller.setRumble(GameController.RumbleStatus.RUMBLE_ON); + } + + public void endRumble() { + controller.setRumble(GameController.RumbleStatus.RUMBLE_OFF); + } +}