vision bringup

Author: jordanjcoderman

src/main/java/frc/robot/Robot.java

@@ -39,8 +39,8 @@ public class Robot extends TimedRobot {
       new IntakeSubsystem(hardware.intakeMain, hardware.intakeCenteringMotor);
   private final SwerveSubsystem swerve = new SwerveSubsystem();
   private final ImuSubsystem imu = new ImuSubsystem(swerve.drivetrainPigeon);
-  private final Limelight leftLimelight = new Limelight();
-  private final Limelight rightLimelight = new Limelight();
+  private final Limelight leftLimelight = new Limelight("left");
+  private final Limelight rightLimelight = new Limelight("right");
 
   private final VisionSubsystem vision = new VisionSubsystem(imu, leftLimelight, rightLimelight);
   private final LocalizationSubsystem localization = new LocalizationSubsystem(imu, vision, swerve);

src/main/java/frc/robot/vision/Limelight.java

@@ -1,40 +1,43 @@
 package frc.robot.vision;
 
 import edu.wpi.first.math.geometry.Pose2d;
+import frc.robot.vision.LimelightHelpers.LimelightResults;
 import frc.robot.vision.interpolation.InterpolatedVision;
 import java.util.Optional;
 
+import dev.doglog.DogLog;
+
 public class Limelight {
   private Optional<VisionResult> rawVisionResult;
   private Optional<VisionResult> processedVisionResult;
-  public String limeLightName = "";
-
-  public Optional<VisionResult> getRawVisionResult() {
-    var estimatePose = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limeLightName);
+  private final String limelightTableName;
+  private final String name;
 
-    if (estimatePose == null) {
-      return Optional.empty();
-    }
+  public Limelight(String name) {
+    limelightTableName = "limelight-" + name;
+    this.name = name;
+  }
 
-    if (estimatePose.tagCount == 0) {
-      return Optional.empty();
-    }
+  public Optional<VisionResult> getInterpolatedVisionResult() {
+    var rawResult = getRawVisionResult();
 
-    // This prevents pose estimator from having crazy poses if the Limelight loses power
-    if (estimatePose.pose.getX() == 0.0 && estimatePose.pose.getY() == 0.0) {
+    if (rawResult.isEmpty()) {
       return Optional.empty();
     }
-
-    return Optional.of(new VisionResult(estimatePose.pose, estimatePose.timestampSeconds));
+    
+    Pose2d interpolatedPose = InterpolatedVision.interpolatePose(rawResult.get().pose());
+    return Optional.of(new VisionResult(interpolatedPose, rawResult.get().timestamp()));
   }
 
-  public Optional<VisionResult> getInterpolatedVisionResult() {
-    var estimatePose = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limeLightName);
+  private Optional<VisionResult> getRawVisionResult() {
+    var estimatePose = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelightTableName);
 
     if (estimatePose == null) {
       return Optional.empty();
     }
 
+    DogLog.log("Vision/" + name + "/RawLimelightPose", estimatePose.pose);
+
     if (estimatePose.tagCount == 0) {
       return Optional.empty();
     }
@@ -43,7 +46,7 @@ public Optional<VisionResult> getInterpolatedVisionResult() {
     if (estimatePose.pose.getX() == 0.0 && estimatePose.pose.getY() == 0.0) {
       return Optional.empty();
     }
-    Pose2d interpolatedPose = InterpolatedVision.interpolatePose(estimatePose.pose);
-    return Optional.of(new VisionResult(interpolatedPose, estimatePose.timestampSeconds));
+
+    return Optional.of(new VisionResult(estimatePose.pose, estimatePose.timestampSeconds));
   }
 }

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

@@ -6,11 +6,12 @@
 import java.util.ArrayList;
 import java.util.List;
 
+import dev.doglog.DogLog;
+
 public class VisionSubsystem extends StateMachine<VisionState> {
   private final ImuSubsystem imu;
   private final Limelight leftLimelight;
   private final Limelight rightLimelight;
-  private final List<VisionResult> processedVisionResult = new ArrayList<>();
   private final List<VisionResult> interpolatedVisionResult = new ArrayList<>();
 
   public VisionSubsystem(ImuSubsystem imu, Limelight leftLimelight, Limelight rightLimelight) {
@@ -22,19 +23,13 @@ public VisionSubsystem(ImuSubsystem imu, Limelight leftLimelight, Limelight righ
 
   @Override
   protected void collectInputs() {
-    var leftResult = leftLimelight.getRawVisionResult();
-    var rightResult = rightLimelight.getRawVisionResult();
     var leftInterpolatedVisionResult = leftLimelight.getInterpolatedVisionResult();
     var rightInterpolatedVisionResult = rightLimelight.getInterpolatedVisionResult();
 
-    processedVisionResult.clear();
 
-    if (leftResult.isPresent()) {
-      processedVisionResult.add(leftResult.get());
-    }
-    if (rightResult.isPresent()) {
-      processedVisionResult.add(rightResult.get());
-    }
+
+    interpolatedVisionResult.clear();
+
     if (leftInterpolatedVisionResult.isPresent()) {
       interpolatedVisionResult.add(leftInterpolatedVisionResult.get());
     }
@@ -43,16 +38,7 @@ protected void collectInputs() {
     }
   }
 
-  public List<VisionResult> getVisionResult() {
-    return processedVisionResult;
-  }
-
   public List<VisionResult> getInterpolatedVisionResult() {
     return interpolatedVisionResult;
   }
-
-  @Override
-  public void robotPeriodic() {
-    super.robotPeriodic();
-  }
 }