Create vision subsystem

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

@@ -0,0 +1,142 @@
+package frc.robot.subsystems.limelight;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
+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 limelightName;
+  private NetworkTable networkTable;
+  private LimelightPoseData limelightPoseData;
+
+  /**
+   * Creates a new limelight
+   *
+   * @param limelightName The name of the limelight
+   */
+  public Limelight(String limelightName) {
+    this.limelightName = limelightName;
+
+    // Initializes and sets camera and pipeline of the limelight
+    this.networkTable = NetworkTableInstance.getDefault().getTable(this.limelightName);
+    this.networkTable.getEntry(LimelightConstants.TableConstants.CAMERA).setNumber(0);
+    this.networkTable.getEntry(LimelightConstants.TableConstants.PIPELINE).setNumber(0);
+  }
+
+  @Override
+  public void updateData(LimelightIOData limelightIOData) {
+    NetworkTableEntry robotPoseEntry;
+
+    // Creates a new limelight pose data instance if it doesn't exist
+    if (this.limelightPoseData == null) {
+      this.limelightPoseData = new LimelightPoseData(new double[7]);
+    }
+
+    // Sets the pose of the limelight based on what alliance we are on
+    if (DriverStation.getAlliance().get() == DriverStation.Alliance.Blue) {
+      robotPoseEntry = this.networkTable.getEntry(LimelightConstants.TableConstants.BOT_POSE_BLUE);
+    } else if (DriverStation.getAlliance().get() == DriverStation.Alliance.Red) {
+      robotPoseEntry = this.networkTable.getEntry(LimelightConstants.TableConstants.BOT_POSE_RED);
+    } else {
+      robotPoseEntry = this.networkTable.getEntry(LimelightConstants.TableConstants.BOT_POSE);
+    }
+
+    // Update the limelight pose data
+    this.limelightPoseData.update(robotPoseEntry.getDoubleArray(new double[7]));
+
+    // Create a 3d pose from data from the limelight
+    Pose3d limelightPose = limelightPoseData.toPose3d();
+
+    // Set if the limelight is locked onto a target
+    if (this.networkTable.getEntry(LimelightConstants.TableConstants.VALID_TARGET).getDouble(0)
+        == 1) {
+      limelightIOData.lockedOnTarget = true;
+    } else {
+      limelightIOData.lockedOnTarget = false;
+    }
+
+    if (limelightIOData.lockedOnTarget) {
+      // Set the time that the limelight was updated
+      limelightIOData.limelightLastUpdated =
+          Timer.getFPGATimestamp() - limelightPoseData.getTotalLatency();
+
+      limelightIOData.isNewPose = true;
+
+      // Set the target x and y
+      limelightIOData.targetX =
+          this.networkTable.getEntry(LimelightConstants.TableConstants.TARGET_X).getDouble(0);
+      limelightIOData.targetY =
+          this.networkTable.getEntry(LimelightConstants.TableConstants.TARGET_Y).getDouble(0);
+
+      // Set the pose of the limelight (x, y, rotation)
+      Pose2d pose2d = limelightPose.toPose2d();
+      limelightIOData.limelightX = pose2d.getX();
+      limelightIOData.limelightY = pose2d.getY();
+      limelightIOData.limelightRotation = pose2d.getRotation().getRadians();
+      limelightIOData.limelightPose = 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.limelightName;
+  }
+
+  /**
+   * 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) {
+    this.networkTable
+        .getEntry(LimelightConstants.TableConstants.LED_MODE)
+        .setNumber(limelightLEDMode.getNetworkTableValue());
+  }
+
+  /**
+   * 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) {
+    this.networkTable
+        .getEntry(LimelightConstants.TableConstants.STREAM)
+        .setNumber(limelightStream.getNetworkTableValue());
+  }
+
+  /**
+   * 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(LimelightConstants.TableConstants.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.limelightName + " x", pose.getX());
+    SmartDashboard.putNumber(this.limelightName + " y", pose.getY());
+    SmartDashboard.putNumber(this.limelightName + " angleDegrees", pose.getRotation().getDegrees());
+  }
+}

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

@@ -0,0 +1,29 @@
+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);
+
+  static class TableConstants {
+    /**
+     * All tables for limelights can be found here
+     * https://docs.limelightvision.io/docs/docs-limelight/apis/complete-networktables-api
+     */
+    public static final String CAMERA = "camera";
+
+    public static final String PIPELINE = "pipeline";
+    public static final String LED_MODE = "ledMode";
+    public static final String STREAM = "stream";
+
+    // Driverstation/botposes
+    public static final String BOT_POSE_BLUE = "botpose_wpiblue";
+    public static final String BOT_POSE_RED = "botpose_wpired";
+    public static final String BOT_POSE = "botpose";
+
+    // Targeting related
+    public static final String VALID_TARGET = "tv";
+    public static final String TARGET_X = "tx";
+    public static final String TARGET_Y = "ty";
+  }
+}

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 limelightX;
+    public double limelightY;
+    public double limelightRotation;
+
+    public double targetX;
+    public double targetY;
+
+    public boolean isNewPose;
+    public Pose2d limelightPose;
+    public double limelightLastUpdated;
+
+    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 limelightPose;
+  double lastUpdatedTimestamp;
+
+  public LimelightPoseAndTimestamp(Pose2d limelightPose, double lastUpdatedTimestamp) {
+    this.limelightPose = limelightPose;
+    this.lastUpdatedTimestamp = lastUpdatedTimestamp;
+  }
+
+  public Pose2d getLimelightPose() {
+    return this.limelightPose;
+  }
+
+  public double getLastUpdatedTimestamp() {
+    return this.lastUpdatedTimestamp;
+  }
+}

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

@@ -0,0 +1,116 @@
+package frc.robot.subsystems.limelight;
+
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+
+public class LimelightPoseData {
+  private double x;
+  private double y;
+  private double z;
+  private double roll;
+  private double pitch;
+  private double yaw;
+  private double totalLatency;
+
+  public LimelightPoseData(double[] data) {
+    if (data.length != 7) {
+      throw new IllegalArgumentException("Data array must have 7 elements");
+    }
+
+    this.x = data[0];
+    this.y = data[1];
+    this.z = data[2];
+    this.roll = data[3];
+    this.pitch = data[4];
+    this.yaw = data[5];
+    this.totalLatency = data[6];
+  }
+
+  /**
+   * Updates the pose data without creating a new instance of limelight pose data
+   *
+   * @param data The data to update the pose data with
+   */
+  public void update(double[] data) {
+    if (data.length != 7) {
+      throw new IllegalArgumentException("Data array must have 7 elements");
+    }
+
+    this.x = data[0];
+    this.y = data[1];
+    this.z = data[2];
+    this.roll = data[3];
+    this.pitch = data[4];
+    this.yaw = data[5];
+    this.totalLatency = data[6];
+  }
+
+  /**
+   * Gets the x position of the limelight
+   *
+   * @return The x position of the limelight
+   */
+  public double getX() {
+    return this.x;
+  }
+
+  /**
+   * Gets the y position of the limelight
+   *
+   * @return The y position of the limelight
+   */
+  public double getY() {
+    return this.y;
+  }
+
+  /**
+   * Gets the z position of the limelight
+   *
+   * @return The z position of the limelight
+   */
+  public double getZ() {
+    return this.z;
+  }
+
+  /**
+   * Gets a 3d rotation position of the limelight
+   *
+   * @return The 3d rotation position of the limelight
+   */
+  public Rotation3d getRotation() {
+    return new Rotation3d(
+        Math.toRadians(this.roll), Math.toRadians(this.pitch), Math.toRadians(this.yaw));
+  }
+
+  /**
+   * Gets the raw latency of the limelight straight from the network table
+   *
+   * @return The raw latency of the limelight straight from the network table
+   */
+  public double getRawTotalLatency() {
+    return this.totalLatency;
+  }
+
+  /**
+   * Gets the total latency of the limelight in seconds
+   *
+   * @return The total latency of the limelight in seconds
+   */
+  public double getTotalLatency() {
+    return this.totalLatency / 1000;
+  }
+
+  /**
+   * Converts the limelight pose data to a 3d pose
+   *
+   * @return A 3d pose of the limelight pose data
+   */
+  public Pose3d toPose3d() {
+    return new Pose3d(
+        this.x,
+        this.y,
+        this.z,
+        new Rotation3d(
+            Math.toRadians(this.roll), Math.toRadians(this.pitch), Math.toRadians(this.yaw)));
+  }
+}