|
1 | 1 | package frc.robot.subsystems.vision;
|
2 | 2 |
|
3 | 3 | import edu.wpi.first.math.geometry.Pose2d;
|
| 4 | +import edu.wpi.first.net.PortForwarder; |
4 | 5 | import edu.wpi.first.wpilibj.DriverStation;
|
5 | 6 | import frc.robot.Constants.FieldConstants;
|
6 | 7 | import frc.robot.extras.util.GeomUtil;
|
@@ -43,7 +44,9 @@ public class PhysicalVision implements VisionInterface {
|
43 | 44 |
|
44 | 45 | public PhysicalVision() {
|
45 | 46 | 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 |
47 | 50 | threadManager.startTask(
|
48 | 51 | limelight.getName(),
|
49 | 52 | () -> checkAndUpdatePose(limelight),
|
@@ -132,6 +135,26 @@ public boolean isValidMeasurement(Limelight limelight) {
|
132 | 135 | return isValidPoseEstimate(limelight) && isConfident(limelight) && !isTeleporting(limelight);
|
133 | 136 | }
|
134 | 137 |
|
| 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 | + |
135 | 158 | /**
|
136 | 159 | * Gets the pose update of the specified limelight while the robot is enabled.
|
137 | 160 | *
|
|
0 commit comments