add mega tag 1 by duplicating mega tag 2 all around

team581 · Nov 27, 2024 · 64d908d · 64d908d
1 parent ad717b6
commit 64d908d
Show file tree

Hide file tree

Showing 5 changed files with 97 additions and 5 deletions.
diff --git a/src/main/java/frc/robot/config/CompConfig.java b/src/main/java/frc/robot/config/CompConfig.java
@@ -216,7 +216,14 @@ class CompConfig {
               },
               -77.0,
               87.0),
-          new VisionConfig(4, 0.4, 0.4, InterpolatedVisionDataset.MADTOWN),
+          new VisionConfig(
+              4,
+              0.4,
+              Double.POSITIVE_INFINITY,
+              InterpolatedVisionDataset.MADTOWN,
+              0.4,
+              Double.POSITIVE_INFINITY,
+              InterpolatedVisionDataset.MADTOWN),
           new LightsConfig("rio", 3));
 
   private CompConfig() {}

diff --git a/src/main/java/frc/robot/config/RobotConfig.java b/src/main/java/frc/robot/config/RobotConfig.java
@@ -63,7 +63,10 @@ public record VisionConfig(
       int translationHistoryArraySize,
       double xyStdDev,
       double thetaStdDev,
-      InterpolatedVisionDataset interpolatedVisionSet) {}
+      InterpolatedVisionDataset interpolatedVisionSet,
+      double xyStdDev1,
+      double thetaStdDev1,
+      InterpolatedVisionDataset interpolatedVisionSet1) {}
 
   public record LightsConfig(String canBusName, int deviceID) {}
 

diff --git a/src/main/java/frc/robot/localization/LocalizationSubsystem.java b/src/main/java/frc/robot/localization/LocalizationSubsystem.java
@@ -31,13 +31,19 @@ public class LocalizationSubsystem extends StateMachine<LocalizationState> {
           RobotConfig.get().vision().xyStdDev(),
           RobotConfig.get().vision().xyStdDev(),
           RobotConfig.get().vision().thetaStdDev());
+  private static final Vector<N3> VISION_STD_DEVS_1 =
+      VecBuilder.fill(
+          RobotConfig.get().vision().xyStdDev(),
+          RobotConfig.get().vision().xyStdDev(),
+          RobotConfig.get().vision().thetaStdDev());
   private final ImuSubsystem imu;
   private final VisionSubsystem vision;
   private final SwerveSubsystem swerve;
   private final TimeInterpolatableBuffer<Pose2d> poseHistory =
       TimeInterpolatableBuffer.createBuffer(1.5);
   private double lastAddedVisionTimestamp = 0;
   private List<VisionResult> latestResult = new ArrayList<>();
+  private List<VisionResult> latestResultMegaTag1 = new ArrayList<>();
 
   public LocalizationSubsystem(ImuSubsystem imu, VisionSubsystem vision, SwerveSubsystem swerve) {
     super(SubsystemPriority.LOCALIZATION, LocalizationState.DEFAULT_STATE);
@@ -49,12 +55,17 @@ public LocalizationSubsystem(ImuSubsystem imu, VisionSubsystem vision, SwerveSub
   @Override
   protected void collectInputs() {
     latestResult = vision.getInterpolatedVisionResult();
+    latestResultMegaTag1 = vision.getInterpolatedVisionResultMegaTag1();
   }
 
   public Pose2d getPose() {
     return swerve.getDrivetrainState().Pose;
   }
 
+  public Pose2d getPoseMegaTag1() {
+    return (Pose2d) vision.getInterpolatedVisionResultMegaTag1();
+  }
+
   @Override
   public void robotPeriodic() {
     super.robotPeriodic();
@@ -73,10 +84,26 @@ public void robotPeriodic() {
 
       poseHistory.addSample(Timer.getFPGATimestamp(), getPose());
     }
-    if (DriverStation.isDisabled()) {
-      if (!MathUtil.isNear(getPose().getRotation().getDegrees(), imu.getRobotHeading(), 1)) {
-        imu.setAngle(getPose().getRotation().getDegrees());
+
+    if (DriverStation.isDisabled()
+        && !MathUtil.isNear(
+            getPoseMegaTag1().getRotation().getDegrees(),
+            imu.getRobotHeading(),
+            1.0,
+            -180.0,
+            180.0)) {
+      for (var results : latestResultMegaTag1) {
+        Pose2d visionPose = results.pose();
+
+        double visionTimestamp = results.timestamp();
+
+        swerve.drivetrain.addVisionMeasurement(visionPose, visionTimestamp, VISION_STD_DEVS_1);
+        lastAddedVisionTimestamp = visionTimestamp;
+
+        poseHistory.addSample(Timer.getFPGATimestamp(), getPose());
       }
+
+      imu.setAngle(getPoseMegaTag1().getRotation().getDegrees());
     }
 
     DogLog.log("Localization/EstimatedPose", getPose());

diff --git a/src/main/java/frc/robot/vision/Limelight.java b/src/main/java/frc/robot/vision/Limelight.java
@@ -48,6 +48,20 @@ public Optional<VisionResult> getInterpolatedVisionResult() {
     return Optional.of(new VisionResult(interpolatedPose, rawResult.get().timestamp()));
   }
 
+  public Optional<VisionResult> getInterpolatedVisionResultMegaTag1() {
+    var rawResult = getRawVisionResultMegaTag1();
+
+    updateState(rawResult);
+
+    if (rawResult.isEmpty()) {
+      return Optional.empty();
+    }
+
+    Pose2d interpolatedPose =
+        InterpolatedVision.interpolatePose(rawResult.get().pose(), interpolationData);
+    return Optional.of(new VisionResult(interpolatedPose, rawResult.get().timestamp()));
+  }
+
   private Optional<VisionResult> getRawVisionResult() {
     var estimatePose = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelightTableName);
 
@@ -69,6 +83,27 @@ private Optional<VisionResult> getRawVisionResult() {
     return Optional.of(new VisionResult(estimatePose.pose, estimatePose.timestampSeconds));
   }
 
+  Optional<VisionResult> getRawVisionResultMegaTag1() {
+    var estimatePose = LimelightHelpers.getBotPoseEstimate_wpiBlue(limelightTableName);
+
+    if (estimatePose == null) {
+      return Optional.empty();
+    }
+
+    DogLog.log("Vision/" + name + "/RawLimelightPose", estimatePose.pose);
+
+    if (estimatePose.tagCount == 0) {
+      return Optional.empty();
+    }
+
+    // This prevents pose estimator from having crazy poses if the Limelight loses power
+    if (estimatePose.pose.getX() == 0.0 && estimatePose.pose.getY() == 0.0) {
+      return Optional.empty();
+    }
+
+    return Optional.of(new VisionResult(estimatePose.pose, estimatePose.timestampSeconds));
+  }
+
   private void updateState(Optional<VisionResult> rawResult) {
     var newHeartbeat = LimelightHelpers.getLimelightNTDouble(limelightTableName, "hb");
 

diff --git a/src/main/java/frc/robot/vision/VisionSubsystem.java b/src/main/java/frc/robot/vision/VisionSubsystem.java
@@ -16,6 +16,8 @@ public class VisionSubsystem extends StateMachine<VisionState> {
   private final Limelight leftLimelight;
   private final Limelight rightLimelight;
   private final List<VisionResult> interpolatedVisionResult = new ArrayList<>();
+  private final List<VisionResult> interpolatedVisionResultMegaTag1 = new ArrayList<>();
+
   private double robotHeading;
   private double pitch;
   private double angularVelocity;
@@ -54,6 +56,10 @@ protected void collectInputs() {
     var leftInterpolatedVisionResult = leftLimelight.getInterpolatedVisionResult();
     var rightInterpolatedVisionResult = rightLimelight.getInterpolatedVisionResult();
 
+    var leftInterpolatedVisionResultMegaTag1 = leftLimelight.getInterpolatedVisionResultMegaTag1();
+    var rightInterpolatedVisionResultMegaTag1 =
+        rightLimelight.getInterpolatedVisionResultMegaTag1();
+
     interpolatedVisionResult.clear();
 
     if (!DriverStation.isAutonomous()) {
@@ -64,12 +70,26 @@ protected void collectInputs() {
     if (rightInterpolatedVisionResult.isPresent()) {
       interpolatedVisionResult.add(rightInterpolatedVisionResult.get());
     }
+    interpolatedVisionResultMegaTag1.clear();
+
+    if (!DriverStation.isAutonomous()) {
+      if (leftInterpolatedVisionResultMegaTag1.isPresent()) {
+        interpolatedVisionResultMegaTag1.add(leftInterpolatedVisionResultMegaTag1.get());
+      }
+    }
+    if (rightInterpolatedVisionResultMegaTag1.isPresent()) {
+      interpolatedVisionResultMegaTag1.add(rightInterpolatedVisionResultMegaTag1.get());
+    }
   }
 
   public List<VisionResult> getInterpolatedVisionResult() {
     return interpolatedVisionResult;
   }
 
+  public List<VisionResult> getInterpolatedVisionResultMegaTag1() {
+    return interpolatedVisionResultMegaTag1;
+  }
+
   @Override
   public void robotPeriodic() {
     super.robotPeriodic();