Skip to content

Commit 5d23f26

Browse files
authored
Merge pull request #66 from TitaniumTigers4829/port-forwarding-insert-question-mark-here
port forwarding? for da limelights
2 parents cc8c56e + 92232c3 commit 5d23f26

File tree

2 files changed

+30
-1
lines changed

2 files changed

+30
-1
lines changed

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

Lines changed: 24 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,7 @@
11
package frc.robot.subsystems.vision;
22

33
import edu.wpi.first.math.geometry.Pose2d;
4+
import edu.wpi.first.net.PortForwarder;
45
import edu.wpi.first.wpilibj.DriverStation;
56
import frc.robot.Constants.FieldConstants;
67
import frc.robot.extras.util.GeomUtil;
@@ -43,7 +44,9 @@ public class PhysicalVision implements VisionInterface {
4344

4445
public PhysicalVision() {
4546
for (Limelight limelight : Limelight.values()) {
46-
// Start a vision input task for each Limelight
47+
// Setupt port forwarding for each limelight
48+
setupPortForwarding(limelight);
49+
// Start a threaded task to check and update the pose for each Limelight
4750
threadManager.startTask(
4851
limelight.getName(),
4952
() -> checkAndUpdatePose(limelight),
@@ -132,6 +135,26 @@ public boolean isValidMeasurement(Limelight limelight) {
132135
return isValidPoseEstimate(limelight) && isConfident(limelight) && !isTeleporting(limelight);
133136
}
134137

138+
/**
139+
* Sets up port forwarding for the specified Limelight. This method forwards a range of ports from
140+
* the robot to the Limelight, allowing network communication between the robot and the Limelight.
141+
*
142+
* <p>Each Limelight is assigned a unique port offset based on its ID. The method forwards ports
143+
* 5800 to 5809 for each Limelight, with the port offset applied to each port number. For example,
144+
* if the Limelight ID is 1, the ports 5810 to 5819 will be forwarded.
145+
*
146+
* @param limelight The Limelight for which to set up port forwarding.
147+
*/
148+
private void setupPortForwarding(Limelight limelight) {
149+
int portOffset = VisionConstants.PORT_OFFSET * limelight.getId();
150+
for (int port = VisionConstants.BASE_PORT;
151+
port < VisionConstants.BASE_PORT + VisionConstants.PORT_RANGE;
152+
port++) {
153+
PortForwarder.add(
154+
port + portOffset, limelight.getName() + VisionConstants.LIMELIGHT_DOMAIN, port);
155+
}
156+
}
157+
135158
/**
136159
* Gets the pose update of the specified limelight while the robot is enabled.
137160
*

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

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -97,6 +97,12 @@ public static Limelight fromId(int id) {
9797
public static final String FRONT_RIGHT_LIMELIGHT_NAME = "limelight-right";
9898
public static final int FRONT_RIGHT_LIMELIGHT_NUMBER = 2;
9999

100+
// Constants for port forwarding
101+
public static final int BASE_PORT = 5800;
102+
public static final int PORT_RANGE = 10;
103+
public static final int PORT_OFFSET = 10;
104+
public static final String LIMELIGHT_DOMAIN = ".local";
105+
100106
public static final double[][] ONE_APRIL_TAG_LOOKUP_TABLE = {
101107
// {distance in meters, x std deviation, y std deviation, r (in degrees) std deviation}
102108
{0, 0.02, 0.02, Units.degreesToRadians(180000)}, // 2

0 commit comments

Comments
 (0)