Revert "Make boiler plate code - Needs to be programmed with robot"

This reverts commit 0f7861ff02.

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

@@ -0,0 +1,36 @@
+package frc4388.robot.subsystems;
+
+//import edu.wpi.first.apriltag.AprilTag;
+//import edu.wpi.first.math.geometry.Pose3d;
+//import edu.wpi.first.math.geometry.Rotation3d;
+//import edu.wpi.first.networktables.NetworkTable;
+//import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+
+public class Apriltags {
+    public static class Tag {
+        public boolean visible = true;
+        public double x,  y,  z  = 0;
+        public double ry, rp, rr = 0;
+    }
+
+    public Tag getTagPosRot() {
+        final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag");
+
+        final Tag tag = new Tag();
+        tag.visible = isAprilTag();
+        tag.x  = tagTable.getEntry("TagPosX").getDouble(0);
+        tag.y  = tagTable.getEntry("TagPosY").getDouble(0);
+        tag.z  = tagTable.getEntry("TagPosZ").getDouble(0);
+        tag.ry = tagTable.getEntry("TagRotY").getDouble(0);
+        tag.rp = tagTable.getEntry("TagRotP").getDouble(0);
+        tag.rr = tagTable.getEntry("TagRotR").getDouble(0);
+
+        return tag;
+    }
+
+    public boolean isAprilTag() {
+        final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag");
+        return tagTable.getEntry("IsTag").getBoolean(false);
+    }
+}

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

@@ -3,33 +3,163 @@
 // the WPILib BSD license file in the root directory of this project.
 
 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.math.geometry.Rotation2d;
-import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
-// Look at vvv for networktables stuff
-// https://docs.limelightvision.io/docs/docs-limelight/apis/complete-networktables-api#apriltag-and-3d-data
+import frc4388.robot.Constants.VisionConstants;
 
 public class Limelight extends SubsystemBase {
-  private double[] cameraPose;
+  
+  private PhotonCamera cam;
+  private PhotonPoseEstimator photonPoseEstimator;
 
-  public Pose2d getPose() {
-    //TODO - Get actual values!
+  private boolean lightOn;
 
-    double x = 0;
-    double y = 0;
-    double yaw = 0;
+  /** Creates a new Limelight. */
+  public Limelight() {
+    cam = new PhotonCamera(VisionConstants.NAME);
+    cam.setDriverMode(false);
+  }
 
-    Rotation2d rot = Rotation2d.fromDegrees(yaw);
+  public void setLEDs(boolean on) {
+    lightOn = on;
+    cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff);
+  }
 
-    return new Pose2d(x, y, rot);
+  public void toggleLEDs() {
+    lightOn = !lightOn;
+    cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff);
+  }
+
+  public void setDriverMode(boolean driverMode) {
+    cam.setDriverMode(driverMode);
+  }
+
+  public void setToLimePipeline() {
+    cam.setPipelineIndex(1);
+    setLEDs(true);
+  }
+
+  public void setToAprilPipeline() {
+    cam.setPipelineIndex(0);
+    setLEDs(false);
+  }
+
+  public PhotonTrackedTarget getAprilPoint() {
+    if (!cam.isConnected()) return null;
+
+    PhotonPipelineResult result = cam.getLatestResult();
+
+    if (!result.hasTargets()) return null;
+
+    return result.getBestTarget();
+  }
+
+  private List<TargetCorner> getAprilCorners() {
+    if (!cam.isConnected()) return null;
+
+    PhotonPipelineResult result = cam.getLatestResult();
+
+    if (!result.hasTargets()) return null;
+
+    return result.getBestTarget().getDetectedCorners();
+  }
+
+  public double getAprilSkew() {
+    List<TargetCorner> corners = getAprilCorners();
+    ArrayList<TargetCorner> bottomSide = getAprilBottomSide(corners);
+
+    if (bottomSide == null) return 0;
+
+    TargetCorner bottomRight = bottomSide.get(0).x > bottomSide.get(1).x ? bottomSide.get(0) : bottomSide.get(1);
+    TargetCorner bottomLeft = bottomRight.x == bottomSide.get(0).x ? bottomSide.get(1) : bottomSide.get(0);
+
+    return bottomLeft.y - bottomRight.y;
+  }
+
+  private ArrayList<TargetCorner> getAprilBottomSide(List<TargetCorner> box) {
+    if (box == null) return null;
+
+    ArrayList<TargetCorner> bottomSide = new ArrayList<>();
+
+    TargetCorner l1 = new TargetCorner(-1, -1);
+    TargetCorner l2 = new TargetCorner(-1, -1);
+    
+    for (TargetCorner c : box) {
+      if (c.y > l1.y) l1 = c;
+    }
+
+    for (TargetCorner c : box) {
+      if (c.y == l1.y) continue;
+      if (c.y > l2.y) l2 = c;
+    }
+
+    bottomSide.add(l1);
+    bottomSide.add(l2);
+
+    return bottomSide;
+  }
+
+  public double getDistanceToApril() {    
+    PhotonTrackedTarget aprilPoint = getAprilPoint();
+    if (aprilPoint == null) return -1;
+
+    double aprilHeight = VisionConstants.APRIL_HEIGHT - VisionConstants.LIME_HEIGHT;
+    double theta = 35.0 + aprilPoint.getPitch();
+
+    double distanceToApril = aprilHeight / Math.tan(Math.toRadians(theta));
+    return distanceToApril;
+  }
+
+  public PhotonTrackedTarget getLowestTape() {
+    if (!cam.isConnected()) return null;
+
+    PhotonPipelineResult result = cam.getLatestResult();
+
+    if (!result.hasTargets()) return null;
+
+    ArrayList<PhotonTrackedTarget> points = (ArrayList<PhotonTrackedTarget>) result.getTargets();
+
+    PhotonTrackedTarget lowest = points.get(0);
+    for (PhotonTrackedTarget point : points) {
+      if (point.getPitch() < lowest.getPitch()) {
+        lowest = point;
+      }
+    }
+
+    return lowest;
+  }
+
+  public double getDistanceToTape() {    
+    PhotonTrackedTarget tapePoint = getLowestTape();
+    if (tapePoint == null) return -1;
+
+    double tapeHeight = VisionConstants.MID_TAPE_HEIGHT - VisionConstants.LIME_HEIGHT;
+    double theta = 35.0 + tapePoint.getPitch();
+
+    double distanceToTape = tapeHeight / Math.tan(Math.toRadians(theta));
+    return distanceToTape;
   }
 
   @Override
-  public void periodic() {
-    // This method will be called once per scheduler run
-    cameraPose = NetworkTableInstance.getDefault().getTable("limelight").getEntry("camerapose_targetspace").getDoubleArray(new double[6]);
-  }
-}
+  public void periodic() {}
+}

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

@@ -0,0 +1,38 @@
+package frc4388.robot.subsystems;
+
+import edu.wpi.first.apriltag.AprilTag;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+
+public class Vision {
+    private final NetworkTableEntry m_isTags;
+    private final NetworkTableEntry m_xPoses;
+    private final NetworkTableEntry m_yPoses;
+    private final NetworkTableEntry m_zPoses;
+
+    public Vision() {
+        final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag");
+
+        m_isTags = tagTable.getEntry("IsTag");
+        m_xPoses = tagTable.getEntry("TagPosX");
+        m_yPoses = tagTable.getEntry("TagPosY");
+        m_zPoses = tagTable.getEntry("TagPosZ");
+    }
+
+    public AprilTag[] getAprilTags() {
+        if (!m_isTags.getBoolean(false)) return new AprilTag[0];
+
+        double xarr[] = m_xPoses.getDoubleArray(new double[] {});
+        double yarr[] = m_yPoses.getDoubleArray(new double[] {});
+        double zarr[] = m_zPoses.getDoubleArray(new double[] {});
+
+        AprilTag tags[] = new AprilTag[xarr.length];
+        for (int i = 0; i < tags.length; i++) {
+            tags[i] = new AprilTag(0, new Pose3d(xarr[i], yarr[i], zarr[i], new Rotation3d()));
+        }
+
+        return tags;
+    }
+}