Vision updates

src/main/java/frc/robot/Constants.java

@@ -23,6 +23,11 @@
 public final class Constants {
   public static final Alliance SAFE_ALLIANCE = Alliance.Red;
 
+  public static class Limelights {
+    public static final String MAIN = "limelight-main";
+    public static final String INTAKE = "limelight-intake";
+  }
+
   public static class CAN {
     public static final int DRIVE_FRONT_LEFT = 1;
     public static final int ROT_FRONT_LEFT = 2;

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();

src/main/java/frc/robot/RobotContainer.java

@@ -39,6 +39,7 @@
 import frc.robot.subsystems.IntakeSubsystem.FeedMode;
 import frc.robot.subsystems.ShooterSubsystem.ShooterState;
 import frc.robot.utils.UtilFuncs;
+import frc.robot.utils.helpers.LimelightHelper.PoseEstimate;
 import frc.robot.subsystems.LEDSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
 import frc.robot.subsystems.SwerveDriveSubsystem;
@@ -196,10 +197,10 @@ private void configureBindings() {
 
     // TESTING ONLY!!!
     _driveController.circle().onTrue(Commands.runOnce(() -> {
-      Optional<double[]> pose = _visionSubsystem.getBotposeBlue();
+      Optional<PoseEstimate> pose = _visionSubsystem.getBotposeBlue();
 
       if (pose.isPresent()) {
-        Pose2d botpose = UtilFuncs.ToPose(pose.get());
+        Pose2d botpose = pose.get().pose;
         _swerveSubsystem.resetPose(new Pose2d(botpose.getX(), botpose.getY(), _swerveSubsystem.getHeading()));
       }
     }, _swerveSubsystem));

src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java

@@ -31,10 +31,13 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
 import frc.robot.Constants.FieldConstants;
+import frc.robot.Constants.Limelights;
 import frc.robot.Constants.PID;
 import frc.robot.Constants.Speeds;
 import frc.robot.utils.SwerveModule;
 import frc.robot.utils.UtilFuncs;
+import frc.robot.utils.helpers.LimelightHelper;
+import frc.robot.utils.helpers.LimelightHelper.PoseEstimate;
 
 /**
  * @author Peter Gutkovich
@@ -98,7 +101,7 @@ Constants.Physical.SWERVE_KINEMATICS, getHeadingRaw(),
     new SwerveModulePosition[] {_frontLeft.getPosition(), _frontRight.getPosition(), _backRight.getPosition(), _backLeft.getPosition()},
     new Pose2d(), 
     VecBuilder.fill(0.03, 0.03, 0.01), 
-    VecBuilder.fill(0.9, 0.9, 9999999)
+    VecBuilder.fill(0.4, 0.4, 9999999)
   );
 
   // OTHER POSSIBLE STD DEV VALUES:
@@ -211,39 +214,32 @@ private boolean updateBotpose() {
       _backLeft.getPosition()
     });
 
-    Optional<double[]> visionBotpose = _visionSubsystem.getBotposeBlue();
+    Optional<PoseEstimate> visionBotpose = _visionSubsystem.getBotposeBlue();
 
     SmartDashboard.putBoolean("SEES TAG(S)", visionBotpose.isPresent());
 
     // UPDATE BOTPOSE WITH VISION
     if (visionBotpose.isPresent()) {
-      double[] llBotpose = visionBotpose.get();
-
-      double tagDistance = llBotpose[9];
-      double tagCount = llBotpose[7];
-
-      double xyStds = 0;
-      double yawStd = 9999999;
-
-      SmartDashboard.putNumber("TAG(S) DISTANCE", tagDistance);
-      SmartDashboard.putNumber("TAG COUNT", tagCount);
-
-      if (tagDistance <= FieldConstants.CLOSE_TAG_DISTANCE_THRESHOLD) {
-        if (tagCount >= 2) { xyStds = 0.25; yawStd = 9000; }
-        else { xyStds = 0.8; yawStd = 10000; }
-      }
-
-      else if (tagDistance <= FieldConstants.FAR_TAG_DISTANCE_THRESHOLD) {
-        if (tagCount >= 2) { xyStds = 0.9; yawStd = 13000; }
-        else { xyStds = 1.2; yawStd = 16000; }
-      }
-
-      else {
-        return false;
-      }
-
-      visionPublisher.set(UtilFuncs.ToPose(llBotpose));
-      _estimator.addVisionMeasurement(UtilFuncs.ToPose(llBotpose), _visionSubsystem.getLatency(), VecBuilder.fill(xyStds, xyStds, yawStd));
+      System.out.println("PRESENT");
+
+      LimelightHelper.SetRobotOrientation(
+        Limelights.MAIN,
+        getHeading().getDegrees(),
+        0,
+        0,
+        0,
+        0,
+        0
+      );
+
+      if (Math.abs(_gyro.getRate()) >= 500) return false;
+
+      visionPublisher.set(visionBotpose.get().pose);
+
+      _estimator.addVisionMeasurement(
+        visionBotpose.get().pose,
+        visionBotpose.get().timestampSeconds
+      );
     }
 
     return true;

src/main/java/frc/robot/subsystems/VisionSubsystem.java

@@ -3,24 +3,18 @@
 
 import java.util.Optional;
 
-import com.fasterxml.jackson.databind.JsonNode;
-import com.fasterxml.jackson.databind.node.ArrayNode;
-
-import edu.wpi.first.networktables.NetworkTableEntry;
-import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants.Limelights;
 import frc.robot.utils.helpers.LimelightHelper;
+import frc.robot.utils.helpers.LimelightHelper.PoseEstimate;
 
 /**
  * @author Lucas Ou
  * @author Alex Reyes
  * @author Peter Gutkovich
  */
 public class VisionSubsystem extends SubsystemBase {
-  private final LimelightHelper _main = new LimelightHelper("limelight-main");
-  private final LimelightHelper _intake = new LimelightHelper("limelight-intake");
-
   /** Creates a new VisionSubsystem. */
   public VisionSubsystem() {}
 
@@ -34,63 +28,29 @@ public void periodic() {
     }
   }
 
-  /**
-   * Returns the latency from the last time data was sent from the main limelight. This
-   * should be used in the pose estimator.
-   */
-  public double getLatency() {
-    double tl = _main.getEntry("tl").getDouble(0);
-    double cl = _main.getEntry("cl").getDouble(0);
-
-    return Timer.getFPGATimestamp() - (tl / 1000.0) - (cl / 1000.0);
-  }
-
   /** Return a boolean for whether a tag is seen by main cam. */
   public boolean isApriltagVisible() {
-    double tv = _main.getEntry("tv").getDouble(0);
-
-    if (tv == 0) {
-      return false;
-    }
-
-    if (tv == 1) {
-      return true;
-    }
-
-    return false;
+    return LimelightHelper.getTV(Limelights.MAIN);
   }
 
   /** Return a boolean for whether a note is seen by intake cam. */
   public boolean isNoteVisible() {
-    double tv = _intake.getEntry("tv").getDouble(0);
-
-    if (tv == 0) {
-      return false;
-    }
-
-    if (tv == 1) {
-      return true;
-    }
-
-    return false;
+    return LimelightHelper.getTV(Limelights.INTAKE);
   }
 
   /**
    * Returns the NT "botpose_wpiblue" as an array from the limelight if tags are visible. 
    * 
-   * @return NT "botpose_wpiblue" limelight topic.
+   * @return NT "botpose_wpiblue" limelight topic. This uses megatag 2.
    * 
    * @see Optional
    */
-  public Optional<double[]> getBotposeBlue() {
+  public Optional<PoseEstimate> getBotposeBlue() {
     if (!isApriltagVisible()) return Optional.empty();
 
-    NetworkTableEntry botpose_entry = _main.getEntry("botpose_wpiblue");
-    if (!botpose_entry.exists()) return Optional.empty();
-
-    double[] botpose_array = botpose_entry.getDoubleArray(new double[11]);
+    PoseEstimate botpose_blue = LimelightHelper.getBotPoseEstimate_wpiBlue_MegaTag2(Limelights.MAIN);
 
-    return Optional.of(botpose_array);
+    return Optional.of(botpose_blue);
   }
 
   /**
@@ -101,14 +61,9 @@ public Optional<double[]> getBotposeBlue() {
   public Optional<double[]> getNoteAngles() {
     if (!isNoteVisible()) return Optional.empty();
 
-    ArrayNode targets = _intake.getNeuralTargets();
-    JsonNode target = targets.get(0);
-
-    if (target == null) return Optional.empty();
-
     double[] angles = {
-      target.get("tx").asDouble(0),
-      target.get("ty").asDouble(0)
+      LimelightHelper.getTX(Limelights.INTAKE),
+      LimelightHelper.getTY(Limelights.INTAKE)
     };
 
     return Optional.of(angles);