added targeting information and unjaming code (both untested)

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

@@ -3,6 +3,7 @@ package org.firstinspires.ftc.teamcode.teleop;
 import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1;
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
 import static org.firstinspires.ftc.teamcode.utils.Servos.spinD;
@@ -123,6 +124,7 @@ public class TeleopV3 extends LinearOpMode {
     private boolean transferIn = false;
     boolean turretInterpolate = false;
     public static double spinSpeedIncrease = 0.04;
+    public static int resetSpinTicks = 20;
 
     public static double velPrediction(double distance) {
         if (distance < 30) {
@@ -582,8 +584,12 @@ public class TeleopV3 extends LinearOpMode {
 //            }
 
             if (enableSpindexerManager) {
-                if (!shootAll) {
+                // Gives some time to reset spindexer
+                if (!shootAll && intakeTicker > resetSpinTicks) {
                     spindexer.processIntake();
+                } else {
+                    robot.spin1.setPosition(spindexer_intakePos1);
+                    robot.spin2.setPosition(1-spindexer_intakePos1);
                 }
 
                 // RIGHT_BUMPER
@@ -606,15 +612,16 @@ public class TeleopV3 extends LinearOpMode {
                     spindexPos = spinStartPos;// TODO: Change starting position based on desired order to shoot green ball
 
                 }
-
+                intakeTicker++;
                 if (shootAll) {
-
+                    intakeTicker = 0;
                     intake = false;
                     reject = false;
 
-                    if (getRuntime() - shootStamp < 3.5) {
+                    if (getRuntime() - shootStamp < 3.5 && servo.getSpinPos() < spindexer_outtakeBall3 + 0.1) {
 
                         if (shooterTicker == 0 && !servo.spinEqual(spindexPos)){
+                            robot.transferServo.setPosition(transferServo_out);
                             robot.spin1.setPosition(spindexPos);
                             robot.spin2.setPosition(1-spindexPos);
                         } else {

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

@@ -39,6 +39,8 @@ public class Spindexer {
     public double distanceFrontDriver = 0.0;
     public double distanceFrontPassenger = 0.0;
 
+    private double prevPos = 0.0;
+
     public Types.Motif desiredMotif = Types.Motif.NONE;
     // For Use
     enum RotatedBallPositionNames {
@@ -246,10 +248,21 @@ public class Spindexer {
 
         return newPos1Detection;
     }
-
+    // Has code to unjam spindexer
-    public void moveSpindexerToPos(double pos) {
+    private void moveSpindexerToPos(double pos) {
         robot.spin1.setPosition(pos);
         robot.spin2.setPosition(1-pos);
+        double currentPos = servos.getSpinPos();
+        if (!servos.spinEqual(pos) && Math.abs(prevPos - currentPos) <= 0){
+            if (currentPos > pos){
+                robot.spin1.setPosition(1);
+                robot.spin2.setPosition(0);
+            } else {
+                robot.spin1.setPosition(0);
+                robot.spin2.setPosition(1);
+            }
+        }
+        prevPos = currentPos;
     }
 
     public void stopSpindexer() {

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

@@ -67,19 +67,19 @@ public class Targeting {
         KNOWNTARGETING[3][4] = new Settings (3100.0, 0.47);
         KNOWNTARGETING[3][5] = new Settings (3100.0, 0.47);
         // ROW 4
-        KNOWNTARGETING[4][0] = new Settings (4540.0, 0.1);
+        KNOWNTARGETING[4][0] = new Settings (3100, 0.49);
-        KNOWNTARGETING[4][1] = new Settings (4541.0, 0.1);
+        KNOWNTARGETING[4][1] = new Settings (3100, 0.49);
-        KNOWNTARGETING[4][2] = new Settings (4542.0, 0.1);
+        KNOWNTARGETING[4][2] = new Settings (3100, 0.5);
-        KNOWNTARGETING[4][3] = new Settings (4543.0, 0.1);
+        KNOWNTARGETING[4][3] = new Settings (3200, 0.5);
-        KNOWNTARGETING[4][4] = new Settings (4544.0, 0.1);
+        KNOWNTARGETING[4][4] = new Settings (3250, 0.49);
-        KNOWNTARGETING[4][5] = new Settings (4545.0, 0.1);
+        KNOWNTARGETING[4][5] = new Settings (3300, 0.49);
-        // ROW 1
+        // ROW 5
-        KNOWNTARGETING[5][0] = new Settings (4550.0, 0.1);
+        KNOWNTARGETING[5][0] = new Settings (3200, 0.48);
-        KNOWNTARGETING[5][1] = new Settings (4551.0, 0.1);
+        KNOWNTARGETING[5][1] = new Settings (3200, 0.48);
-        KNOWNTARGETING[5][2] = new Settings (4552.0, 0.1);
+        KNOWNTARGETING[5][2] = new Settings (3300, 0.48);
-        KNOWNTARGETING[5][3] = new Settings (4553.0, 0.1);
+        KNOWNTARGETING[5][3] = new Settings (3350, 0.48);
-        KNOWNTARGETING[5][4] = new Settings (4554.0, 0.1);
+        KNOWNTARGETING[5][4] = new Settings (3350, 0.48);
-        KNOWNTARGETING[5][5] = new Settings (4555.0, 0.1);
+        KNOWNTARGETING[5][5] = new Settings (3350, 0.48);
     }
 
     public Targeting()

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

@@ -119,17 +119,14 @@ public class Turret {
     }
 
     public double getTy() {
-        limelightRead();
         return ty;
     }
 
     public double getLimelightX() {
-        limelightRead();
         return limelightPosX;
     }
 
     public double getLimelightY() {
-        limelightRead();
         return limelightPosY;
     }