got it to work

Team334 · Apr 5, 2024 · 2f2eb08 · 2f2eb08
1 parent 4f9cc78
commit 2f2eb08
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 3 deletions.
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -39,9 +39,13 @@ public void robotInit() {
     // for (int port = 5800; port <= 5807; port++) {
     //   PortForwarder.add(port, "limelight.local", port);
     // }
-    PortForwarder.add(5800, "limelight.local", 5800);
-    PortForwarder.add(5801, "limelight.local", 5801);
-    PortForwarder.add(5805, "limelight.local", 5805);
+    PortForwarder.add(5800, "limelight-main.local", 5800);
+    PortForwarder.add(5801, "limelight-main.local", 5801);
+    PortForwarder.add(5805, "limelight-main.local", 5805);
+
+    // PortForwarder.add(5810, "limelight-intake.local", 5810);
+    // PortForwarder.add(5811, "limelight-intake.local", 5811);
+    // PortForwarder.add(5815, "limelight-intake.local", 5815);
 
     // CameraServer.startAutomaticCapture(0);
     UtilFuncs.LoadField();

diff --git a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java
@@ -220,6 +220,8 @@ private boolean updateBotpose() {
 
     // UPDATE BOTPOSE WITH VISION
     if (visionBotpose.isPresent()) {
+      System.out.println("PRESENT");
+
       LimelightHelper.SetRobotOrientation(
         Limelights.MAIN,
         getHeading().getDegrees(),
@@ -232,6 +234,8 @@ private boolean updateBotpose() {
 
       if (Math.abs(_gyro.getRate()) >= 500) return false;
 
+      visionPublisher.set(visionBotpose.get().pose);
+
       _estimator.addVisionMeasurement(
         visionBotpose.get().pose,
         visionBotpose.get().timestampSeconds