Loop on separate thread for PV

Frc5572 · Sep 25, 2024 · 0db7fcb · 0db7fcb
1 parent f0cc09e
commit 0db7fcb
Show file tree

Hide file tree

Showing 2 changed files with 27 additions and 19 deletions.
diff --git a/src/main/java/frc/lib/util/photon/PhotonCameraWrapper.java b/src/main/java/frc/lib/util/photon/PhotonCameraWrapper.java
@@ -1,7 +1,5 @@
 package frc.lib.util.photon;
 
-import java.io.File;
-import java.io.IOException;
 import java.util.Optional;
 import org.littletonrobotics.junction.Logger;
 import org.photonvision.EstimatedRobotPose;
@@ -16,7 +14,6 @@
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
-import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.lib.util.photon.PhotonIO.PhotonInputs;
@@ -63,16 +60,16 @@ public PhotonCameraWrapper(PhotonIO io, Transform3d robotToCam) {
     public void periodic() {
         this.io.updateInputs(this.inputs);
         Logger.processInputs("PhotonVision/" + inputs.name, inputs);
-        if (this.inputs.distCoeffs.length == 0 && Timer.getFPGATimestamp() - resetTimer > 5) {
-            resetTimer = Timer.getFPGATimestamp();
-            try {
-                this.io.uploadSettings(this.io.ip + ":5800",
-                    new File(Filesystem.getDeployDirectory().getAbsoluteFile(),
-                        "photon-configs/" + inputs.name + ".zip"));
-            } catch (IOException e) {
-                e.printStackTrace();
-            }
-        }
+        // if (this.inputs.distCoeffs.length == 0 && Timer.getFPGATimestamp() - resetTimer > 5) {
+        // resetTimer = Timer.getFPGATimestamp();
+        // try {
+        // this.io.uploadSettings(this.io.ip + ":5800",
+        // new File(Filesystem.getDeployDirectory().getAbsoluteFile(),
+        // "photon-configs/" + inputs.name + ".zip"));
+        // } catch (IOException e) {
+        // e.printStackTrace();
+        // }
+        // }
     }
 
     /**

diff --git a/src/main/java/frc/lib/util/photon/PhotonReal.java b/src/main/java/frc/lib/util/photon/PhotonReal.java
@@ -4,6 +4,7 @@
 import java.io.IOException;
 import java.io.InputStream;
 import java.nio.charset.StandardCharsets;
+import java.util.concurrent.atomic.AtomicBoolean;
 import org.apache.http.HttpEntity;
 import org.apache.http.client.methods.CloseableHttpResponse;
 import org.apache.http.client.methods.HttpPost;
@@ -104,6 +105,8 @@ public boolean uploadSettings(String ip, File file) throws IOException {
         }
     }
 
+    private AtomicBoolean isPhotonOk = new AtomicBoolean(false);
+
     /**
      * Constructs a PhotonReal from a root table.
      *
@@ -113,12 +116,18 @@ public boolean uploadSettings(String ip, File file) throws IOException {
     public PhotonReal(NetworkTableInstance instance, String cameraName, String cameraIP) {
         super(cameraName, cameraIP);
         new Thread(() -> {
-            try {
-                uploadSettings(cameraIP + ":5800",
-                    new File(Filesystem.getDeployDirectory().getAbsoluteFile(),
-                        "photon-configs/" + cameraName + ".zip"));
-            } catch (IOException e) {
-                e.printStackTrace();
+            Timer timer = new Timer();
+            while (true) {
+                if (timer.advanceIfElapsed(5.0) && !isPhotonOk.get()) {
+                    try {
+                        uploadSettings(cameraIP + ":5800",
+                            new File(Filesystem.getDeployDirectory().getAbsoluteFile(),
+                                "photon-configs/" + cameraName + ".zip"));
+                    } catch (IOException e) {
+                        e.printStackTrace();
+                    }
+                }
+                Thread.yield();
             }
         }).start();
         var photonvision_root_table = instance.getTable(kTableName);
@@ -174,6 +183,8 @@ public void updateInputs(PhotonInputs inputs) {
 
         inputs.cameraMatrix = cameraIntrinsicsSubscriber.get(EMPTY);
         inputs.distCoeffs = cameraDistortionSubscriber.get(EMPTY);
+
+        isPhotonOk.set(inputs.distCoeffs.length != 0);
     }
 
     @Override