From e90c640077fa6dd4a3e209ee08d909a7abef65c1 Mon Sep 17 00:00:00 2001
From: iefomit <timofei.stem@gmail.com>
Date: Thu, 9 Apr 2026 13:34:14 -0700
Subject: [PATCH] somewhat working just spindexer already maxing out

---
 .../frc/robot/controls/BaseDriverConfig.java  |  2 +-
 .../controls/PS5ControllerDriverConfig.java   | 22 +++++++++++++++++++
 .../robot/subsystems/spindexer/Spindexer.java |  2 +-
 3 files changed, 24 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/controls/BaseDriverConfig.java b/src/main/java/frc/robot/controls/BaseDriverConfig.java
index b9bd052..c0a3fc1 100644
--- a/src/main/java/frc/robot/controls/BaseDriverConfig.java
+++ b/src/main/java/frc/robot/controls/BaseDriverConfig.java
@@ -13,7 +13,7 @@ import frc.robot.util.MathUtils;
  */
 public abstract class BaseDriverConfig {
 
-    private final Drivetrain drive;
+    public final Drivetrain drive;
 
     private double previousHeading = 0;
 
diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
index 1f81078..ced4ad0 100644
--- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
+++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
@@ -13,12 +13,14 @@ 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.Climb.LinearClimb;
 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;
@@ -148,6 +150,26 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
                 hood.forceHoodDown(false);
             }));
         }
+
+        // Hood
+        if (spindexer != null) {
+            // Set the hood down -- for safety measures under trench
+            controller.get(PS5Button.RB).onTrue(new InstantCommand(() -> shootFocus(true)))
+            .onFalse(new InstantCommand(()->  shootFocus(false)));
+        }
+    }
+
+    private void shootFocus(boolean turnOn) {
+        if (turnOn) {
+            System.out.println("Shooting is now Focused");
+            spindexer.setNewCurrentLimit(SpindexerConstants.currentLimit * 2.5);
+            drive.applyNewModuleCurrents(DriveConstants.STEER_PEAK_CURRENT_LIMIT, DriveConstants.DRIVE_PEAK_CURRENT_LIMIT * 0.25
+            );
+        } else {
+            System.out.println("Shooting back to normal (From focus)");
+            spindexer.setNewCurrentLimit(SpindexerConstants.currentLimit);
+            drive.applyNewModuleCurrents(DriveConstants.STEER_PEAK_CURRENT_LIMIT, DriveConstants.DRIVE_PEAK_CURRENT_LIMIT);
+        }
     }
 
     @Override
diff --git a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
index cd99588..e9cb913 100644
--- a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
+++ b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
@@ -133,7 +133,7 @@ public class Spindexer extends SubsystemBase implements SpindexerIO {
 
     public void setNewCurrentLimit(double newCurrentLimit) {
         CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs();
-        limitConfig.StatorCurrentLimit = SpindexerConstants.CURRENT_SPIKE_LIMIT;
+        limitConfig.StatorCurrentLimit = newCurrentLimit;
         limitConfig.StatorCurrentLimitEnable = true;
         limitConfig.SupplyCurrentLowerLimit = newCurrentLimit;
         limitConfig.SupplyCurrentLowerTime = 1.5;
-- 
2.39.5