From 9ebe31f406ab3d2e5de14be4dfa1c008588c1985 Mon Sep 17 00:00:00 2001 From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com> Date: Fri, 5 Jan 2024 14:53:16 -0700 Subject: [PATCH] Vision Code --- src/main/java/frc4388/robot/Constants.java | 22 +++ .../frc4388/robot/subsystems/Apriltags.java | 36 ++++ .../frc4388/robot/subsystems/Limelight.java | 165 ++++++++++++++++++ .../java/frc4388/robot/subsystems/Vision.java | 38 ++++ vendordeps/photonlib.json | 42 +++++ 5 files changed, 303 insertions(+) create mode 100644 src/main/java/frc4388/robot/subsystems/Apriltags.java create mode 100644 src/main/java/frc4388/robot/subsystems/Limelight.java create mode 100644 src/main/java/frc4388/robot/subsystems/Vision.java create mode 100644 vendordeps/photonlib.json diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 7077079..a58a1d0 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -111,6 +111,28 @@ public final class Constants { public static final int SMARTDASHBOARD_UPDATE_FRAME = 2; } + public static final class VisionConstants { + public static final String NAME = "photonCamera"; + + public static final int LIME_HIXELS = 640; + public static final int LIME_VIXELS = 480; + + public static final double H_FOV = 59.6; + public static final double V_FOV = 45.7; + + public static final double LIME_HEIGHT = 6.0; + public static final double LIME_ANGLE = 55.0; + + // public static final double HIGH_TARGET_HEIGHT = 46.0; + public static final double HIGH_TAPE_HEIGHT = 44.0; + + // public static final double MID_TARGET_HEIGHT = 34.0; + public static final double MID_TAPE_HEIGHT = 24.0; + + public static final double APRIL_HEIGHT = -1.0; // TODO: find actual value + + } + public static final class DriveConstants { public static final int DRIVE_PIGEON_ID = 6; diff --git a/src/main/java/frc4388/robot/subsystems/Apriltags.java b/src/main/java/frc4388/robot/subsystems/Apriltags.java new file mode 100644 index 0000000..c6062e8 --- /dev/null +++ b/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); + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Limelight.java b/src/main/java/frc4388/robot/subsystems/Limelight.java new file mode 100644 index 0000000..9d1289b --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Limelight.java @@ -0,0 +1,165 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// 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.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; + + /** Creates a new Limelight. */ + public Limelight() { + cam = new PhotonCamera(VisionConstants.NAME); + cam.setDriverMode(false); + } + + public void setLEDs(boolean on) { + lightOn = on; + cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff); + } + + 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 getAprilCorners() { + if (!cam.isConnected()) return null; + + PhotonPipelineResult result = cam.getLatestResult(); + + if (!result.hasTargets()) return null; + + return result.getBestTarget().getDetectedCorners(); + } + + public double getAprilSkew() { + List corners = getAprilCorners(); + ArrayList 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 getAprilBottomSide(List box) { + if (box == null) return null; + + ArrayList 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 points = (ArrayList) 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() {} +} diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java new file mode 100644 index 0000000..371f621 --- /dev/null +++ b/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; + } +} diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json new file mode 100644 index 0000000..c940b75 --- /dev/null +++ b/vendordeps/photonlib.json @@ -0,0 +1,42 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2024.1.1-beta-3.2", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", + "frcYear": "2024", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal", + "https://maven.photonvision.org/repository/snapshots" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/PhotonLib-json/1.0/PhotonLib-json-1.0.json", + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "PhotonLib-cpp", + "version": "v2024.1.1-beta-3.2", + "libName": "Photon", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "PhotonLib-java", + "version": "v2024.1.1-beta-3.2" + }, + { + "groupId": "org.photonvision", + "artifactId": "PhotonTargeting-java", + "version": "v2024.1.1-beta-3.2" + } + ] +} \ No newline at end of file