delete april tag positions array (#83)

TitaniumTigers4829 · Dec 2, 2024 · 23c692f · 23c692f
1 parent c585796
commit 23c692f
Show file tree

Hide file tree

Showing 2 changed files with 4 additions and 49 deletions.
diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java
@@ -24,53 +24,8 @@ public final class VisionConstants {
   public static final String FRONT_RIGHT_LIMELIGHT_NAME = "limelight-right";
   public static final int FRONT_RIGHT_LIMELIGHT_NUMBER = 2;
 
-  public static final double[][] APRIL_TAG_POSITIONS = {
-    // {x, y, z, rotation (degrees)}
-    {
-      Units.inchesToMeters(593.68), Units.inchesToMeters(9.68), Units.inchesToMeters(53.38), 120
-    }, // 1
-    {
-      Units.inchesToMeters(637.21), Units.inchesToMeters(34.79), Units.inchesToMeters(53.38), 120
-    }, // 2
-    {
-      Units.inchesToMeters(652.73), Units.inchesToMeters(196.17), Units.inchesToMeters(57.13), 180
-    }, // 3
-    {
-      Units.inchesToMeters(652.73), Units.inchesToMeters(218.42), Units.inchesToMeters(57.13), 180
-    }, // 4
-    {
-      Units.inchesToMeters(578.77), Units.inchesToMeters(323.0), Units.inchesToMeters(53.38), 270
-    }, // 5
-    {
-      Units.inchesToMeters(72.5), Units.inchesToMeters(323.0), Units.inchesToMeters(53.38), 270
-    }, // 6
-    {-Units.inchesToMeters(1.5), Units.inchesToMeters(218.42), Units.inchesToMeters(57.13), 0}, // 7
-    {-Units.inchesToMeters(1.5), Units.inchesToMeters(196.17), Units.inchesToMeters(57.13), 0}, // 8
-    {
-      Units.inchesToMeters(14.02), Units.inchesToMeters(34.79), Units.inchesToMeters(53.38), 60
-    }, // 9
-    {
-      Units.inchesToMeters(57.54), Units.inchesToMeters(9.68), Units.inchesToMeters(53.38), 60
-    }, // 10
-    {
-      Units.inchesToMeters(468.69), Units.inchesToMeters(146.19), Units.inchesToMeters(52.0), 300
-    }, // 11
-    {
-      Units.inchesToMeters(468.69), Units.inchesToMeters(177.1), Units.inchesToMeters(52.0), 60
-    }, // 12
-    {
-      Units.inchesToMeters(441.74), Units.inchesToMeters(161.62), Units.inchesToMeters(52.0), 180
-    }, // 13
-    {
-      Units.inchesToMeters(209.48), Units.inchesToMeters(161.62), Units.inchesToMeters(52.0), 0
-    }, // 14
-    {
-      Units.inchesToMeters(182.73), Units.inchesToMeters(177.1), Units.inchesToMeters(52.0), 120
-    }, // 15
-    {
-      Units.inchesToMeters(182.73), Units.inchesToMeters(146.19), Units.inchesToMeters(52.0), 240
-    }, // 16
-  };
+  public static final int MIN_APRIL_TAG_ID = 1;
+  public static final int MAX_APRIL_TAG_ID = 16;
 
   public static final double[][] ONE_APRIL_TAG_LOOKUP_TABLE = {
     // {distance in meters, x std deviation, y std deviation, r (in degrees) std deviation}

diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOReal.java b/src/main/java/frc/robot/subsystems/vision/VisionIOReal.java
@@ -71,8 +71,8 @@ public void updateInputs(VisionIOInputs inputs) {
   public boolean canSeeAprilTags(int limelightNumber) {
     // First checks if it can see an april tag, then checks if it is fully in frame
     // Different Limelights have different FOVs
-    if (getNumberOfAprilTags(limelightNumber) > 0
-        && getNumberOfAprilTags(limelightNumber) <= VisionConstants.APRIL_TAG_POSITIONS.length) {
+    if (getNumberOfAprilTags(limelightNumber) >= VisionConstants.MIN_APRIL_TAG_ID
+        && getNumberOfAprilTags(limelightNumber) <= VisionConstants.MAX_APRIL_TAG_ID) {
       if (getLimelightName(limelightNumber).equals(VisionConstants.SHOOTER_LIMELIGHT_NAME)) {
         return Math.abs(LimelightHelpers.getTX(getLimelightName(limelightNumber)))
             <= VisionConstants.LL3G_FOV_MARGIN_OF_ERROR;