Add coloooor sensooooooer!!!!

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java

@@ -26,7 +26,7 @@ public class Poses {
 
 
 
-    public static double rx1 = 50, ry1 = -4, rh1 = 0;
+    public static double rx1 = 46, ry1 = -4, rh1 = 0;
 
     public static double rx2a = 45, ry2a = 5, rh2a = Math.toRadians(140);
 
@@ -36,7 +36,7 @@ public class Poses {
 
     public static double rx3b = 34, ry3b = 58, rh3b = Math.toRadians(140);
 
-    public static double bx1 = 50, by1 = 4, bh1 = 0;
+    public static double bx1 = 46, by1 = 4, bh1 = 0;
 
     public static double bx2a = 45, by2a = -5, bh2a = Math.toRadians(-140);
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java

@@ -1,6 +1,5 @@
 package org.firstinspires.ftc.teamcode.constants;
 
-
 import com.acmerobotics.dashboard.config.Config;
 
 @Config
@@ -21,15 +20,12 @@ public class ServoPositions {
 
     public static double transferServo_in = 0.38;
 
-
-
     public static double turret_range = 0.9;
 
     public static double hoodDefault = 0.6;
 
     public static double hoodAuto = 0.59;
 
-
     public static double hoodHigh = 0.21;
 
     public static double hoodLow = 1.0;
@@ -41,5 +37,4 @@ public class ServoPositions {
 
     public static double turret_detectBlue = 0.6;
 
-
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java

@@ -40,6 +40,7 @@ public class TeleopV2 extends LinearOpMode {
     public static double hoodDefaultPos = 0.5;
     public static double desiredTurretAngle = 180;
     public static double velMultiplier = 20;
+    public static double shootStamp2 = 0.0;
     public double vel = 3000;
     public boolean autoVel = true;
     public double manualOffset = 0.0;
@@ -51,6 +52,7 @@ public class TeleopV2 extends LinearOpMode {
     public boolean circle = false;
     public boolean square = false;
     public boolean triangle = false;
+    double autoHoodOffset = 0.0;
     Robot robot;
     MultipleTelemetry TELE;
     boolean intake = false;
@@ -399,15 +401,19 @@ public class TeleopV2 extends LinearOpMode {
             //HOOD:
 
             if (autoHood) {
-                robot.hood.setPosition(hoodAnglePrediction(distanceToGoal));
+                robot.hood.setPosition(hoodAnglePrediction(distanceToGoal) + autoHoodOffset);
             } else {
                 robot.hood.setPosition(hoodDefaultPos + hoodOffset);
             }
 
             if (gamepad2.dpadUpWasPressed()) {
                 hoodOffset -= 0.03;
+                autoHoodOffset -= 0.02;
+
             } else if (gamepad2.dpadDownWasPressed()) {
                 hoodOffset += 0.03;
+                autoHoodOffset += 0.02;
+
             }
 
             if (gamepad2.left_stick_x > 0.5) {
@@ -415,6 +421,7 @@ public class TeleopV2 extends LinearOpMode {
             } else if (gamepad2.left_stick_x < -0.5) {
                 manualTurret = true;
                 manualOffset = 0;
+                autoHoodOffset = 0;
                 if (gamepad2.left_bumper) {
                     xOffset = robotX;
                     yOffset = robotY;
@@ -461,7 +468,6 @@ public class TeleopV2 extends LinearOpMode {
             } else if (shootAll) {
 
                 TELE.addData("100% works", shootOrder);
-                TELE.update();
 
                 intake = false;
                 reject = false;
@@ -493,7 +499,7 @@ public class TeleopV2 extends LinearOpMode {
 
                     switch (currentSlot) {
                         case 1:
-                            shootA = shootTeleop(spindexer_outtakeBall1, outtake1);
+                            shootA = shootTeleop(spindexer_outtakeBall1, outtake1, shootStamp2);
                             TELE.addData("shootA", shootA);
 
                             if ((getRuntime() - shootStamp) < 4 * (4 - shootOrder.size())) {
@@ -503,7 +509,7 @@ public class TeleopV2 extends LinearOpMode {
                             }
                             break;
                         case 2:
-                            shootB = shootTeleop(spindexer_outtakeBall2, outtake2);
+                            shootB = shootTeleop(spindexer_outtakeBall2, outtake2, shootStamp2);
                             TELE.addData("shootB", shootB);
                             if ((getRuntime() - shootStamp) < 4 * (4 - shootOrder.size())) {
                                 shootingDone = !shootB;
@@ -512,7 +518,7 @@ public class TeleopV2 extends LinearOpMode {
                             }
                             break;
                         case 3:
-                            shootC = shootTeleop(spindexer_outtakeBall3, outtake3);
+                            shootC = shootTeleop(spindexer_outtakeBall3, outtake3, shootStamp2);
                             TELE.addData("shootC", shootC);
                             if ((getRuntime() - shootStamp) < 4 * (4 - shootOrder.size())) {
                                 shootingDone = !shootC;
@@ -525,6 +531,8 @@ public class TeleopV2 extends LinearOpMode {
                     // Remove from the list only if shooting is complete
                     if (shootingDone) {
                         shootOrder.remove(0);
+                        shootStamp2 = getRuntime();
+
                     }
 
                 } else {
@@ -548,6 +556,7 @@ public class TeleopV2 extends LinearOpMode {
             if (gamepad2.squareWasPressed()) {
                 square = true;
                 shootStamp = getRuntime();
+                shootStamp2 = getRuntime();
                 outtake1 = false;
                 outtake2 = false;
                 outtake3 = false;
@@ -556,6 +565,8 @@ public class TeleopV2 extends LinearOpMode {
             if (gamepad2.circleWasPressed()) {
                 circle = true;
                 shootStamp = getRuntime();
+                shootStamp2 = getRuntime();
+
                 outtake1 = false;
                 outtake2 = false;
                 outtake3 = false;
@@ -565,6 +576,8 @@ public class TeleopV2 extends LinearOpMode {
             if (gamepad2.triangleWasPressed()) {
                 triangle = true;
                 shootStamp = getRuntime();
+                shootStamp2 = getRuntime();
+
                 outtake1 = false;
                 outtake2 = false;
                 outtake3 = false;
@@ -614,9 +627,39 @@ public class TeleopV2 extends LinearOpMode {
                 outtake3 = false;
 
                 // Fastest order (example: slot 3 → 2 → 1)
+
                 shootOrder.add(3);
+
                 shootOrder.add(2);
+
                 shootOrder.add(1);
+
+                shootAll = true;
+                shootPos = drive.localizer.getPose();
+
+            }
+
+            // Right bumper shoots all balls fastest, ignoring colors
+            if (gamepad2.leftBumperWasPressed()) {
+                shootOrder.clear();
+                shootStamp = getRuntime();
+
+                outtake1 = false;
+                outtake2 = false;
+                outtake3 = false;
+
+                // Fastest order (example: slot 3 → 2 → 1)
+
+                if (ballIn(3)) {
+                    shootOrder.add(3);
+                }
+
+                if (ballIn(2)) {
+                    shootOrder.add(2);
+                }
+                if (ballIn(1)) {
+                    shootOrder.add(1);
+                }
                 shootAll = true;
                 shootPos = drive.localizer.getPose();
 
@@ -635,9 +678,9 @@ public class TeleopV2 extends LinearOpMode {
                 hub.clearBulkCache();
             }
 
-            TELE.addData("Spin1Green", green1);
+            TELE.addData("Spin1Green", green1 + ": " + ballIn(1));
-            TELE.addData("Spin2Green", green2);
+            TELE.addData("Spin2Green", green2 + ": " + ballIn(2));
-            TELE.addData("Spin3Green", green3);
+            TELE.addData("Spin3Green", green3 + ": " + ballIn(3));
 
             TELE.addData("pose", drive.localizer.getPose());
             TELE.addData("heading", drive.localizer.getPose().heading.toDouble());
@@ -683,13 +726,13 @@ public class TeleopV2 extends LinearOpMode {
                 scalar * ((robot.spin1Pos.getVoltage() - restPos) / 3.3) < spindexer + 0.01);
     }
 
-    public boolean shootTeleop(double spindexer, boolean spinOk) {
+    public boolean shootTeleop(double spindexer, boolean spinOk, double stamp) {
         // Set spin positions
         robot.spin1.setPosition(spindexer);
         robot.spin2.setPosition(1 - spindexer);
 
         // Check if spindexer has reached the target position
-        if (spinOk) {
+        if (spinOk || getRuntime() - stamp > 1.5) {
             if (tickerA == 1) {
                 transferStamp = getRuntime();
                 tickerA++;
@@ -743,7 +786,6 @@ public class TeleopV2 extends LinearOpMode {
         }
     }
 
-
     void addOddThenRest(List<Integer> order, boolean oddColor) {
         // Odd ball first
         for (int i = 1; i <= 3; i++) if (getBallColor(i) == oddColor) order.add(i);
@@ -816,4 +858,31 @@ public class TeleopV2 extends LinearOpMode {
         return false; // default
     }
 
+    boolean ballIn(int slot) {
+        switch (slot) {
+            case 1:
+
+                if (!s1T.isEmpty()) {
+
+                    return !(s1T.get(s1T.size() - 1) < (getRuntime()) - 3);
+                }
+
+            case 2:
+
+                if (!s2T.isEmpty()) {
+
+                    return !(s2T.get(s2T.size() - 1) < (getRuntime()) - 3);
+                }
+
+            case 3:
+
+                if (!s3T.isEmpty()) {
+
+                    return !(s3T.get(s3T.size() - 1) < (getRuntime()) - 3);
+
+                }
+        }
+        return true; // default
+    }
+
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java

@@ -17,7 +17,7 @@ public class PositionalServoProgrammer extends LinearOpMode {
     public static double hoodPos = 0.501;
 
     public static double scalar = 1.112;
-    public static double restPos = 0.158;
+    public static double restPos = 0.15;
     @Override
     public void runOpMode() throws InterruptedException {
         robot = new Robot(hardwareMap);