🚗 F1 Mode

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 (file)
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 cd5208d37b2932f9ee4e8660d8ac2d884dda2fe1..ce4e04d7c83367501cf8025e9764b5516e912b6d 100644 (file)
--- 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