diff --git a/src/main/java/frc4388/robot/subsystems/Limelight.java b/src/main/java/frc4388/robot/subsystems/Limelight.java index 9d1289b..46f115b 100644 --- a/src/main/java/frc4388/robot/subsystems/Limelight.java +++ b/src/main/java/frc4388/robot/subsystems/Limelight.java @@ -130,35 +130,35 @@ public class Limelight extends SubsystemBase { return distanceToApril; } - public PhotonTrackedTarget getLowestTape() { - if (!cam.isConnected()) return null; + // public PhotonTrackedTarget getLowestTape() { + // if (!cam.isConnected()) return null; - PhotonPipelineResult result = cam.getLatestResult(); + // PhotonPipelineResult result = cam.getLatestResult(); - if (!result.hasTargets()) return null; + // if (!result.hasTargets()) return null; - ArrayList points = (ArrayList) result.getTargets(); + // ArrayList points = (ArrayList) result.getTargets(); - PhotonTrackedTarget lowest = points.get(0); - for (PhotonTrackedTarget point : points) { - if (point.getPitch() < lowest.getPitch()) { - lowest = point; - } - } + // PhotonTrackedTarget lowest = points.get(0); + // for (PhotonTrackedTarget point : points) { + // if (point.getPitch() < lowest.getPitch()) { + // lowest = point; + // } + // } - return lowest; - } + // return lowest; + // } - public double getDistanceToTape() { - PhotonTrackedTarget tapePoint = getLowestTape(); - if (tapePoint == null) return -1; + // 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 tapeHeight = VisionConstants.MID_TAPE_HEIGHT - VisionConstants.LIME_HEIGHT; + // double theta = 35.0 + tapePoint.getPitch(); - double distanceToTape = tapeHeight / Math.tan(Math.toRadians(theta)); - return distanceToTape; - } + // double distanceToTape = tapeHeight / Math.tan(Math.toRadians(theta)); + // return distanceToTape; + // } @Override public void periodic() {} diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index 371f621..9917776 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -7,32 +7,41 @@ 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; + private final NetworkTableEntry nt_ll_tx; + private final NetworkTableEntry nt_ll_ty; + private final NetworkTableEntry nt_ll_ta; + private final NetworkTableEntry nt_ll_ts; public Vision() { - final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag"); + final var tagTable = NetworkTableInstance.getDefault().getTable("limelight`"); - m_isTags = tagTable.getEntry("IsTag"); - m_xPoses = tagTable.getEntry("TagPosX"); - m_yPoses = tagTable.getEntry("TagPosY"); - m_zPoses = tagTable.getEntry("TagPosZ"); + nt_ll_tx = tagTable.getEntry("tx"); //TODO + nt_ll_ty = tagTable.getEntry("ty"); //TODO + nt_ll_ta = tagTable.getEntry("ta"); //TODO + nt_ll_ts = tagTable.getEntry("ts"); //TODO + + // I do not know what these values represent + // There are diffrent values that are sent + // when "3D" mode is enabled on the limelight + // So this needs to be updated! } - public AprilTag[] getAprilTags() { - if (!m_isTags.getBoolean(false)) return new AprilTag[0]; + // public AprilTag[] getAprilTags() { This func should return an apriltag object! + public void 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[] {}); + double ll_tx = nt_ll_tx.getDouble(new Double(0)); + double ll_ty = nt_ll_ty.getDouble(new Double(0)); + double ll_ta = nt_ll_ta.getDouble(new Double(0)); + double ll_ts = nt_ll_ts.getDouble(new Double(0)); - 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())); - } + // 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; + // return tags; + + } }