From b62942a18e0fae094d2c13bab2a0e40f20eb9c98 Mon Sep 17 00:00:00 2001
From: Team 199 Driver Station Computer
 <35879629+DriverStationComputer@users.noreply.github.com>
Date: Sat, 20 Jul 2024 08:53:41 -0700
Subject: [PATCH] added limelight port fowarding

---
 src/main/java/org/carlmontrobotics/Robot.java | 11 +++++++++-
 .../subsystems/Limelight.java                 | 22 ++++++-------------
 2 files changed, 17 insertions(+), 16 deletions(-)

diff --git a/src/main/java/org/carlmontrobotics/Robot.java b/src/main/java/org/carlmontrobotics/Robot.java
index 46720cba..822396e6 100644
--- a/src/main/java/org/carlmontrobotics/Robot.java
+++ b/src/main/java/org/carlmontrobotics/Robot.java
@@ -4,8 +4,12 @@
 
 package org.carlmontrobotics;
 
+import static org.carlmontrobotics.Constants.Limelightc.INTAKE_LL_NAME;
+import static org.carlmontrobotics.Constants.Limelightc.SHOOTER_LL_NAME;
+
 import com.pathplanner.lib.commands.FollowPathCommand;
 
+import edu.wpi.first.net.PortForwarder;
 import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.TimedRobot;
@@ -31,7 +35,12 @@ public void robotInit() {
     DataLogManager.start();
     DriverStation.startDataLog(DataLogManager.getLog());
     FollowPathCommand.warmupCommand().schedule();
-    
+    for (int port = 5800; port <= 5809; port++) {
+      PortForwarder.add(port, INTAKE_LL_NAME + ".local", port);
+    }
+    for (int port = 5800; port <= 5809; port++) {
+      PortForwarder.add(port + 10, SHOOTER_LL_NAME + ".local", port);
+    }
   }
 
   @Override
diff --git a/src/main/java/org/carlmontrobotics/subsystems/Limelight.java b/src/main/java/org/carlmontrobotics/subsystems/Limelight.java
index 092b9799..c5a7534e 100644
--- a/src/main/java/org/carlmontrobotics/subsystems/Limelight.java
+++ b/src/main/java/org/carlmontrobotics/subsystems/Limelight.java
@@ -87,9 +87,8 @@ public double getDistanceToSpeakerMeters() {
       Rotation2d angleToGoal = Rotation2d.fromDegrees(MOUNT_ANGLE_DEG_SHOOTER)
           .plus(Rotation2d.fromDegrees(getTYDeg(SHOOTER_LL_NAME))); // because limelight is mounted
                                                                     // horizontally
-      double distance =
-          (SPEAKER_CENTER_HEIGHT_METERS - HEIGHT_FROM_GROUND_METERS_SHOOTER)
-              / angleToGoal.getTan();
+      double distance = (SPEAKER_CENTER_HEIGHT_METERS - HEIGHT_FROM_GROUND_METERS_SHOOTER)
+          / angleToGoal.getTan();
       // SmartDashboard.putNumber("limelight distance", distance);
       return distance;
     } else {
@@ -113,37 +112,30 @@ public double getDistanceToNoteMeters() {
   }
 
   public double getArmAngleToShootSpeakerRad() {
-    double armRestingHeightToSubwooferMeters =
-        HEIGHT_FROM_RESTING_ARM_TO_SPEAKER_METERS;
-    double horizontalDistanceMeters =
-        getDistanceToSpeakerMeters() + SIDEWAYS_OFFSET_TO_OUTTAKE_MOUTH;
+    double armRestingHeightToSubwooferMeters = HEIGHT_FROM_RESTING_ARM_TO_SPEAKER_METERS;
+    double horizontalDistanceMeters = getDistanceToSpeakerMeters() + SIDEWAYS_OFFSET_TO_OUTTAKE_MOUTH;
     return END_EFFECTOR_BASE_ANGLE_RADS - Math
         .atan(armRestingHeightToSubwooferMeters / horizontalDistanceMeters);
   }
 
-
   public double getRotateAngleRadMT2() {
-    Pose3d targetPoseRobotSpace =
-        LimelightHelpers.getTargetPose3d_RobotSpace(SHOOTER_LL_NAME); // pose of the target
+    Pose3d targetPoseRobotSpace = LimelightHelpers.getTargetPose3d_RobotSpace(SHOOTER_LL_NAME); // pose of the target
 
     double targetX = targetPoseRobotSpace.getX(); // the forward offset between the center of the
                                                   // robot and target
     double targetZ = -targetPoseRobotSpace.getZ(); // the sideways offset
 
-    double targetOffsetRads =
-        MathUtil.inputModulus(Math.atan2(targetX, targetZ), -Math.PI, Math.PI);
+    double targetOffsetRads = MathUtil.inputModulus(Math.atan2(targetX, targetZ), -Math.PI, Math.PI);
 
     return targetOffsetRads;
   }
 
   public double getDistanceToSpeakerMetersMT2() {
-    Pose3d targetPoseRobotSpace =
-        LimelightHelpers.getTargetPose3d_RobotSpace(SHOOTER_LL_NAME);
+    Pose3d targetPoseRobotSpace = LimelightHelpers.getTargetPose3d_RobotSpace(SHOOTER_LL_NAME);
 
     double x = targetPoseRobotSpace.getX();
     double z = targetPoseRobotSpace.getZ();
 
-
     return Math.hypot(x, z);
   }