yay it works im so happy

TitaniumTigers4829 · Oct 3, 2024 · f63cb47 · f63cb47
1 parent a945add
commit f63cb47
Showing 1 changed file with 5 additions and 5 deletions.
diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
@@ -67,10 +67,6 @@ && getNumberOfAprilTags(limelightNumber) <= VisionConstants.APRIL_TAG_POSITIONS.
    * @param limelightNumber the number of the limelight
    */
   public PoseEstimate enabledPoseUpdate(int limelightNumber) {
-    // if (!canSeeAprilTags(limelightNumber)) {
-    //   return limelightEstimates[limelightNumber] = new PoseEstimate();
-    // }
-    // Soon to be implemented code:
     if (canSeeAprilTags(limelightNumber) && isValidPoseEstimate(limelightNumber)) {
       if (isLargeDiscrepancyBetweenMegaTag1And2(limelightNumber)
           && getLimelightAprilTagDistance(limelightNumber)
@@ -84,7 +80,7 @@ && getLimelightAprilTagDistance(limelightNumber)
         return limelightEstimates[limelightNumber] = getMegaTag1PoseEstimate(limelightNumber);
       }
     }
-    return new PoseEstimate();
+    return limelightEstimates[limelightNumber] = new PoseEstimate();
   }
 
   /**
@@ -396,5 +392,9 @@ public void periodic() {
     visionThread(VisionConstants.FRONT_LEFT_LIMELIGHT_NUMBER);
     visionThread(VisionConstants.FRONT_RIGHT_LIMELIGHT_NUMBER);
     SmartDashboard.putNumber("april tag dist", getLimelightAprilTagDistance(0));
+    SmartDashboard.putString("shooter ll odom", getPoseFromAprilTags(0).toString());
+    SmartDashboard.putString("left ll odom", getPoseFromAprilTags(1).toString());
+
+    SmartDashboard.putString("right ll odom", getPoseFromAprilTags(2).toString());
   }
 }