Merge pull request #37 from Team4388/new-features

New features from 2023

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;
 

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

@@ -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<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() {}
+}

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;
+    }
+}

src/main/java/frc4388/utility/AprilTag.java

@@ -0,0 +1,13 @@
+package frc4388.utility;
+
+// This is a seperate class in case I want to encode rotation or other
+// information about the tag
+public class AprilTag {
+    public final double x, y, z;
+
+    public AprilTag(double _x, double _y, double _z) {
+        x = _x;
+        y = _y;
+        z = _z;
+    }
+}

src/main/java/frc4388/utility/DeferredBlock.java

@@ -0,0 +1,23 @@
+package frc4388.utility;
+
+import java.util.ArrayList;
+
+public class DeferredBlock {
+    private static ArrayList<Runnable> m_blocks = new ArrayList<>();
+    private static boolean             m_hasRun = false;
+
+    public DeferredBlock(Runnable block) {
+        m_blocks.add(block);
+    }
+
+    public static void execute() {
+        if (m_hasRun) return;
+
+        for (Runnable block : m_blocks) {
+            block.run();
+        }
+
+        m_blocks.clear(); // for garbage collection
+        m_hasRun = true;
+    }
+}

src/main/java/frc4388/utility/UtilityStructs.java

@@ -0,0 +1,12 @@
+package frc4388.utility;
+
+public class UtilityStructs {
+    public static class TimedOutput {
+        public double leftX  = 0.0;
+        public double leftY  = 0.0;
+        public double rightX = 0.0;
+        public double rightY = 0.0;
+    
+        public long timedOffset = 0;
+    }
+}

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"
+        }
+    ]
+}