diff --git a/AdvantageScope Swerve Calibration.json b/AdvantageScope Swerve Calibration.json index a970739d..6aa1547a 100644 --- a/AdvantageScope Swerve Calibration.json +++ b/AdvantageScope Swerve Calibration.json @@ -122,7 +122,7 @@ } } ], - "game": "2024 Field", + "game": "2025 Field", "origin": "blue" }, "controllerUUID": "psf0y633oclnjyocus23hcnq1d4tpyte", diff --git a/settings.gradle b/settings.gradle index d33a3a30..46bcd53b 100644 --- a/settings.gradle +++ b/settings.gradle @@ -4,7 +4,7 @@ pluginManagement { repositories { mavenLocal() gradlePluginPortal() - String frcYear = '2024' + String frcYear = '2025' File frcHome if (OperatingSystem.current().isWindows()) { String publicFolder = System.getenv('PUBLIC') diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/vision/VisionIOLimelight.java b/src/main/java/org/curtinfrc/frc2025/subsystems/vision/VisionIOLimelight.java index 7d3aacb0..46f96141 100644 --- a/src/main/java/org/curtinfrc/frc2025/subsystems/vision/VisionIOLimelight.java +++ b/src/main/java/org/curtinfrc/frc2025/subsystems/vision/VisionIOLimelight.java @@ -17,6 +17,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.util.Units; +import edu.wpi.first.net.PortForwarder; import edu.wpi.first.networktables.DoubleArrayPublisher; import edu.wpi.first.networktables.DoubleArraySubscriber; import edu.wpi.first.networktables.DoubleSubscriber; @@ -30,6 +31,8 @@ /** IO implementation for real Limelight hardware. */ public class VisionIOLimelight implements VisionIO { + public static int numberLimelights = 0; + private final Supplier rotationSupplier; private final DoubleArrayPublisher orientationPublisher; @@ -55,6 +58,12 @@ public VisionIOLimelight(String name, Supplier rotationSupplier) { megatag1Subscriber = table.getDoubleArrayTopic("botpose_wpiblue").subscribe(new double[] {}); megatag2Subscriber = table.getDoubleArrayTopic("botpose_orb_wpiblue").subscribe(new double[] {}); + + int portOffset = 10 * numberLimelights; + for (int port = 5800; port <= 5809; port++) { + PortForwarder.add(port+portOffset, name + ".local", port); + } + numberLimelights += 1; } @Override diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/vision/VisionIOLimelightGamepiece.java b/src/main/java/org/curtinfrc/frc2025/subsystems/vision/VisionIOLimelightGamepiece.java index 9a003640..ddec5e9c 100644 --- a/src/main/java/org/curtinfrc/frc2025/subsystems/vision/VisionIOLimelightGamepiece.java +++ b/src/main/java/org/curtinfrc/frc2025/subsystems/vision/VisionIOLimelightGamepiece.java @@ -1,6 +1,7 @@ package org.curtinfrc.frc2025.subsystems.vision; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.net.PortForwarder; import edu.wpi.first.networktables.DoubleSubscriber; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.RobotController; @@ -15,6 +16,12 @@ public VisionIOLimelightGamepiece(String name) { latencySubscriber = table.getDoubleTopic("tl").subscribe(0.0); txSubscriber = table.getDoubleTopic("tx").subscribe(0.0); tySubscriber = table.getDoubleTopic("ty").subscribe(0.0); + + int portOffset = 10 * VisionIOLimelight.numberLimelights; + for (int port = 5800; port <= 5809; port++) { + PortForwarder.add(port+portOffset, name + ".local", port); + } + VisionIOLimelight.numberLimelights += 1; } @Override