somewhat working just spindexer already maxing out

diff --git a/src/main/java/frc/robot/controls/BaseDriverConfig.java b/src/main/java/frc/robot/controls/BaseDriverConfig.java
index b9bd05292c123b49cc2726940629ef434a70a33a..c0a3fc17c779c197d6eb32b5647211ab82069c4e 100644 (file)
--- 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 1f810785a8b5a7c6c37c73779225fc3ce54c275a..ced4ad053d9a3183b4bd8688c3bc6e2ca36f0897 100644 (file)
--- 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 cd9958864344aa761d0e794a7f926082cdb6560d..e9cb913d10c9ece92e084a7c97040ffd93f90231 100644 (file)
--- 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;