From 91ef73a441c10552a8e1ed7de85624a5b2a30ee9 Mon Sep 17 00:00:00 2001
From: Abhishrek05 <abhi.p.sachi@gmail.com>
Date: Wed, 15 Mar 2023 14:50:30 -0600
Subject: [PATCH] gud

---
 src/main/java/frc4388/robot/Constants.java    |  8 ++-
 .../frc4388/robot/subsystems/Limelight.java   | 55 +++++++++++++++++--
 2 files changed, 55 insertions(+), 8 deletions(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 9953934..d58160e 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -146,13 +146,17 @@ public final class Constants {
   public static final class VisionConstants {
     public static final String NAME = "Aarav is poopy";
 
-    public static final int LIME_FOV = -1;
+    public static final int LIME_HIXELS = 640;
+    public static final int LIME_VIXELS = 480;
+
+    public static final double H_FOV = 59.6;
+    public static final double V_FOV = 45.7;
 
     public static final double LIME_HEIGHT = -1.0;
     public static final double LIME_ANGLE = -1.0;
 
     public static final double HIGH_TARGET_HEIGHT = -1.0;
-    public static final double HIGHT_TAPE_HEIGHT = -1.0;
+    public static final double HIGH_TAPE_HEIGHT = -1.0;
     
     public static final double MID_TARGET_HEIGHT = -1.0;
     public static final double MID_TAPE_HEIGHT = -1.0;
diff --git a/src/main/java/frc4388/robot/subsystems/Limelight.java b/src/main/java/frc4388/robot/subsystems/Limelight.java
index a6d951a..9c91afb 100644
--- a/src/main/java/frc4388/robot/subsystems/Limelight.java
+++ b/src/main/java/frc4388/robot/subsystems/Limelight.java
@@ -11,8 +11,10 @@ import org.opencv.core.Point;
 import org.photonvision.PhotonCamera;
 import org.photonvision.common.hardware.VisionLEDMode;
 import org.photonvision.targeting.PhotonPipelineResult;
+import org.photonvision.targeting.PhotonTrackedTarget;
 import org.photonvision.targeting.TargetCorner;
 
+import edu.wpi.first.math.Pair;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Constants.VisionConstants;
 import frc4388.utility.AbhiIsADumbass;
@@ -32,18 +34,59 @@ public class Limelight extends SubsystemBase {
     cam.setDriverMode(driverMode);
   }
 
-  public Point getTargetPoints() throws AbhiIsADumbass {
+  public ArrayList<Point> getTargetPoints() throws AbhiIsADumbass {
     PhotonPipelineResult result = cam.getLatestResult();
 
     if (!result.hasTargets()) throw new AbhiIsADumbass();
 
-    Point point = new Point();
-    List<TargetCorner> corners = result.getTargets().get(0).getDetectedCorners();
+    ArrayList<Point> points = new ArrayList<>(2);
 
-    double x1, x2, y1, y2;
-    double mx, my;
+    for(PhotonTrackedTarget target : result.getTargets()){
+      List<TargetCorner> corners = target.getDetectedCorners();
+  
+      double sumX = 0.0;
+      double sumY = 0.0;
+      double mx = 0.0;
+      double my = 0.0;
+  
+      for (TargetCorner c : corners) {
+        sumX += c.x;
+        sumY += c.y;
+      }
+  
+      mx = sumX / 4.0;
+      my = sumY / 4.0;
 
-    return new Point(mx, my);
+      points.add(new Point(mx, my));
+    }
+
+    return points;
+  }
+
+  private double getPointAngle(Point point) {
+    return (VisionConstants.LIME_VIXELS - point.y) * (VisionConstants.V_FOV / VisionConstants.LIME_VIXELS);
+  }
+
+  public double getDistanceToTarget(boolean high) throws AbhiIsADumbass {
+    ArrayList<Point> targetPoints = getTargetPoints();
+
+    Point highPoint = targetPoints.get(0).y <= targetPoints.get(1).y ? targetPoints.get(0) : targetPoints.get(1);
+    Point midPoint = targetPoints.get(0).y >= targetPoints.get(1).y ? targetPoints.get(0) : targetPoints.get(1);
+
+    Point tapePoint = high ? highPoint : midPoint;
+    double tapeHeight = high ? VisionConstants.HIGH_TAPE_HEIGHT : VisionConstants.MID_TAPE_HEIGHT;
+    double targetHeight = high ? VisionConstants.HIGH_TARGET_HEIGHT : VisionConstants.MID_TARGET_HEIGHT;
+
+    double theta = VisionConstants.LIME_ANGLE + getPointAngle(tapePoint);
+
+    double effectiveTapeHeight = tapeHeight - VisionConstants.LIME_HEIGHT;
+    double effectiveTargetHeight = targetHeight - VisionConstants.LIME_HEIGHT;
+
+    double distanceToTape = effectiveTapeHeight / Math.sin(Math.toRadians(theta));
+
+    double distanceToTarget = effectiveTargetHeight * distanceToTape / effectiveTapeHeight;
+
+    return distanceToTarget;
   }
 
   @Override