diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java index da36dfc..1306d47 100644 --- 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 index 14f4343..87ba8a9 100644 --- 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; } /**