From db382716c4cb65b243bc866f5ab8291bb0c3a1c7 Mon Sep 17 00:00:00 2001
From: Nummun14 <142012009+Nummun14@users.noreply.github.com>
Date: Thu, 21 Aug 2025 16:56:07 +0300
Subject: [PATCH] removed the option for multiple cameras

---
 .../ObjectPoseEstimator.java                  | 36 +++++++++----------
 1 file changed, 17 insertions(+), 19 deletions(-)

diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java
index b7532202..6828f023 100644
--- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java
+++ b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java
@@ -13,7 +13,7 @@
 
 public class ObjectPoseEstimator extends SubsystemBase {
     private final HashMap<Translation2d, Double> knownObjectPositions;
-    private final ObjectDetectionCamera[] cameras;
+    private final ObjectDetectionCamera camera;
     private final double deletionThresholdSeconds;
     private final SimulatedGamePieceConstants.GamePieceType gamePieceType;
     private final double rotationToTranslation;
@@ -23,12 +23,12 @@ public class ObjectPoseEstimator extends SubsystemBase {
      *
      * @param deletionThresholdSeconds the time in seconds after which an object is considered old and removed
      * @param gamePieceType            the type of game piece to track
-     * @param cameras                  the cameras used for detecting objects
+     * @param camera                   the camera used for detecting objects
      */
-    public ObjectPoseEstimator(double deletionThresholdSeconds, DistanceCalculationMethod distanceCalculationMethod, SimulatedGamePieceConstants.GamePieceType gamePieceType, ObjectDetectionCamera... cameras) {
+    public ObjectPoseEstimator(double deletionThresholdSeconds, DistanceCalculationMethod distanceCalculationMethod, SimulatedGamePieceConstants.GamePieceType gamePieceType, ObjectDetectionCamera camera) {
         this.deletionThresholdSeconds = deletionThresholdSeconds;
         this.gamePieceType = gamePieceType;
-        this.cameras = cameras;
+        this.camera = camera;
         this.knownObjectPositions = new HashMap<>();
         this.rotationToTranslation = distanceCalculationMethod.rotationToTranslation;
     }
@@ -142,23 +142,21 @@ private double calculateObjectDistanceRating(Translation2d objectTranslation, Po
 
     private void updateObjectPositions() {
         final double currentTimestamp = Timer.getTimestamp();
-        for (ObjectDetectionCamera camera : this.cameras) {
-            for (Translation2d visibleObject : camera.getObjectPositionsOnField(gamePieceType)) {
-                Translation2d closestObjectToVisibleObject = new Translation2d();
-                double closestObjectToVisibleObjectDistanceMeters = Double.POSITIVE_INFINITY;
-
-                for (Translation2d knownObject : knownObjectPositions.keySet()) {
-                    final double currentObjectDistanceMeters = visibleObject.getDistance(knownObject);
-                    if (currentObjectDistanceMeters < closestObjectToVisibleObjectDistanceMeters) {
-                        closestObjectToVisibleObjectDistanceMeters = currentObjectDistanceMeters;
-                        closestObjectToVisibleObject = knownObject;
-                    }
+        for (Translation2d visibleObject : camera.getObjectPositionsOnField(gamePieceType)) {
+            Translation2d closestObjectToVisibleObject = new Translation2d();
+            double closestObjectToVisibleObjectDistanceMeters = Double.POSITIVE_INFINITY;
+
+            for (Translation2d knownObject : knownObjectPositions.keySet()) {
+                final double currentObjectDistanceMeters = visibleObject.getDistance(knownObject);
+                if (currentObjectDistanceMeters < closestObjectToVisibleObjectDistanceMeters) {
+                    closestObjectToVisibleObjectDistanceMeters = currentObjectDistanceMeters;
+                    closestObjectToVisibleObject = knownObject;
                 }
-
-                if (closestObjectToVisibleObjectDistanceMeters < ObjectDetectionCameraConstants.TRACKED_OBJECT_TOLERANCE_METERS && knownObjectPositions.get(closestObjectToVisibleObject) != currentTimestamp)
-                    knownObjectPositions.remove(closestObjectToVisibleObject);
-                knownObjectPositions.put(visibleObject, currentTimestamp);
             }
+
+            if (closestObjectToVisibleObjectDistanceMeters < ObjectDetectionCameraConstants.TRACKED_OBJECT_TOLERANCE_METERS && knownObjectPositions.get(closestObjectToVisibleObject) != currentTimestamp)
+                knownObjectPositions.remove(closestObjectToVisibleObject);
+            knownObjectPositions.put(visibleObject, currentTimestamp);
         }
     }