From 02dc2b49c59b8b436586489bfad3dcc06139fe43 Mon Sep 17 00:00:00 2001
From: Titan Robotics Club <trc492@users.noreply.github.com>
Date: Sun, 13 Oct 2024 13:10:01 -0700
Subject: [PATCH] Updated ftclib. Fixed CameraStreamProcessor bug to prevent
 adding RectInfo if the processor is not enabled. Made the
 CameraStreamProcessor easier to use.

---
 TeamCode/src/main/java/ftclib                 |  2 +-
 TeamCode/src/main/java/teamcode/FtcTest.java  |  2 ++
 TeamCode/src/main/java/teamcode/Robot.java    |  1 +
 .../src/main/java/teamcode/vision/Vision.java | 28 +++++++++++++++++--
 4 files changed, 29 insertions(+), 4 deletions(-)

diff --git a/TeamCode/src/main/java/ftclib b/TeamCode/src/main/java/ftclib
index 2af0213..97820d6 160000
--- a/TeamCode/src/main/java/ftclib
+++ b/TeamCode/src/main/java/ftclib
@@ -1 +1 @@
-Subproject commit 2af0213e992e5b17da8ed4e6e69f5c31ac247256
+Subproject commit 97820d65a67f33378ca98ce9e6230e1bb94362fa
diff --git a/TeamCode/src/main/java/teamcode/FtcTest.java b/TeamCode/src/main/java/teamcode/FtcTest.java
index 5918f4d..fd81d3c 100644
--- a/TeamCode/src/main/java/teamcode/FtcTest.java
+++ b/TeamCode/src/main/java/teamcode/FtcTest.java
@@ -223,6 +223,7 @@ public class FtcTest extends FtcTeleOp
             case VISION_TEST:
                 if (robot.vision != null)
                 {
+                    robot.vision.setCameraStreamEnabled(true);
                     if (robot.vision.vision != null)
                     {
                         exposure = robot.vision.vision.getCurrentExposure();
@@ -258,6 +259,7 @@ public class FtcTest extends FtcTeleOp
                 if (robot.vision != null && robot.vision.rawColorBlobVision != null)
                 {
                     robot.globalTracer.traceInfo(moduleName, "Enabling FtcRawEocvVision.");
+                    robot.vision.setCameraStreamEnabled(true);
                     robot.vision.setRawColorBlobVisionEnabled(true);
                     colorThresholds = robot.vision.getRawColorBlobThresholds();
                     colorThresholdIndex = 0;
diff --git a/TeamCode/src/main/java/teamcode/Robot.java b/TeamCode/src/main/java/teamcode/Robot.java
index 8797323..96d55a1 100644
--- a/TeamCode/src/main/java/teamcode/Robot.java
+++ b/TeamCode/src/main/java/teamcode/Robot.java
@@ -195,6 +195,7 @@ public class Robot
         //
         if (vision != null)
         {
+            vision.setCameraStreamEnabled(false);
             if (vision.rawColorBlobVision != null)
             {
                 globalTracer.traceInfo(moduleName, "Disabling RawColorBlobVision.");
diff --git a/TeamCode/src/main/java/teamcode/vision/Vision.java b/TeamCode/src/main/java/teamcode/vision/Vision.java
index f7e772f..16125ab 100644
--- a/TeamCode/src/main/java/teamcode/vision/Vision.java
+++ b/TeamCode/src/main/java/teamcode/vision/Vision.java
@@ -239,7 +239,7 @@ public class Vision
     {
         if (vision != null)
         {
-            vision.getVisionPortal().close();
+            vision.close();
         }
     }   //close
 
@@ -375,6 +375,12 @@ public class Vision
         TrcVisionTargetInfo<TrcOpenCvDetector.DetectedObject<?>> colorBlobInfo =
             rawColorBlobVision != null? rawColorBlobVision.getBestDetectedTargetInfo(null, null, 0.0, 0.0): null;
 
+        if (cameraStreamProcessor != null && colorBlobInfo != null)
+        {
+            cameraStreamProcessor.addRectInfo(
+                colorBlobInfo.detectedObj.label, colorBlobInfo.detectedObj.getRotatedRectVertices());
+        }
+
         if (lineNum != -1)
         {
             robot.dashboard.displayPrintf(
@@ -481,7 +487,10 @@ public class Vision
      */
     public void setCameraStreamEnabled(boolean enabled)
     {
-        setVisionProcessorEnabled(cameraStreamProcessor, enabled);
+        if (vision != null && cameraStreamProcessor != null)
+        {
+            cameraStreamProcessor.setCameraStreamEnabled(vision, enabled);
+        }
     }   //setCameraStreamEnabled
 
     /**
@@ -491,7 +500,7 @@ public class Vision
      */
     public boolean isCameraStreamEnabled()
     {
-        return isVisionProcessorEnabled(cameraStreamProcessor);
+        return cameraStreamProcessor != null && cameraStreamProcessor.isCameraStreamEnabled();
     }   //isAprilTagVisionEnabled
 
     /**
@@ -526,6 +535,13 @@ public class Vision
         TrcVisionTargetInfo<FtcVisionAprilTag.DetectedObject> aprilTagInfo =
             aprilTagVision.getBestDetectedTargetInfo(id, null);
 
+        if (cameraStreamProcessor != null && aprilTagInfo != null)
+        {
+            cameraStreamProcessor.addRectInfo(
+                Integer.toString(aprilTagInfo.detectedObj.aprilTagDetection.id),
+                                 aprilTagInfo.detectedObj.getRotatedRectVertices());
+        }
+
         if (aprilTagInfo != null && robot.blinkin != null)
         {
             robot.blinkin.setDetectedPattern(BlinkinLEDs.APRIL_TAG);
@@ -704,6 +720,12 @@ public class Vision
                 break;
         }
 
+        if (cameraStreamProcessor != null && colorBlobInfo != null)
+        {
+            cameraStreamProcessor.addRectInfo(
+                colorBlobInfo.detectedObj.label, colorBlobInfo.detectedObj.getRotatedRectVertices());
+        }
+
         if (colorBlobInfo != null && robot.blinkin != null)
         {
             robot.blinkin.setDetectedPattern(colorBlobName);