From e8f80ef39ec15d0073c3268fe79ac1c2e3fe550b Mon Sep 17 00:00:00 2001
From: DanTheMan-byte <danibarrf@gmail.com>
Date: Wed, 10 Jun 2026 10:43:02 -0500
Subject: [PATCH] match 69

---
 .../ftc/teamcode/teleop/TeleopV4.java         | 15 +++++-------
 .../utilsv2/SpindexerTransferIntake.java      | 23 +++++++++++--------
 .../teamcode/utilsv2/VelocityCommander.java   |  2 +-
 3 files changed, 21 insertions(+), 19 deletions(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java
index bb2fe84..e05fc60 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java
@@ -132,7 +132,9 @@ public class TeleopV4 extends LinearOpMode {
                 gamepad1.rumble(100);
             }
 
-            follower.update();
+            if (!isStopRequested()){
+                follower.update();
+            }
             Pose currentPose = follower.getPose();
             teleStart = currentPose;
 
@@ -209,16 +211,11 @@ public class TeleopV4 extends LinearOpMode {
                     shooting = false;
                 }
 
-                if (robot.insideBeam.isPressed() && robot.outsideBeam.isPressed() && !shooting){
-                    intakeFull = true;
-                } else {
-                    intakeFull = false;
-                    firstTickFull = true;
-                }
-
-                if (intakeFull && firstTickFull){
+                if (SpindexerTransferIntake.intakeFull && firstTickFull){
                     gamepad1.rumble(250);
                     firstTickFull = false;
+                } else if (!SpindexerTransferIntake.intakeFull){
+                    firstTickFull = true;
                 }
 
                 if (gamepad1.right_trigger > 0.5 &&
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java
index 42a3bba..a3eae13 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java
@@ -53,7 +53,7 @@ public class SpindexerTransferIntake {
     }
 
     int[] shootOrder = {0, 1, 2};
-    public static final double sensorDistanceThreshold = 6.0;//4.85//5.35
+    public static final double sensorDistanceThreshold = 5.0;//6.0;//4.85//5.35
     final long pulseTime = 70; // ms
 
     private DesiredPattern desiredPattern = DesiredPattern.GPP;
@@ -289,6 +289,7 @@ public class SpindexerTransferIntake {
     private int greenTicks = 0;
     private int ballTicks = 0;
     private int holdBallsTicker = 0;
+    public static boolean intakeFull = false;
     public void update() {
 
 //        TELE.addData("Sorted State", sortedIntakeStates);
@@ -318,7 +319,7 @@ public class SpindexerTransferIntake {
 
                     case INTAKE:
 
-                        robot.setIntakePower(0.8);
+                        robot.setIntakePower(1);
                         robot.setRapidFireBlockerPos(
                                 ServoPositions.rapidFireBlocker_Closed
                         );
@@ -330,11 +331,11 @@ public class SpindexerTransferIntake {
                                 ServoPositions.transferServo_out
                         );
 
-                        if (robot.insideBeam.isPressed()) {
+                        if (robot.insideBeam.isPressed() && robot.outsideBeam.isPressed()) {
                             holdBallsTicker++;
                         }
 
-                        if (holdBallsTicker > 15){
+                        if (holdBallsTicker > 5){
                             setRapidMode(RapidMode.TRANSFER_OFF);
                             holdBallsTicker = 0;
                         }
@@ -346,6 +347,7 @@ public class SpindexerTransferIntake {
                         if (robot.insideBeam.isPressed() && robot.outsideBeam.isPressed()) {
                             setRapidMode(RapidMode.BEFORE_PULSE_OUT);
                         }
+                        robot.setTransferServoPos(ServoPositions.transferServo_in);
                         robot.setTransferPower(-0.3);
 
                         break;
@@ -354,7 +356,7 @@ public class SpindexerTransferIntake {
 
                         robot.setIntakePower(1.0);
 
-                        if (stateTime() >= 700) {
+                        if (stateTime() >= 100) {
                             setRapidMode(RapidMode.PULSE_OUT);
                         }
 
@@ -362,7 +364,7 @@ public class SpindexerTransferIntake {
 
                     case PULSE_OUT:
 
-                        robot.setIntakePower(-0.1);
+//                        robot.setIntakePower(0);
 
                         if (stateTime() >= pulseTime) {
                             setRapidMode(RapidMode.PULSE_IN);
@@ -374,7 +376,7 @@ public class SpindexerTransferIntake {
 
                         robot.setIntakePower(1.0);
 
-                        if (stateTime() >= 400) {
+                        if (stateTime() >= 100) {
                             setRapidMode(RapidMode.HOLD_BALLS);
                         }
 
@@ -383,7 +385,9 @@ public class SpindexerTransferIntake {
                     case HOLD_BALLS:
 
                         if (robot.insideBeam.isPressed()
-                                && robot.outsideBeam.isPressed() && robot.revSensor.getDistance(DistanceUnit.CM) < sensorDistanceThreshold) {
+                                && robot.outsideBeam.isPressed()) {
+
+                            intakeFull = true;
 
                             robot.setTransferPower(0);
                             robot.setIntakePower(0.1);
@@ -391,11 +395,12 @@ public class SpindexerTransferIntake {
 
                         } else {
                             setRapidMode(RapidMode.INTAKE);
+                            intakeFull = false;
                         }
                         break;
 
                     case OPEN_GATE:
-
+                        intakeFull = false;
                         if (stateTime() >= 100) {
                             setRapidMode(RapidMode.SHOOT);
                         }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/VelocityCommander.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/VelocityCommander.java
index 7ef9287..b28952c 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/VelocityCommander.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/VelocityCommander.java
@@ -79,7 +79,7 @@ public class VelocityCommander {
 
     private void distToTransferPow(double dist, double voltage){
         if (dist < 140){
-            transferPow = -0.8;
+            transferPow = -0.85;
         } else {
             transferPow = -0.7;
         }