started adding code for getting closest note (based on target area)

Team334 · Mar 26, 2024 · cb38c33 · cb38c33
1 parent e3a91b3
commit cb38c33
Show file tree

Hide file tree

Showing 2 changed files with 37 additions and 5 deletions.
diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java
@@ -1,6 +1,8 @@
 /* Copyright (C) 2024 Team 334. All Rights Reserved.*/
 package frc.robot.subsystems;
 
+import java.util.Collections;
+import java.util.Comparator;
 import java.util.Optional;
 
 import javax.swing.text.html.Option;
@@ -183,8 +185,38 @@ public double[] tagAngleOffsets(int ID) {
   }
 
   public double[] getNoteAngles() {
-    JsonNode target = _intake.getNeuralTarget();
-    double[] angles = {target.get("tx").asDouble(), target.get("ty").asDouble()};
+    ArrayNode targets = _intake.getNeuralTargets();
+
+    // Collections.sort(
+    //   targets, new Comparator() {
+    //     @Override
+    //     public int compare(JsonNode noteA, JsonNode noteB) {
+    //       double noteA_area = noteA.get("ta").asDouble();
+    //       double noteB_area = noteB.get("ta").asDouble();
+
+    //       if (noteA_area > noteB_area) {
+    //         return 1;
+    //       }
+
+    //       if (noteB_area > noteA_area) {
+    //         return -1;
+    //       }
+
+    //       else {
+    //         return 0;
+    //       }
+    //     }
+    //   }
+    // );
+
+    // return {0, 0};
+
+    JsonNode target = targets.get(0);
+
+    double[] angles = {
+      target.get("tx").asDouble(),
+      target.get("ty").asDouble()
+    };
 
     return angles;
   }

diff --git a/src/main/java/frc/robot/utils/helpers/LimelightHelper.java b/src/main/java/frc/robot/utils/helpers/LimelightHelper.java
@@ -41,13 +41,13 @@ public JsonNode getJson() {
   }
 
   /** 
-   * Returns the neural target from the limelight.
+   * Returns the neural targets from the limelight.
    * 
    */
-  public JsonNode getNeuralTarget() {
+  public ArrayNode getNeuralTargets() {
     ArrayNode targets = (ArrayNode) getJson().get("Results").get("Detector");
 
-    return targets.get(0);
+    return targets;
   }
 
   /**