toggle spindexer

diff --git a/src/main/java/frc/robot/commands/gpm/RunSpindexer.java b/src/main/java/frc/robot/commands/gpm/RunSpindexer.java
index 45bc8e6c220b9f8c4a913fb274f56d1e76fcd599..17a615fea8bd2c00860e9d68746b8d578451a2dd 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/RunSpindexer.java
+++ b/src/main/java/frc/robot/commands/gpm/RunSpindexer.java
@@ -6,19 +6,29 @@ import frc.robot.subsystems.turret.Turret;
 
 public class RunSpindexer extends Command {
     private Spindexer spindexer;
-    public RunSpindexer(Spindexer spindexer){
+    private Turret turret;
+    public RunSpindexer(Spindexer spindexer, Turret turret){
         this.spindexer = spindexer;
-        
-        addRequirements(spindexer);
+        this.turret = turret;
+        addRequirements(spindexer, turret);
     }
 
     @Override
     public void execute() {
-        spindexer.maxSpindexer();
+        if (turret.atSetpoint()) {
+            spindexer.maxSpindexer();
+        } else {
+            spindexer.stopSpindexer();
+        }
     }
 
     @Override
     public void end(boolean interrupted) {
         spindexer.stopSpindexer();
     }
+
+    @Override
+    public boolean isFinished() {
+        return false;  // never ends on its own
+    }
 }

diff --git a/src/main/java/frc/robot/constants/FieldConstants.java b/src/main/java/frc/robot/constants/FieldConstants.java
index 424667a9c7226a891a10da42dc6c31030a2c0a76..2f33c897a40faae8d8f50c19a8ebe0f3ab74ee55 100644 (file)
--- a/src/main/java/frc/robot/constants/FieldConstants.java
+++ b/src/main/java/frc/robot/constants/FieldConstants.java
@@ -185,14 +185,6 @@ public class FieldConstants {
     }
   }
 
-  public static FieldZone getWorkingZone(Translation2d drivepose) {
-    double x = drivepose.getX();
-    double y = drivepose.getY();
-    Alliance alliance = Robot.getAlliance();
-
-
-  }
-
   /**
    * 
    * @return Whether Y coordinate is in the upper half (left side on blue alliance)

diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
index cd4a59308d363c042acbc716ef2eb399b402a806..5c6626612147fe1c79bc5e61eeebd0507b77ada7 100644 (file)
--- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
+++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
@@ -140,15 +140,9 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
         // Spindexer
         if (spindexer != null) {
             // Toggle spindexer
-            controller.get(PS5Button.LEFT_TRIGGER).onTrue(new InstantCommand(() -> {
-                if (spindexerBoolean) {
-                    spindexer.maxSpindexer();
-                    intakeBoolean = false;
-                } else {
-                    spindexer.stopSpindexer();
-                    intakeBoolean = true;
-                }
-            }));
+            controller.get(PS5Button.LEFT_TRIGGER).toggleOnTrue(
+                new RunSpindexer(spindexer, turret)
+            );
         }
 
         // Auto shoot