From 70593bbe4fe352494fcb5c5a17474ebb8fb2e460 Mon Sep 17 00:00:00 2001 From: Wesley28w Date: Mon, 20 Apr 2026 06:49:38 -0700 Subject: [PATCH] =?utf8?q?=F0=9F=9A=97=20F1=20Mode?= MIME-Version: 1.0 Content-Type: text/plain; charset=utf8 Content-Transfer-Encoding: 8bit --- .../java/frc/robot/commands/gpm/F1Mode.java | 57 +++++++++++++++++++ .../controls/PS5ControllerDriverConfig.java | 32 ++++++----- 2 files changed, 75 insertions(+), 14 deletions(-) create mode 100644 src/main/java/frc/robot/commands/gpm/F1Mode.java diff --git a/src/main/java/frc/robot/commands/gpm/F1Mode.java b/src/main/java/frc/robot/commands/gpm/F1Mode.java new file mode 100644 index 0000000..8f611d1 --- /dev/null +++ b/src/main/java/frc/robot/commands/gpm/F1Mode.java @@ -0,0 +1,57 @@ +package frc.robot.commands.gpm; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constants.swerve.DriveConstants; +import frc.robot.subsystems.Intake.Intake; +import frc.robot.subsystems.drivetrain.Drivetrain; +import frc.robot.subsystems.hood.Hood; +import frc.robot.subsystems.shooter.Shooter; +import frc.robot.subsystems.shooter.ShooterConstants; +import frc.robot.subsystems.spindexer.Spindexer; +import frc.robot.subsystems.turret.Turret; +import frc.robot.subsystems.turret.TurretConstants; + +public class F1Mode extends Command { + private Intake intake; + private Drivetrain drive; + private Hood hood; + private Shooter shooter; + private Turret turret; + + public F1Mode(Turret turret, Shooter shooter, Hood hood, Intake intake, Drivetrain drive) { + this.intake = intake; + this.drive = drive; + this.hood = hood; + this.shooter = shooter; + this.turret = turret; + + addRequirements(intake, drive, hood, shooter, turret); + } + + @Override + public void initialize() { + drive.applyNewModuleCurrents( + DriveConstants.STEER_CONTINUOUS_CURRENT_LIMIT * 1.75, + DriveConstants.STEER_CONTINUOUS_CURRENT_LIMIT * 1.75, + DriveConstants.DRIVE_CONTINUOUS_CURRENT_LIMIT * 1.75, + DriveConstants.DRIVE_CONTINUOUS_CURRENT_LIMIT * 1.75 + ); + intake.retract(); // pull intake in + hood.forceHoodDown(true); // force the hood down (spindexer will stop) + shooter.setNewCurrentLimit(0, 0); + turret.setNewCurrentLimit(0, 0); + } + + @Override + public void end(boolean interrupted) { + drive.applyNewModuleCurrents( + DriveConstants.STEER_CONTINUOUS_CURRENT_LIMIT * 1.75, + DriveConstants.STEER_CONTINUOUS_CURRENT_LIMIT * 1.75, + DriveConstants.DRIVE_CONTINUOUS_CURRENT_LIMIT * 1.75, + DriveConstants.DRIVE_CONTINUOUS_CURRENT_LIMIT * 1.75 + ); + hood.forceHoodDown(false); + shooter.setNewCurrentLimit(ShooterConstants.SHOOTER_CURRENT_LIMIT, ShooterConstants.SHOOTER_CURRENT_LIMIT); + turret.setNewCurrentLimit(TurretConstants.STATOR_CURRENT_LIMIT, TurretConstants.SUPPLY_CURRENT_LIMIT); + } +} diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java index cd5208d..ce4e04d 100644 --- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java +++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java @@ -8,24 +8,22 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.InstantCommand; import frc.robot.Robot; +import frc.robot.commands.gpm.F1Mode; import frc.robot.commands.gpm.IntakeMovementCommand; import frc.robot.commands.gpm.ReverseMotors; import frc.robot.commands.gpm.RunSpindexer; import frc.robot.commands.gpm.Superstructure; import frc.robot.constants.Constants; -import frc.robot.constants.swerve.DriveConstants; import frc.robot.subsystems.Intake.Intake; import frc.robot.subsystems.drivetrain.Drivetrain; import frc.robot.subsystems.hood.Hood; import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.spindexer.Spindexer; -import frc.robot.subsystems.spindexer.SpindexerConstants; import frc.robot.subsystems.turret.Turret; import lib.controllers.PS5Controller; import lib.controllers.PS5Controller.DPad; import lib.controllers.PS5Controller.PS5Axis; import lib.controllers.PS5Controller.PS5Button; -import org.littletonrobotics.junction.Logger; /** * Driver controls for the PS5 controller @@ -40,7 +38,6 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig { private Hood hood; private Intake intake; private Spindexer spindexer; - private double originalSpindexerCurrentLimit; public PS5ControllerDriverConfig( Drivetrain drive, @@ -63,7 +60,7 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig { new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI)))); // Cancel commands - controller.get(PS5Button.RB).onTrue(new InstantCommand(() -> { + controller.get(PS5Button.PS).onTrue(new InstantCommand(() -> { getDrivetrain().setIsAlign(false); getDrivetrain().setDesiredPose(() -> null); CommandScheduler.getInstance().cancelAll(); @@ -101,15 +98,15 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig { .alongWith(new InstantCommand(()-> intakeBoolean = true))); // Calibration: you can now calibrate easily using this button - if (hood != null && intake != null) { - controller.get(PS5Button.PS).onTrue(new InstantCommand(() -> { - intake.calibrate(); - hood.calibrate(); - }, intake, hood)).onFalse(new InstantCommand(() -> { - intake.stopCalibrating(); - hood.stopCalibrating(); - }, intake, hood)); - } + // if (hood != null && intake != null) { + // controller.get(PS5Button.PS).onTrue(new InstantCommand(() -> { + // intake.calibrate(); + // hood.calibrate(); + // }, intake, hood)).onFalse(new InstantCommand(() -> { + // intake.stopCalibrating(); + // hood.stopCalibrating(); + // }, intake, hood)); + // } // Stop intake roller controller.get(DPad.DOWN).onTrue(new InstantCommand(()->{ @@ -148,6 +145,13 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig { hood.forceHoodDown(false); })); } + + if (turret != null && hood != null && shooter != null && spindexer != null && drive != null) { + // should be hold + controller.get(PS5Button.RB).onTrue(new InstantCommand(() -> { + new F1Mode(turret, shooter, hood, intake, drive); + }, turret, shooter, hood, intake, drive)); + } } @Override -- 2.39.5