sorting ahh thing

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java

@@ -5,10 +5,16 @@ import static org.firstinspires.ftc.teamcode.constants.Front_Poses.*;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.blueObeliskTurrPos1;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.blueObeliskTurrPos2;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.blueObeliskTurrPos3;
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.redObeliskTurrPos0;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.redObeliskTurrPos1;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.redObeliskTurrPos2;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.redObeliskTurrPos3;
 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_outtakeBall1;
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2;
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3;
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3b;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
 import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed;
 import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault;
@@ -127,6 +133,7 @@ public class Auto_LT_Close extends LinearOpMode {
     double obeliskTurrPos2 = 0.0;
     double obeliskTurrPos3 = 0.0;
     double waitToPickupGate = 0;
+    double obeliskTurrPosAutoStart = 0;
 
     // initialize path variables here
     TrajectoryActionBuilder shoot0 = null;
@@ -168,19 +175,45 @@ public class Auto_LT_Close extends LinearOpMode {
 
         autoActions = new AutoActions(robot, drive, TELE, servos, flywheel, spindexer, targeting, targetingSettings, turret, light);
 
-        servos.setSpinPos(spinStartPos);
+        servos.setSpinPos(spindexer_intakePos1);
 
         servos.setTransferPos(transferServo_out);
-
+        limelightUsed = false;
-        limelightUsed = true;
 
         robot.light.setPosition(1);
 
         hardwareMap.get(GoBildaPinpointDriver.class, "pinpoint").resetPosAndIMU();
 
-
         while (opModeInInit()) {
+            if (limelightUsed && !gateCycle){
+                Actions.runBlocking(
+                        autoActions.detectObelisk(
+                                0.1,
+                                0.501,
+                                0.501,
+                                0.501,
+                                0.501,
+                                obeliskTurrPosAutoStart
+                        )
+                );
+                motif = turret.getObeliskID();
 
+                if (motif == 21){
+                    AutoActions.firstSpindexShootPos = spindexer_outtakeBall1;
+                } else if (motif == 22){
+                    AutoActions.firstSpindexShootPos = spindexer_outtakeBall3b;
+                } else {
+                    AutoActions.firstSpindexShootPos = spindexer_outtakeBall2;
+                }
+            }
+
+            if (!gateCycle) {
+                turret.pipelineSwitch(1);
+            } else if (redAlliance) {
+                turret.pipelineSwitch(4);
+            } else {
+                turret.pipelineSwitch(2);
+            }
 
             if (gateCycle) {
                 servos.setHoodPos(hoodGate0Start);
@@ -188,8 +221,6 @@ public class Auto_LT_Close extends LinearOpMode {
                 servos.setHoodPos(shoot0Hood);
             }
 
-            turret.setTurret(turrDefault);
-
             if (gamepad2.crossWasPressed()) {
                 redAlliance = !redAlliance;
             }
@@ -209,24 +240,17 @@ public class Auto_LT_Close extends LinearOpMode {
                 ballCycles--;
             }
 
+            if (gamepad2.triangleWasPressed()){
+                gateCycle = !gateCycle;
+            }
+
 
 
             if (gamepad2.squareWasPressed()) {
 
-
                 drive = new MecanumDrive(hardwareMap,new Pose2d(0,0,0));
-
-
-                if (!gateCycle) {
-                    turret.pipelineSwitch(1);
-                } else if (redAlliance) {
-                    turret.pipelineSwitch(4);
-                } else {
-                    turret.pipelineSwitch(2);
-                }
                 robot.limelight.start();
-
+                limelightUsed = true;
-
 
                 gamepad2.rumble(500);
             }
@@ -283,6 +307,7 @@ public class Auto_LT_Close extends LinearOpMode {
                 pickupGateBY = rPickupGateBY;
                 pickupGateBH = rPickupGateBH;
 
+                obeliskTurrPosAutoStart = turrDefault + redObeliskTurrPos0;
                 obeliskTurrPos1 = turrDefault + redObeliskTurrPos1;
                 obeliskTurrPos2 = turrDefault + redObeliskTurrPos2;
                 obeliskTurrPos3 = turrDefault + redObeliskTurrPos3;
@@ -338,6 +363,7 @@ public class Auto_LT_Close extends LinearOpMode {
                 pickupGateBY = bPickupGateBY;
                 pickupGateBH = bPickupGateBH;
 
+                obeliskTurrPosAutoStart = turrDefault + redObeliskTurrPos0;
                 obeliskTurrPos1 = turrDefault + blueObeliskTurrPos1;
                 obeliskTurrPos2 = turrDefault + blueObeliskTurrPos2;
                 obeliskTurrPos3 = turrDefault + blueObeliskTurrPos3;
@@ -414,10 +440,6 @@ public class Auto_LT_Close extends LinearOpMode {
                         .strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot));
             }
 
-
-
-
-
             pickup3 = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot)))
                     .strafeToLinearHeading(new Vector2d(x4a, y4a), Math.toRadians(h4a))
                     .strafeToLinearHeading(new Vector2d(x4b, y4b), Math.toRadians(h4b),
@@ -432,13 +454,13 @@ public class Auto_LT_Close extends LinearOpMode {
                 waitToPickupGate = waitToPickupGateSolo;
             }
 
-
-
-
             teleStart = drive.localizer.getPose();
             TELE.addData("Red?", redAlliance);
             TELE.addData("Turret Default", turrDefault);
+            TELE.addData("Gate Cycle?", gateCycle);
             TELE.addData("Ball Cycles", ballCycles);
+            TELE.addData("Limelight Started?", limelightUsed);
+            TELE.addData("Motif", motif);
 
             TELE.update();
         }
@@ -528,9 +550,19 @@ public class Auto_LT_Close extends LinearOpMode {
                                 y1,
                                 h1,
                                 false
+                        ),
+                        autoActions.detectObelisk(
+                                flywheel0Time,
+                                0.501,
+                                0.501,
+                                0.501,
+                                0.501,
+                                obeliskTurrPosAutoStart
                         )
                 )
         );
+
+        motif = turret.getObeliskID();
     }
 
 
@@ -561,24 +593,10 @@ public class Auto_LT_Close extends LinearOpMode {
                                 x2b,
                                 y2b,
                                 h2b
-                        ),
-                        autoActions.detectObelisk(
-                                intake1Time,
-                                x2b,
-                                y2b,
-                                posXTolerance,
-                                posYTolerance,
-                                obeliskTurrPos1
                         )
-
                 )
         );
 
-        motif = turret.getObeliskID();
-
-        if (motif == 0) motif = 22;
-        prevMotif = motif;
-
         double posX;
         double posY;
         double posH;
@@ -616,24 +634,10 @@ public class Auto_LT_Close extends LinearOpMode {
                                 x3b,
                                 y3b,
                                 h3b
-                        ),
-                        autoActions.detectObelisk(
-                                intake2Time,
-                                x3b,
-                                y3b,
-                                posXTolerance,
-                                posYTolerance,
-                                obeliskTurrPos2
                         )
-
                 )
         );
 
-        motif = turret.getObeliskID();
-
-        if (motif == 0) motif = prevMotif;
-        prevMotif = motif;
-
         double posX;
         double posY;
         double posH;
@@ -670,16 +674,7 @@ public class Auto_LT_Close extends LinearOpMode {
                                 x4b,
                                 y4b,
                                 h4b
-                        ),
-                        autoActions.detectObelisk(
-                                intake3Time,
-                                x4b,
-                                y4b,
-                                posXTolerance,
-                                posYTolerance,
-                                obeliskTurrPos3
                         )
-
                 )
         );
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java

@@ -48,7 +48,7 @@ public class AutoActions {
     private int passengerSlotGreen = 0;
     private int rearSlotGreen = 0;
     private int mostGreenSlot = 0;
-    private double firstSpindexShootPos = spinStartPos;
+    public static double firstSpindexShootPos = spinStartPos;
     private boolean shootForward = true;
     public int motif = 0;
     double spinEndPos = ServoPositions.spinEndPos;
@@ -85,13 +85,13 @@ public class AutoActions {
             void spin1PosFirst() {
                 firstSpindexShootPos = spindexer_outtakeBall1;
                 shootForward = true;
-                spinEndPos = spindexer_outtakeBall3 + 0.1;
+                spinEndPos = 0.95;
             }
 
             void spin2PosFirst() {
                 firstSpindexShootPos = spindexer_outtakeBall2;
                 shootForward = false;
-                spinEndPos = spindexer_outtakeBall3b - 0.1;
+                spinEndPos = 0.05;
             }
 
             void reverseSpin2PosFirst() {
@@ -103,13 +103,13 @@ public class AutoActions {
             void spin3PosFirst() {
                 firstSpindexShootPos = spindexer_outtakeBall3;
                 shootForward = false;
-                spinEndPos = spindexer_outtakeBall1 - 0.1;
+                spinEndPos = 0.05;
             }
 
             void oddSpin3PosFirst() {
                 firstSpindexShootPos = spindexer_outtakeBall3b;
                 shootForward = true;
-                spinEndPos = spindexer_outtakeBall2 + 0.1;
+                spinEndPos = 0.95;
             }
 
             Action manageShooter = null;
@@ -119,6 +119,9 @@ public class AutoActions {
                 if (ticker == 0) {
                     stamp = System.currentTimeMillis();
                     manageShooter = manageShooterAuto(time, posX, posY, posH, false);
+                    driverSlotGreen = 0;
+                    passengerSlotGreen = 0;
+                    rearSlotGreen = 0;
                 }
                 ticker++;
                 servos.setTransferPos(transferServo_out);
@@ -130,6 +133,12 @@ public class AutoActions {
 
                 manageShooter.run(telemetryPacket);
 
+                TELE.addData("Most Green Slot", mostGreenSlot);
+                TELE.addData("Driver Slot Greeness", driverSlotGreen);
+                TELE.addData("Passenger Slot Greeness", passengerSlotGreen);
+                TELE.addData("Rear Greeness", rearSlotGreen);
+                TELE.update();
+
                 if ((System.currentTimeMillis() - stamp) < (colorSenseTime * 1000)) {
 
                     spindexerWiggle *= -1.0;
@@ -150,7 +159,7 @@ public class AutoActions {
                         rearSlotGreen++;
                     }
 
-                    spindexer.setIntakePower(1);
+                    spindexer.setIntakePower(-0.1);
 
                     decideGreenSlot = true;
 
@@ -242,9 +251,9 @@ public class AutoActions {
 
                 boolean end;
                 if (shootForward) {
-                    end = prevSpinPos > spinEndPos;
+                    end = servos.getSpinPos() > spinEndPos;
                 } else {
-                    end = prevSpinPos < spinEndPos;
+                    end = servos.getSpinPos() < spinEndPos;
                 }
 
                 if (System.currentTimeMillis() - stamp < shootTime * 1000 && (!end || shooterTicker < Spindexer.waitFirstBallTicks + 1)) {
@@ -380,7 +389,6 @@ public class AutoActions {
                 manageShooter.run(telemetryPacket);
 
                 if ((System.currentTimeMillis() - stamp) > (time * 1000) || spindexer.isFull()) {
-                    spindexer.setIntakePower(-0.1);
                     return false;
                 } else {
                     return true;
@@ -408,6 +416,7 @@ public class AutoActions {
 
             double stamp = 0.0;
             int ticker = 0;
+            int prevMotif = 0;
 
             @Override
             public boolean run(@NonNull TelemetryPacket telemetryPacket) {
@@ -418,18 +427,23 @@ public class AutoActions {
                 if (ticker == 0) {
                     stamp = System.currentTimeMillis();
                     turret.pipelineSwitch(1);
+                    ticker++;
                 }
 
-                ticker++;
                 motif = turret.detectObelisk();
 
+                if (prevMotif == motif){
+                    ticker++;
+                }
+                prevMotif = motif;
+
                 turret.setTurret(turrPos);
 
                 boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000;
                 boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance;
                 boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance;
 
-                boolean shouldFinish = timeDone || (xDone && yDone) || spindexer.isFull();
+                boolean shouldFinish = timeDone || (xDone && yDone) || spindexer.isFull() || ticker > 10;
 
                 teleStart = currentPose;
 

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

@@ -10,10 +10,10 @@ public class Front_Poses {
     public static double rx1 = 20, ry1 = 0.5, rh1 = 0.1;
     public static double bx1 = 20, by1 = -0.5, bh1 = -0.1;
 
-    public static double rx2a = 41, ry2a = 18, rh2a = 155;
+    public static double rx2a = 41, ry2a = 18, rh2a = 140;
     public static double bx2a = 41, by2a = -18, bh2a = -140;
 
-    public static double rx2b = 21, ry2b = 34, rh2b = 155.1;
+    public static double rx2b = 21, ry2b = 34, rh2b = 140.1;
     public static double bx2b = 23, by2b = -36, bh2b = -140.1;
 
     public static double rx3a = 55, ry3a = 39, rh3a = 140;

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

@@ -33,12 +33,14 @@ public class ServoPositions {
     public static double turret_blueClose = 0;
 
     // These values are ADDED to turrDefault
-    public static double redObeliskTurrPos1 = 0.12;
+    public static double redObeliskTurrPos0 = -0.35;
-    public static double redObeliskTurrPos2 = 0.13;
+    public static double redObeliskTurrPos1 = 0.15;
-    public static double redObeliskTurrPos3 = 0.14;
+    public static double redObeliskTurrPos2 = 0.16;
-    public static double blueObeliskTurrPos1 = -0.12;
+    public static double redObeliskTurrPos3 = 0.17;
-    public static double blueObeliskTurrPos2 = -0.13;
+    public static double blueObeliskTurrPos0 = 0.35;
-    public static double blueObeliskTurrPos3 = -0.14;
+    public static double blueObeliskTurrPos1 = -0.15;
+    public static double blueObeliskTurrPos2 = -0.16;
+    public static double blueObeliskTurrPos3 = -0.17;
     public static double redTurretShootPos = 0.05;
     public static double blueTurretShootPos = -0.05;
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ColorTest.java

@@ -7,6 +7,7 @@ import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.NormalizedRGBA;
 
 import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
 import org.firstinspires.ftc.teamcode.utils.Robot;
@@ -28,16 +29,20 @@ public class ColorTest extends LinearOpMode {
         if (isStopRequested()) return;
 
         while(opModeIsActive()){
-            double green1 = robot.color1.getNormalizedColors().green;
+
-            double blue1 = robot.color1.getNormalizedColors().blue;
+
-            double red1 = robot.color1.getNormalizedColors().red;
+            NormalizedRGBA color1RGBA = robot.color1.getNormalizedColors();
+
+            double gP1 = color1RGBA.green / (color1RGBA.green + color1RGBA.red + color1RGBA.blue);
+
             double dist1 = robot.color1.getDistance(DistanceUnit.MM);
             color1Distance = (colorFilterAlpha * dist1) + ((1-colorFilterAlpha) * color1Distance);
 
             TELE.addData("Color1 toColor", robot.color1.getNormalizedColors().toColor());
-            TELE.addData("Color1 green", green1 / (green1 + blue1 + red1));
+            TELE.addData("Color1 green", gP1);
             TELE.addData("Color1 distance (mm)", color1Distance);
 
+
 // ----- COLOR 2 -----
             double green2 = robot.color2.getNormalizedColors().green;
             double blue2 = robot.color2.getNormalizedColors().blue;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/SortingTest.java

@@ -0,0 +1,107 @@
+package org.firstinspires.ftc.teamcode.tests;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.ftc.Actions;
+import com.acmerobotics.roadrunner.Pose2d;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.teamcode.autonomous.actions.AutoActions;
+import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
+import org.firstinspires.ftc.teamcode.utils.Flywheel;
+import org.firstinspires.ftc.teamcode.utils.Light;
+import org.firstinspires.ftc.teamcode.utils.Robot;
+import org.firstinspires.ftc.teamcode.utils.Servos;
+import org.firstinspires.ftc.teamcode.utils.Spindexer;
+import org.firstinspires.ftc.teamcode.utils.Targeting;
+import org.firstinspires.ftc.teamcode.utils.Turret;
+
+@Config
+@TeleOp
+public class SortingTest extends LinearOpMode {
+    Robot robot;
+    MultipleTelemetry TELE;
+    MecanumDrive drive;
+    Servos servos;
+    Spindexer spindexer;
+    Flywheel flywheel;
+    Turret turret;
+    Targeting targeting;
+    Targeting.Settings targetingSettings;
+    AutoActions autoActions;
+    Light light;
+    @Override
+    public void runOpMode() throws InterruptedException {
+        robot = new Robot(hardwareMap);
+
+        TELE = new MultipleTelemetry(
+                telemetry, FtcDashboard.getInstance().getTelemetry()
+        );
+
+        flywheel = new Flywheel(hardwareMap);
+
+        targeting = new Targeting();
+        targetingSettings = new Targeting.Settings(0.0, 0.0);
+
+        spindexer = new Spindexer(hardwareMap);
+
+        servos = new Servos(hardwareMap);
+
+        turret = new Turret(robot, TELE, robot.limelight);
+
+        drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
+
+        light = Light.getInstance();
+
+        light.init(robot.light, spindexer, turret);
+
+        autoActions = new AutoActions(robot, drive, TELE, servos, flywheel, spindexer, targeting, targetingSettings, turret, light);
+
+        int motif = 21;
+        boolean intaking = true;
+
+        waitForStart();
+        if (isStopRequested()) return;
+
+        while (opModeIsActive()){
+            spindexer.setIntakePower(1);
+            robot.transfer.setPower(1);
+
+            if (gamepad1.crossWasPressed()){
+                motif = 21;
+            } else if (gamepad1.squareWasPressed()){
+                motif = 22;
+            } else if (gamepad1.triangleWasPressed()){
+                motif = 23;
+            }
+            flywheel.manageFlywheel(2500);
+
+            if (gamepad1.leftBumperWasPressed()){
+                intaking = false;
+                Actions.runBlocking(
+                        autoActions.prepareShootAll(
+                                3,
+                                5,
+                                motif,
+                                0.501,
+                                0.501,
+                                0.501
+                        )
+                );
+            } else if (gamepad1.rightBumperWasPressed()){
+                intaking = false;
+                Actions.runBlocking(
+                        autoActions.shootAllAuto(
+                                3.5,
+                                0.014
+                        )
+                );
+                intaking = true;
+            } else if (intaking){
+                spindexer.processIntake();
+            }
+        }
+    }
+}

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

@@ -187,7 +187,7 @@ public class Spindexer {
         distanceFrontPassenger = (colorFilterAlpha * dFrontPassenger) + ((1-colorFilterAlpha) * distanceFrontPassenger);
 
         // Position 1
-        if (distanceRearCenter < 48) {
+        if (distanceRearCenter < 52) {
 
             // Mark Ball Found
             newPos1Detection = true;
@@ -200,9 +200,9 @@ public class Spindexer {
 
                 // FIXIT - Add filtering to improve accuracy.
                 if (gP >= 0.38) {
-                    ballPositions[commandedIntakePosition].ballColor = BallColor.GREEN;  // green
+                    ballPositions[0].ballColor = BallColor.GREEN;  // green
                 } else {
-                    ballPositions[commandedIntakePosition].ballColor = BallColor.PURPLE;  // purple
+                    ballPositions[0].ballColor = BallColor.PURPLE;  // purple
                 }
             }
         }
@@ -218,9 +218,9 @@ public class Spindexer {
                 double gP = color2RGBA.green / (color2RGBA.green + color2RGBA.red + color2RGBA.blue);
 
                 if (gP >= 0.4) {
-                    ballPositions[spindexerBallPos].ballColor = BallColor.GREEN;  // green
+                    ballPositions[2].ballColor = BallColor.GREEN;  // green
                 } else {
-                    ballPositions[spindexerBallPos].ballColor = BallColor.PURPLE;  // purple
+                    ballPositions[2].ballColor = BallColor.PURPLE;  // purple
                 }
             }
         } else {
@@ -244,10 +244,10 @@ public class Spindexer {
 
                 double gP = color3RGBA.green / (color3RGBA.green + color3RGBA.red + color3RGBA.blue);
 
-                if (gP >= 0.42) {
+                if (gP >= 0.4) {
-                    ballPositions[spindexerBallPos].ballColor = BallColor.GREEN;  // green
+                    ballPositions[1].ballColor = BallColor.GREEN;  // green
                 } else {
-                    ballPositions[spindexerBallPos].ballColor = BallColor.PURPLE;  // purple
+                    ballPositions[1].ballColor = BallColor.PURPLE;  // purple
                 }
             }
         } else {
@@ -671,8 +671,10 @@ public class Spindexer {
         return ballPositions[RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.REARCENTER.ordinal()]].ballColor;
     }
     private double prevPow = 0.501;
+    private boolean firstIntakePow = true;
     public void setIntakePower(double pow){
-        if (prevPow != 0.501 && prevPow != pow){
+        if (firstIntakePow || prevPow != pow){
+            firstIntakePow = false;
             robot.intake.setPower(pow);
         }
         prevPow = pow;

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

@@ -14,6 +14,7 @@ import com.qualcomm.robotcore.hardware.DcMotor;
 import org.firstinspires.ftc.robotcore.external.navigation.Pose3D;
 import org.firstinspires.ftc.teamcode.constants.Color;
 
+import java.util.ArrayList;
 import java.util.List;
 
 @Config
@@ -130,7 +131,7 @@ public class Turret {
             }
         }
         if (xPos != null){
-            if (zPos>0) {
+            if (zPos<0) {
                 limelightTagX = (alphaPosConstant * xPos) + ((1 - alphaPosConstant) * limelightTagX);
                 limelightTagY = (alphaPosConstant * yPos) + ((1 - alphaPosConstant) * limelightTagY);
                 limelightTagZ = (alphaPosConstant * zPos) + ((1 - alphaPosConstant) * limelightTagZ);
@@ -171,10 +172,14 @@ public class Turret {
         LLResult result = webcam.getLatestResult();
         if (result != null && result.isValid()) {
             List<LLResultTypes.FiducialResult> fiducials = result.getFiducialResults();
+            double prevTx  = -1000;
             for (LLResultTypes.FiducialResult fiducial : fiducials) {
+                double currentTx = fiducial.getTargetXDegrees();
+                if (currentTx > prevTx){
                     obeliskID = fiducial.getFiducialId();
                 }
             }
+        }
         return obeliskID;
     }