Current working version of MegaTag2 and MegaTag1

Author: codeNinja-1

src/main/java/com/team6962/lib/swerve/SwerveCore.java

@@ -142,6 +142,11 @@ public void addVisionMeasurement(
     poseEstimator.addVisionMeasurement(visionRobotPoseMeters, timestamp, visionMeasurementStdDevs);
   }
 
+  public void addVisionMeasurement(
+      Pose2d visionRobotPoseMeters, Time timestamp) {
+    poseEstimator.addVisionMeasurement(visionRobotPoseMeters, timestamp);
+  }
+
   public void addVisionHeading(Rotation2d visionHeading) {
     poseEstimator.addVisionHeading(visionHeading);
   }
@@ -166,6 +171,10 @@ public Angle getContinuousGyroscopeAngle() {
     return poseEstimator.getContinuousGyroscopeAngle();
   }
 
+  public PoseEstimator getPoseEstimator() {
+    return poseEstimator;
+  }
+
   /** Should be called after CommandScheduler.run() to mimimize latency. */
   public void latePeriodic() {
     if (currentMovement == null) {

src/main/java/com/team6962/lib/swerve/SwerveDrive.java

@@ -7,6 +7,10 @@
 import static edu.wpi.first.units.Units.Rotations;
 import static edu.wpi.first.units.Units.Seconds;
 
+import java.util.List;
+import java.util.Set;
+import java.util.function.Supplier;
+
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.commands.FollowPathCommand;
 import com.pathplanner.lib.config.PIDConstants;
@@ -24,6 +28,7 @@
 import com.team6962.lib.utils.CommandUtils;
 import com.team6962.lib.utils.KinematicsUtils;
 import com.team6962.lib.utils.MeasureMath;
+
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Pose2d;
@@ -44,9 +49,6 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import java.util.List;
-import java.util.Set;
-import java.util.function.Supplier;
 
 /**
  * The main class for the swerve drive system. This class extends {@link SwerveCore} to provide the

src/main/java/com/team6962/lib/swerve/auto/PoseEstimator.java

@@ -2,9 +2,12 @@
 
 import static edu.wpi.first.units.Units.Seconds;
 
+import java.util.function.Supplier;
+
 import com.team6962.lib.telemetry.Logger;
 import com.team6962.lib.utils.KinematicsUtils;
 import com.team6962.lib.utils.RotationUtils;
+
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
@@ -20,10 +23,6 @@
 import edu.wpi.first.units.measure.Time;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.constants.Constants.LIMELIGHT;
-import frc.robot.vision.AprilTags;
-
-import java.util.function.Supplier;
 
 /**
  * {@code PoseEstimator} is a class that estimates the pose of a swerve drive robot using a
@@ -76,7 +75,7 @@ public void periodic() {
 
     poseEstimator.updateWithTime(
         timestamp.in(Seconds), RotationUtils.fromAngle(gyroscope.getHeading()), modulePositions);
-    AprilTags.injectVisionData(LIMELIGHT.APRILTAG_CAMERA_POSES, this);
+    // AprilTags.injectVisionData(LIMELIGHT.APRILTAG_CAMERA_POSES, this);
     chassisVelocity =
         kinematics.toTwist2d(
             KinematicsUtils.toModulePositions(moduleStatesSupplier.get(), Seconds.of(1.0)));
@@ -86,6 +85,10 @@ public void periodic() {
     lastPositions = modulePositions;
   }
 
+  public SwerveGyroscope getGyroscope() {
+    return gyroscope;
+  }
+
   public Angle getContinuousGyroscopeAngle() {
     return gyroscope.getHeading();
   }
@@ -96,6 +99,12 @@ public void addVisionMeasurement(
         visionRobotPoseMeters, timestamp.in(Seconds), visionMeasurementStdDevs);
   }
 
+  public void addVisionMeasurement(
+      Pose2d visionRobotPoseMeters, Time timestamp) {
+    poseEstimator.addVisionMeasurement(
+        visionRobotPoseMeters, timestamp.in(Seconds));
+  }
+
   public void resetPoseEstimate(Pose2d expectedFieldPose) {
     poseEstimator.resetPosition(
         RotationUtils.fromAngle(gyroscope.getHeading()),

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

@@ -25,21 +25,22 @@
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.livewindow.LiveWindow;
 import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.constants.Constants.CAN;
-import frc.robot.constants.Constants.SWERVE;
 import frc.robot.auto.AutoAlign;
 import frc.robot.auto.Autonomous;
 import frc.robot.auto.Autonomous.Side;
 import frc.robot.commands.PieceCombos;
 import frc.robot.commands.SafeSubsystems;
+import frc.robot.constants.Constants.CAN;
+import frc.robot.constants.Constants.SWERVE;
 import frc.robot.subsystems.Controls;
-import frc.robot.subsystems.leds.LEDs;
 import frc.robot.subsystems.elevator.Elevator;
 import frc.robot.subsystems.hang.Hang;
+import frc.robot.subsystems.leds.LEDs;
 import frc.robot.subsystems.manipulator.Manipulator;
 import frc.robot.util.CachedRobotState;
 import frc.robot.util.RobotEvent;
 import frc.robot.vision.Algae;
+import frc.robot.vision.MegaTag2;
 
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -71,6 +72,7 @@ public static RobotContainer getInstance() {
   public final PieceCombos pieceCombos;
   public final SafeSubsystems safeties;
   private final Command autonomousCommand;
+  private final MegaTag2 megaTag2;
 
   private static PowerDistribution PDH = new PowerDistribution(CAN.PDH, ModuleType.kRev);
 
@@ -116,6 +118,7 @@ public RobotContainer() {
     autov3 = new Autonomous(swerveDrive, manipulator, elevator, pieceCombos);
     algaeDetector = new Algae();
     hang = Hang.create();
+    megaTag2 = new MegaTag2(swerveDrive);
 
     // // Configure the trigger bindings
     Controls.configureBindings(

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

@@ -0,0 +1,122 @@
+package frc.robot.vision;
+
+import static edu.wpi.first.units.Units.Seconds;
+
+import java.util.Set;
+
+import com.team6962.lib.swerve.SwerveDrive;
+import com.team6962.lib.telemetry.Logger;
+import com.team6962.lib.utils.KinematicsUtils;
+
+import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.Constants.LIMELIGHT;
+import frc.robot.util.CachedRobotState;
+import io.limelightvision.LimelightHelpers;
+import io.limelightvision.LimelightHelpers.PoseEstimate;
+import io.limelightvision.LimelightHelpers.RawFiducial;
+
+public class MegaTag2 extends SubsystemBase {
+    private SwerveDrive swerveDrive;
+    private SwerveDrivePoseEstimator megaTag1PoseEstimator;
+
+    public MegaTag2(SwerveDrive swerveDrive) {
+        this.swerveDrive = swerveDrive;
+
+        megaTag1PoseEstimator = new SwerveDrivePoseEstimator(
+            swerveDrive.getKinematics(),
+            new Rotation2d(swerveDrive.getContinuousGyroscopeAngle()),
+            KinematicsUtils.blankModulePositions(4),
+            new Pose2d()
+        );
+    }
+
+    @Override
+    public void periodic() {
+        if (CachedRobotState.isDisabled()) seedGyroscopeAngle();
+
+        addVisionEstimates();
+    }
+
+    // 1. Get pose estimates from MegaTag1
+    // 2. Filter out bad pose estimates
+    // 3. Add offset to gyroscope angle
+
+    public void seedGyroscopeAngle() {
+        Set<String> cameraIds = LIMELIGHT.APRILTAG_CAMERA_POSES.keySet();
+
+        for (String cameraId : cameraIds) {
+            PoseEstimate estimate = LimelightHelpers.getBotPoseEstimate_wpiBlue(cameraId);
+            
+            for (RawFiducial fiducial : estimate.rawFiducials) {
+                Logger.log("Fiducials/" + cameraId + "/" + fiducial.id + "/ambiguity", fiducial.ambiguity);
+                Logger.log("Fiducials/" + cameraId + "/" + fiducial.id + "/distToCamera", fiducial.distToCamera);
+                Logger.log("Fiducials/" + cameraId + "/" + fiducial.id + "/tagArea", fiducial.ta);
+            }
+
+            if (!isEstimateGood(estimate)) continue;
+
+            Logger.getField().getObject("Estimate from " + cameraId).setPose(estimate.pose);
+
+            megaTag1PoseEstimator.addVisionMeasurement(
+                estimate.pose,
+                estimate.timestampSeconds
+            );
+        }
+
+        megaTag1PoseEstimator.updateWithTime(
+            Timer.getFPGATimestamp(),
+            new Rotation2d(swerveDrive.getContinuousGyroscopeAngle()),
+            KinematicsUtils.blankModulePositions(4)
+        );
+
+        Rotation2d fieldRelativeHeading = megaTag1PoseEstimator.getEstimatedPosition().getRotation();
+
+        Logger.getField().getObject("Field Relative Heading").setPose(megaTag1PoseEstimator.getEstimatedPosition());
+
+        for (String cameraId : cameraIds) {
+            LimelightHelpers.SetRobotOrientation(
+                cameraId,
+                fieldRelativeHeading.getDegrees(),
+                0,
+                0,
+                0,
+                0,
+                0
+            );
+        }
+    }
+
+    public boolean isEstimateGood(PoseEstimate estimate) {
+        for (RawFiducial fiducial : estimate.rawFiducials) {
+            if (fiducial.ambiguity < 0.4) return true;
+        }
+
+        return false;
+    }
+
+    // 4. Get pose estimates from MegaTag2
+    // 5. Filter out bad pose estimates
+    // 6. Send MegaTag2 estimates to swerve
+
+    public void addVisionEstimates() {
+        Set<String> cameraIds = LIMELIGHT.APRILTAG_CAMERA_POSES.keySet();
+
+        for (String cameraId : cameraIds) {
+            PoseEstimate estimate = CachedRobotState.isBlue().orElse(false) ? LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(cameraId) :
+                LimelightHelpers.getBotPoseEstimate_wpiRed_MegaTag2(cameraId);
+            
+            if (!isEstimateGood(estimate)) continue;
+
+            Logger.getField().getObject("MegaTag2 estimate from " + cameraId).setPose(estimate.pose);
+
+            swerveDrive.addVisionMeasurement(
+                estimate.pose,
+                Seconds.of(estimate.timestampSeconds)
+            );
+        }
+    }
+}