Add VisionSubsystem

Author: kcooney

README.md

@@ -56,6 +56,7 @@ To upgrade the version of the lib2813 libraries you are using, simply update the
 - `com.team2813.lib2813:limelight`:
   - `Phoenix6.json`
 - `com.team2813.lib2813:vision`:
+  - `WPILibNewCommands.json`
   - `photonlib.json`
 - `com.team2813.lib2813:testing`:
   - `WPILibNewCommands.json`

vision/src/main/java/com/team2813/lib2813/vision/VisionSubsystem.java

@@ -0,0 +1,75 @@
+package com.team2813.lib2813.vision;
+;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import org.photonvision.EstimatedRobotPose;
+
+/**
+ * Defines a vision subsystem.
+ *
+ * @param <C> the type for the camera
+ */
+public abstract class VisionSubsystem<C extends Camera> extends SubsystemBase {
+  protected final MultiPhotonPoseEstimator<C> poseEstimator;
+
+  /**
+   * Constructor.
+   *
+   * @param poseEstimator The pose estimator to use.
+   */
+  protected VisionSubsystem(MultiPhotonPoseEstimator<C> poseEstimator) {
+    this.poseEstimator = poseEstimator;
+  }
+
+  /**
+   * Constructor.
+   *
+   * @param poseEstimator The pose estimator to use.
+   * @param name Name of the subsystem for telemetry and logging.
+   */
+  protected VisionSubsystem(MultiPhotonPoseEstimator<C> poseEstimator, String name) {
+    super(name);
+    this.poseEstimator = poseEstimator;
+  }
+
+  @Override
+  public void periodic() {
+    if (poseEstimator.poseStrategyRequiresHeadingData()) {
+      poseEstimator.addHeadingData(Timer.getTimestamp(), getRotation3d());
+    }
+    poseEstimator.processAllUnreadResults(this::handleEstimatedPose, this::handleRejectedPose);
+    poseEstimator.publishCameraPosesRelativeTo(getDrivetrainPose());
+  }
+
+  /**
+   * Gets the current orientation of the robot as a {@link Rotation3d} from
+   * the Pigeon 2 quaternion values.
+   *
+   * @return The robot orientation.
+   */
+  protected abstract Rotation3d getRotation3d();
+
+  /**
+   * Gets the current pose of the robot as a {@link Pose2d} from the drivetrain.
+   *
+   * @return The robot position.
+   */
+  protected abstract Pose2d getDrivetrainPose();
+
+  /**
+   * Called when new estimated robot positions are available.
+   *
+   * @param estimatedPose The estimated robot position.
+   * @param camera The camera.
+   */
+  protected abstract void handleEstimatedPose(EstimatedRobotPose estimatedPose, C camera);
+
+  /**
+   * Called when invalid robot positions were computed by the estimator.
+   *
+   * @param rejectedPose The rejected estimated robot position.
+   */
+  protected void handleRejectedPose(EstimatedRobotPose rejectedPose) {}
+}

vision/vendordeps/WPILibNewCommands.json

@@ -0,0 +1,39 @@
+{
+  "fileName": "WPILibNewCommands.json",
+  "name": "WPILib-New-Commands",
+  "version": "1.0.0",
+  "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266",
+  "frcYear": "2026",
+  "mavenUrls": [],
+  "jsonUrl": "",
+  "javaDependencies": [
+    {
+      "groupId": "edu.wpi.first.wpilibNewCommands",
+      "artifactId": "wpilibNewCommands-java",
+      "version": "wpilib"
+    }
+  ],
+  "jniDependencies": [],
+  "cppDependencies": [
+    {
+      "groupId": "edu.wpi.first.wpilibNewCommands",
+      "artifactId": "wpilibNewCommands-cpp",
+      "version": "wpilib",
+      "libName": "wpilibNewCommands",
+      "headerClassifier": "headers",
+      "sourcesClassifier": "sources",
+      "sharedLibrary": true,
+      "skipInvalidPlatforms": true,
+      "binaryPlatforms": [
+        "linuxsystemcore",
+        "linuxathena",
+        "linuxarm32",
+        "linuxarm64",
+        "windowsx86-64",
+        "windowsx86",
+        "linuxx86-64",
+        "osxuniversal"
+      ]
+    }
+  ]
+}