From 76dc6b12bf409596841a0b13b48fe9d95b6ce6cb Mon Sep 17 00:00:00 2001
From: Matt9150 <matthewrkruger@gmail.com>
Date: Sat, 28 Feb 2026 02:49:50 -0600
Subject: [PATCH] Open up SpinEqual position test.  Eliminate pause after
 moving spindexer before going back to intake mode.

---
 .../firstinspires/ftc/teamcode/utils/Servos.java |  2 +-
 .../ftc/teamcode/utils/Spindexer.java            | 16 ++++++++--------
 2 files changed, 9 insertions(+), 9 deletions(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java
index 8f6d885..232ed35 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java
@@ -79,6 +79,6 @@ public class Servos {
     }
 
     public boolean spinEqual(double pos) {
-        return Math.abs(pos - this.getSpinPos()) < 0.03;
+        return Math.abs(pos - this.getSpinPos()) < 0.05;
     }
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java
index 7b11bb8..75bb5f1 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java
@@ -48,7 +48,7 @@ public class Spindexer {
     public double distanceFrontDriver = 0.0;
     public double distanceFrontPassenger = 0.0;
 
-    public double spindexerWiggle = 0.01;
+    public double spindexerWiggle = 0.03;
 
     public double spindexerOuttakeWiggle = 0.01;
     private double prevPos = 0.0;
@@ -205,7 +205,7 @@ public class Spindexer {
             // Mark Ball Found
             newPos1Detection = true;
 
-            if (detectRearColor) {
+            if (detectRearColor && !teleop) {
                 // Detect which color
                 NormalizedRGBA color1RGBA = robot.color1.getNormalizedColors();
 
@@ -225,7 +225,7 @@ public class Spindexer {
         if (distanceFrontDriver < frontDriverDistance) {
             // reset FoundEmpty because looking for 3 in a row before reset
             ballPositions[spindexerBallPos].foundEmpty = 0;
-            if (detectFrontColor) {
+            if (detectFrontColor && !teleop) {
                 NormalizedRGBA color2RGBA = robot.color2.getNormalizedColors();
 
                 double gP = color2RGBA.green / (color2RGBA.green + color2RGBA.red + color2RGBA.blue);
@@ -252,7 +252,7 @@ public class Spindexer {
 
             // reset FoundEmpty because looking for 3 in a row before reset
             ballPositions[spindexerBallPos].foundEmpty = 0;
-            if (detectFrontColor) {
+            if (detectFrontColor && !teleop) {
                 NormalizedRGBA color3RGBA = robot.color3.getNormalizedColors();
 
                 double gP = color3RGBA.green / (color3RGBA.green + color3RGBA.red + color3RGBA.blue);
@@ -446,13 +446,13 @@ public class Spindexer {
             case MOVING:
                 // Stopping when we get to the new position
                 if (servos.spinEqual(intakePositions[commandedIntakePosition])) {
-                    if (intakeTicker > 1){
+                    //if (intakeTicker > 1){
                         currentIntakeState = Spindexer.IntakeState.INTAKE;
                         stopSpindexer();
                         intakeTicker = 0;
-                    } else {
-                        intakeTicker++;
-                    }
+                    //} else {
+                    //    intakeTicker++;
+                    //}
                     //detectBalls(false, false);
                 } else {
                     // Keep moving the spindexer