added obelisk code

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

@@ -1,10 +1,14 @@
 package org.firstinspires.ftc.teamcode.utilsv2;
 
 import com.acmerobotics.dashboard.config.Config;
+import com.qualcomm.hardware.limelightvision.LLResult;
+import com.qualcomm.hardware.limelightvision.LLResultTypes;
 import com.qualcomm.robotcore.util.Range;
 
 import org.firstinspires.ftc.teamcode.utils.Robot;
 
+import java.util.List;
+
 @Config
 public class Turret {
     Robot robot;
@@ -13,16 +17,16 @@ public class Turret {
     private final double neutralPosition = 0.3; //TODO: Tune
     private final double turretMin = 0.04; //TODO: Tune
     private final double turretMax = 0.94; //TODO: Tune
-
     private final double hVelK = 0.12; // TODO: Tune
     private final double hAccK = 0.02; // TODO: Tune
-
     private final double xVelK = 0.10; // TODO: Tune
     private final double xAccK = 0.02; // TODO: Tune
-
     private final double yVelK = 0.10; // TODO: Tune
     private final double yAccK = 0.02; // TODO: Tune
 
+    private int obeliskID = 0;
+
+
 
     public Turret(Robot rob) {
         this.robot = rob;
@@ -34,6 +38,50 @@ public class Turret {
         return angle;
     }
 
+    public void trackObelisk(double dx, double dy, double h) {
+
+        double heading = wrapAngle(h);
+
+        double fieldRelativeHeading = Math.atan2(dy, dx);
+
+        double desiredAngle = fieldRelativeHeading - heading;
+        double angleDelta = desiredAngle - Math.PI;
+        angleDelta = wrapAngle(angleDelta);
+
+        double servoTicksFromNeutral = (angleDelta / (2.0 * Math.PI)) * (2.0 * servoTicksPer180);
+
+        double servoAngle = neutralPosition + servoTicksFromNeutral;
+
+        servoAngle = Range.clip(servoAngle, turretMin, turretMax);
+
+        robot.turr1.setPosition(servoAngle);
+        robot.turr2.setPosition(1.0 - servoAngle);
+
+        detectObelisk();
+
+    }
+
+    public int getObeliskID() {
+        return obeliskID;
+    }
+
+    private int detectObelisk() {
+        robot.limelight.pipelineSwitch(1);
+        LLResult result = robot.limelight.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;
+    }
+
+
     public void trackGoal(double dx, double dy, double h, double hVel, double hAcc, double xVel, double xAcc, double yVel, double yAcc) {
         // dx, dy, dz is target - robot
         // h is the raw heading where 0 degrees is positive x in the system of x, y