Merge pull request #66 from TitaniumTigers4829/port-forwarding-insert…

…-question-mark-here

port forwarding? for da limelights

src/main/java/frc/robot/subsystems/vision/PhysicalVision.java

@@ -1,6 +1,7 @@
 package frc.robot.subsystems.vision;
 
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.net.PortForwarder;
 import edu.wpi.first.wpilibj.DriverStation;
 import frc.robot.Constants.FieldConstants;
 import frc.robot.extras.util.GeomUtil;
@@ -43,7 +44,9 @@ public class PhysicalVision implements VisionInterface {
 
   public PhysicalVision() {
     for (Limelight limelight : Limelight.values()) {
-      // Start a vision input task for each Limelight
+      // Setupt port forwarding for each limelight
+      setupPortForwarding(limelight);
+      // Start a threaded task to check and update the pose for each Limelight
       threadManager.startTask(
           limelight.getName(),
           () -> checkAndUpdatePose(limelight),
@@ -132,6 +135,26 @@ public boolean isValidMeasurement(Limelight limelight) {
     return isValidPoseEstimate(limelight) && isConfident(limelight) && !isTeleporting(limelight);
   }
 
+  /**
+   * Sets up port forwarding for the specified Limelight. This method forwards a range of ports from
+   * the robot to the Limelight, allowing network communication between the robot and the Limelight.
+   *
+   * <p>Each Limelight is assigned a unique port offset based on its ID. The method forwards ports
+   * 5800 to 5809 for each Limelight, with the port offset applied to each port number. For example,
+   * if the Limelight ID is 1, the ports 5810 to 5819 will be forwarded.
+   *
+   * @param limelight The Limelight for which to set up port forwarding.
+   */
+  private void setupPortForwarding(Limelight limelight) {
+    int portOffset = VisionConstants.PORT_OFFSET * limelight.getId();
+    for (int port = VisionConstants.BASE_PORT;
+        port < VisionConstants.BASE_PORT + VisionConstants.PORT_RANGE;
+        port++) {
+      PortForwarder.add(
+          port + portOffset, limelight.getName() + VisionConstants.LIMELIGHT_DOMAIN, port);
+    }
+  }
+
   /**
    * Gets the pose update of the specified limelight while the robot is enabled.
    *

src/main/java/frc/robot/subsystems/vision/VisionConstants.java

@@ -97,6 +97,12 @@ public static Limelight fromId(int id) {
   public static final String FRONT_RIGHT_LIMELIGHT_NAME = "limelight-right";
   public static final int FRONT_RIGHT_LIMELIGHT_NUMBER = 2;
 
+  // Constants for port forwarding
+  public static final int BASE_PORT = 5800;
+  public static final int PORT_RANGE = 10;
+  public static final int PORT_OFFSET = 10;
+  public static final String LIMELIGHT_DOMAIN = ".local";
+
   public static final double[][] ONE_APRIL_TAG_LOOKUP_TABLE = {
     // {distance in meters, x std deviation, y std deviation, r (in degrees) std deviation}
     {0, 0.02, 0.02, Units.degreesToRadians(180000)}, // 2