Add vision minimum

src/main/java/frc4388/robot/Constants.java

@@ -306,7 +306,9 @@ public final class Constants {
 
         public static final Transform3d CAMERA_POS = new Transform3d(new Translation3d(-.3048, 0.2413*0, .2794), new Rotation3d(0,0.52333,Math.PI));
         
+        public static final double MIN_ESTIMATION_DISTANCE = 1; // Meters
 
+        // Photonvision thing
         // The standard deviations of our vision estimated poses, which affect correction rate
         // (Fake values. Experiment and determine estimation noise on an actual robot.)
         public static final Matrix<N3, N1> kSingleTagStdDevs = VecBuilder.fill(4, 4, 8);

src/main/java/frc4388/robot/subsystems/Vision.java

@@ -20,6 +20,7 @@ import org.photonvision.simulation.PhotonCameraSim;
 import org.photonvision.simulation.SimCameraProperties;
 import org.photonvision.simulation.VisionSystemSim;
 import org.photonvision.targeting.MultiTargetPNPResult;
+import org.photonvision.targeting.PhotonPipelineResult;
 import org.photonvision.targeting.PhotonTrackedTarget;
 
 import com.ctre.phoenix6.Utils;
@@ -80,26 +81,14 @@ public class Vision extends Subsystem {
 
     @Override
     public void periodic() {
-        var result = camera.getLatestResult();
+        // var result = camera.getLatestResult();
-        // var results = camera.getAllUnreadResults();
+        var results = camera.getAllUnreadResults();
-        // if (results.size() == 0) return;
+        if (results.size() == 0) return;
-        // var result = results.get(results.size()-1);
+        var result = results.get(results.size()-1);
+
         isTagDetected = result.hasTargets();
         isTagProcessed = false;
         
-        // Optional<MultiTargetPNPResult> multitag = result.getMultiTagResult();
-
-        // if (multitag.isEmpty()) {
-        //     Transform3d fieldToCamera = result.getMultiTagResult().estimatedPose.best;
-        // }else if()
-
-        
-        // sbTag.setBoolean(isTag);
-        // field.setRobotPose(getPose2d());
-
-        // sbCamConnected.setBoolean(camera);
-
-        // System.out.println(isTag);
 
         if(!isTagDetected){
             // sbTagDetected.setBoolean(isTagDetected);
@@ -107,12 +96,11 @@ public class Vision extends Subsystem {
             return;
         }
 
-        var EstimatedRobotPose = getEstimatedGlobalPose();
+        var EstimatedRobotPose = getEstimatedGlobalPose(result);
 
         // In case the pose estimator fails to estimate the pose, fallback to physical odometry.
         if(EstimatedRobotPose.isEmpty()){
-            // sbTagProcessed.setBoolean(isTagProcessed);
+            field.setRobotPose(getPose2d());
-            field.setRobotPose(lastVisionPose);
             return;
         }
 
@@ -143,9 +131,22 @@ public class Vision extends Subsystem {
      * @return An {@link EstimatedRobotPose} with an estimated pose, estimate timestamp, and targets
      *     used for estimation.
      */
-    public Optional<EstimatedRobotPose> getEstimatedGlobalPose() {
+    public Optional<EstimatedRobotPose> getEstimatedGlobalPose(PhotonPipelineResult change) {
         Optional<EstimatedRobotPose> visionEst = Optional.empty();
-        for (var change : camera.getAllUnreadResults()) {
+        // for (var change : camera.getAllUnreadResults()) {
+
+            var targets = change.getTargets();
+            for(int i = targets.size()-1; i >= 0; i--){
+                Transform3d pos = targets.get(i).getBestCameraToTarget();
+                double distance = Math.sqrt(Math.pow(pos.getX(),2) + Math.pow(pos.getY(),2) + Math.pow(pos.getZ(),2));
+                if (distance > VisionConstants.MIN_ESTIMATION_DISTANCE) {
+                    change.targets.remove(i);
+                }
+            }
+
+            if(targets.size() <= 0)
+                return visionEst; // Will be empty
+
             visionEst = photonEstimator.update(change);
             updateEstimationStdDevs(visionEst, change.getTargets());
 
@@ -159,16 +160,11 @@ public class Vision extends Subsystem {
             //                 getSimDebugField().getObject("VisionEstimation").setPoses();
             //             });
             // }
-        }
+        // }
         return visionEst;
     }
 
 
-
-
-
-
-
     /**
      * Calculates new standard deviations This algorithm is a heuristic that creates dynamic standard
      * deviations based on number of tags, estimation strategy, and distance from the tags.
@@ -192,13 +188,17 @@ public class Vision extends Subsystem {
             for (var tgt : targets) {
                 var tagPose = photonEstimator.getFieldTags().getTagPose(tgt.getFiducialId());
                 if (tagPose.isEmpty()) continue;
-                numTags++;
+                
-                avgDist +=
+                double distance = tagPose
-                        tagPose
+                .get()
-                                .get()
+                .toPose2d()
-                                .toPose2d()
+                .getTranslation()
-                                .getTranslation()
+                .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation());
-                                .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation());
+                
+                if (distance < VisionConstants.MIN_ESTIMATION_DISTANCE) {
+                    numTags++;
+                    avgDist += distance;
+                }
             }
 
             if (numTags == 0) {