Create vision subsystem

src/main/java/frc/robot/subsystems/limelight/Limelight.java

@@ -0,0 +1,151 @@
+package frc.robot.subsystems.limelight;
+
+import edu.wpi.first.math.geometry.Pose2d;
+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;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.subsystems.limelight.enums.LimelightLEDMode;
+import frc.robot.subsystems.limelight.enums.LimelightStream;
+
+public class Limelight implements LimelightIO {
+  private String name; // The name of the limelight
+  private NetworkTable networkTable;
+
+  /**
+   * Creates a new limelight
+   *
+   * @param name The name of the limelight
+   */
+  public Limelight(String name) {
+    this.name = name;
+
+    // Initializes and sets camera and pipeline of the limelight
+    this.networkTable = NetworkTableInstance.getDefault().getTable(this.name);
+    this.networkTable.getEntry("camera").setNumber(0);
+    this.networkTable.getEntry("pipeline").setNumber(0);
+  }
+
+  @Override
+  public void updateData(LimelightIOData limelightIOData) {
+    NetworkTableEntry robotPoseEntry;
+    double[] dataFromLimelight;
+
+    // Sets the pose of the limelight based on what alliance we are on
+    if (DriverStation.getAlliance().get() == DriverStation.Alliance.Blue) {
+      robotPoseEntry = this.networkTable.getEntry("botpose_wpiblue");
+    } else if (DriverStation.getAlliance().get() == DriverStation.Alliance.Red) {
+      robotPoseEntry = this.networkTable.getEntry("botpose_wpired");
+    } else {
+      robotPoseEntry = this.networkTable.getEntry("botpose");
+    }
+
+    dataFromLimelight = robotPoseEntry.getDoubleArray(new double[7]);
+
+    // Create a 3d pose from data from the limelight
+    Pose3d pose =
+        new Pose3d(
+            dataFromLimelight[0],
+            dataFromLimelight[1],
+            dataFromLimelight[2],
+            new Rotation3d(
+                Math.toRadians(dataFromLimelight[3]),
+                Math.toRadians(dataFromLimelight[4]),
+                Math.toRadians(dataFromLimelight[5]))); // .transformBy(cameraOffset);
+
+    // Set if the limelight is locked onto a target
+    if (this.networkTable.getEntry("tv").getDouble(0) == 1) {
+      limelightIOData.lockedOnTarget = true;
+    } else {
+      limelightIOData.lockedOnTarget = false;
+    }
+
+    // Calculate the latency of the limelight
+    double latency = dataFromLimelight[6] / 1000;
+
+    if (limelightIOData.lockedOnTarget) {
+      // Set the time that the limelight was updated
+      limelightIOData.timestamp = Timer.getFPGATimestamp() - latency;
+
+      limelightIOData.isNewPose = true;
+
+      // Set the target x and y
+      limelightIOData.targetX = this.networkTable.getEntry("tx").getDouble(0);
+      limelightIOData.targetY = this.networkTable.getEntry("ty").getDouble(0);
+
+      // Set the pose of the limelight (x, y, rotation)
+      Pose2d pose2d = pose.toPose2d();
+      limelightIOData.x = pose2d.getX();
+      limelightIOData.y = pose2d.getY();
+      limelightIOData.rotation = pose2d.getRotation().getRadians();
+      limelightIOData.pose = pose2d;
+
+    } else {
+      limelightIOData.isNewPose = false;
+
+      // Set the target x and y to 0 because we don't have a target
+      limelightIOData.targetX = 0;
+      limelightIOData.targetY = 0;
+    }
+  }
+
+  /** Gets the name of a limelight that's registered into the network table */
+  @Override
+  public String getName() {
+    return this.name;
+  }
+
+  /**
+   * Sets the led mode of the limelight
+   *
+   * @param limelightLEDMode The led mode to set the limelight to (see LimelightLEDMode.java for
+   *     options)
+   */
+  public void setLEDMode(LimelightLEDMode limelightLEDMode) {
+    switch (limelightLEDMode) {
+      case PIPELINE_DEFAULT -> this.networkTable.getEntry("ledMode").setNumber(0);
+      case OFF -> this.networkTable.getEntry("ledMode").setNumber(1);
+      case BLINK -> this.networkTable.getEntry("ledMode").setNumber(2);
+      case ON -> this.networkTable.getEntry("ledMode").setNumber(3);
+      default -> this.networkTable.getEntry("ledMode").setNumber(0); // Default to pipeline default
+    }
+  }
+
+  /**
+   * Sets the streaming mode of the limelight
+   *
+   * @param stream The streaming mode to set the limelight to as a limelight stream type
+   */
+  public void setStream(LimelightStream limelightStream) {
+    switch (limelightStream) {
+      case STANDARD -> this.networkTable.getEntry("stream").setNumber(0);
+      case PIP_MAIN -> this.networkTable.getEntry("stream").setNumber(1);
+      case PIP_SECONDARY -> this.networkTable.getEntry("stream").setNumber(2);
+      default -> this.networkTable.getEntry("stream").setNumber(0); // Default to standard
+    }
+  }
+
+  /**
+   * Sets the pipeline of the limelight based on our presets for it
+   *
+   * @param pipeline The pipeline to set the limelight to as an integer
+   */
+  public void setPipeline(int pipeline) {
+    this.networkTable.getEntry("pipeline").setNumber(pipeline);
+  }
+
+  /**
+   * Displays the pose values of the limelight on the smart dashboard
+   *
+   * @param pose The pose to retrieve and display the values from
+   */
+  public void displayLimelightPoseValues(Pose2d pose) {
+    SmartDashboard.putNumber(this.name + " x", pose.getX());
+    SmartDashboard.putNumber(this.name + " y", pose.getY());
+    SmartDashboard.putNumber(this.name + " angleDegrees", pose.getRotation().getDegrees());
+  }
+}

src/main/java/frc/robot/subsystems/limelight/LimelightConstants.java

@@ -0,0 +1,7 @@
+package frc.robot.subsystems.limelight;
+
+import edu.wpi.first.math.util.Units;
+
+public class LimelightConstants {
+  public static final double LOWEST_DISTANCE = Units.feetToMeters(10.0);
+}

src/main/java/frc/robot/subsystems/limelight/LimelightIO.java

@@ -0,0 +1,52 @@
+package frc.robot.subsystems.limelight;
+
+import edu.wpi.first.math.geometry.Pose2d;
+
+public interface LimelightIO {
+
+  /** Data that the limelight will collect and track */
+  class LimelightIOData {
+    public double x; // x position of the limelight itself
+    public double y; // y position of the limelight itself
+    public double rotation; // rotation of the limelight itself
+
+    public double targetX; // x position of the target
+    public double targetY; // y position of the target
+
+    public boolean isNewPose;
+    public Pose2d pose;
+    public double timestamp;
+
+    public double maxDistance;
+    public double minDistance;
+
+    public boolean lockedOnTarget = false;
+    public int singleIDUsed;
+
+    public double translationToTargetX;
+    public double translationToTargetY;
+  }
+
+  /**
+   * Updates the data of the limelight
+   *
+   * @param data
+   */
+  default void updateData(LimelightIOData data) {}
+
+  /**
+   * Gets, or sets the name of the limelight
+   *
+   * @return
+   */
+  default String getName() {
+    return "";
+  }
+
+  /**
+   * Sets the refrence pose of the limelight
+   *
+   * @param pose
+   */
+  default void setRefrencePose(Pose2d pose) {}
+}

src/main/java/frc/robot/subsystems/limelight/LimelightPoseAndTimestamp.java

@@ -0,0 +1,21 @@
+package frc.robot.subsystems.limelight;
+
+import edu.wpi.first.math.geometry.Pose2d;
+
+public class LimelightPoseAndTimestamp {
+  Pose2d pose;
+  double timestamp;
+
+  public LimelightPoseAndTimestamp(Pose2d pose, double timestamp) {
+    this.pose = pose;
+    this.timestamp = timestamp;
+  }
+
+  public Pose2d getPose() {
+    return this.pose;
+  }
+
+  public double getTimestamp() {
+    return this.timestamp;
+  }
+}

src/main/java/frc/robot/subsystems/limelight/LimelightSubsystem.java

@@ -0,0 +1,100 @@
+package frc.robot.subsystems.limelight;
+
+import java.util.ArrayList;
+import java.util.List;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.subsystems.limelight.LimelightIO.LimelightIOData;
+
+public class LimelightSubsystem extends SubsystemBase {
+  private final Limelight[] limelights;
+  private final LimelightIOData[] limelightData;
+
+  private final List<LimelightPoseAndTimestamp> results = new ArrayList<>();
+
+  private int acceptableTagID;
+  private boolean useSingleTag = false;
+
+  /**
+   * Initializes cameras and input loggers
+   *
+   * @param cameras Array of cameras being used
+   */
+  public LimelightSubsystem(Limelight[] limelights) {
+    this.limelights = limelights;
+    this.limelightData = new LimelightIOData[this.limelights.length];
+
+    for (int i = 0; i < this.limelights.length; i++) {
+      this.limelightData[i] = new LimelightIOData();
+    }
+  }
+
+  @Override
+  public void periodic() {
+    // Clear results from last periodic
+    this.results.clear();
+
+    for (int i = 0; i < this.limelightData.length; i++) {
+      // update and process new inputs
+      this.limelights[i].updateData(limelightData[i]);
+
+      if (this.limelightData[i].lockedOnTarget
+          && this.limelightData[i].isNewPose
+          && !DriverStation.isAutonomous()
+          && this.limelightData[i].maxDistance < LimelightConstants.LOWEST_DISTANCE) {
+        if (this.useSingleTag) {
+          if (this.limelightData[i].singleIDUsed == this.acceptableTagID) {
+            this.processLimelight(i);
+          }
+        } else {
+          this.processLimelight(i);
+        }
+      }
+    }
+  }
+
+  /**
+   * Processes the limelight data and adds the pose to the list of poses
+   *
+   * @param limelightNumber The numerical ID of the limelight
+   */
+  public void processLimelight(int limelightNumber) {
+    // Create a new pose based off the new limelight data
+    Pose2d currentPose =
+        new Pose2d(
+            this.limelightData[limelightNumber].x,
+            this.limelightData[limelightNumber].y,
+            new Rotation2d(this.limelightData[limelightNumber].rotation));
+
+    // Add the new pose to the list of poses
+    this.results.add(
+        new LimelightPoseAndTimestamp(currentPose, limelightData[limelightNumber].timestamp));
+  }
+
+  /** Returns the last recorded pose */
+  public List<LimelightPoseAndTimestamp> getLimelightOdometry() {
+    return this.results;
+  }
+
+  public void setUseSingleTag(boolean useSingleTag) {
+    this.setUseSingleTag(useSingleTag, 0);
+  }
+
+  public void setUseSingleTag(boolean useSingleTag, int acceptableTagID) {
+    this.useSingleTag = useSingleTag;
+    this.acceptableTagID = acceptableTagID;
+  }
+
+  public void setReferencePose(Pose2d pose) {
+    for (Limelight limelight : this.limelights) {
+      limelight.setRefrencePose(pose);
+    }
+  }
+
+  public double getMinDistance(int camera) {
+    return this.limelightData[camera].minDistance;
+  }
+}

src/main/java/frc/robot/subsystems/limelight/enums/LimelightLEDMode.java

@@ -0,0 +1,8 @@
+package frc.robot.subsystems.limelight.enums;
+
+public enum LimelightLEDMode {
+  PIPELINE_DEFAULT, // 0 in the network table
+  OFF, // 1 in the network table
+  BLINK, // 2 in the network table
+  ON // 3 in the network table
+}

src/main/java/frc/robot/subsystems/limelight/enums/LimelightStream.java

@@ -0,0 +1,9 @@
+package frc.robot.subsystems.limelight.enums;
+
+public enum LimelightStream {
+  STANDARD, // 0 in the network table; side-by-side streams if a webcam is attached to Limelight
+  PIP_MAIN, // 1 in the network table; secondary camera stream in the lower-right corner of the
+            // primary camera stream
+  PIP_SECONDARY // 2 in the network table; primary camera stream in the lower-right corner of the
+                // secondary camera stream
+}