From 0ac737460ae3d33846a45930f4ec1b53b72c19a6 Mon Sep 17 00:00:00 2001
From: aarav18 <aaravshah18@gmail.com>
Date: Sat, 18 Mar 2023 17:23:38 -0600
Subject: [PATCH] sum change

---
 src/main/java/frc4388/robot/Constants.java       |  6 ++++++
 .../java/frc4388/robot/subsystems/Limelight.java | 16 +++++++++++++---
 2 files changed, 19 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index f771c80..af39c6a 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -7,9 +7,14 @@
 
 package frc4388.robot;
 
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.math.util.Units;
 import frc4388.utility.Gains;
 import frc4388.utility.LEDPatterns;
+import frc4388.utility.RobotUnits;
 
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
@@ -168,5 +173,6 @@ public final class Constants {
     public static final double MID_TAPE_HEIGHT = 24.0;
 
     public static final double APRIL_HEIGHT = -1.0; // TODO: find actual value
+
   }
 }
diff --git a/src/main/java/frc4388/robot/subsystems/Limelight.java b/src/main/java/frc4388/robot/subsystems/Limelight.java
index 262b0e7..9d1289b 100644
--- a/src/main/java/frc4388/robot/subsystems/Limelight.java
+++ b/src/main/java/frc4388/robot/subsystems/Limelight.java
@@ -4,21 +4,33 @@
 
 package frc4388.robot.subsystems;
 
+import java.io.IOException;
 import java.util.ArrayList;
 import java.util.List;
+import java.util.Optional;
 
+import org.photonvision.EstimatedRobotPose;
 import org.photonvision.PhotonCamera;
+import org.photonvision.PhotonPoseEstimator;
+import org.photonvision.PhotonPoseEstimator.PoseStrategy;
 import org.photonvision.common.hardware.VisionLEDMode;
 import org.photonvision.targeting.PhotonPipelineResult;
 import org.photonvision.targeting.PhotonTrackedTarget;
 import org.photonvision.targeting.TargetCorner;
 
+import edu.wpi.first.apriltag.AprilTag;
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Constants.VisionConstants;
 
 public class Limelight extends SubsystemBase {
+  
   private PhotonCamera cam;
+  private PhotonPoseEstimator photonPoseEstimator;
 
   private boolean lightOn;
 
@@ -149,7 +161,5 @@ public class Limelight extends SubsystemBase {
   }
 
   @Override
-  public void periodic() {
-    SmartDashboard.putNumber("April Skew", getAprilSkew());
-  }
+  public void periodic() {}
 }