Limelight MegaTag1 (#140)

* Limelight Megatag1 * Update src/main/java/frc/robot/RobotPreferences.java Co-authored-by: Tayler Uva <[email protected]> --------- Co-authored-by: Tayler Uva <[email protected]>
FRCTeam3255 · Dec 16, 2024 · af68551 · af68551
1 parent 6005a7d
commit af68551
Show file tree

Hide file tree

Showing 6 changed files with 53 additions and 24 deletions.
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -57,6 +57,7 @@ public void robotPeriodic() {
   public void disabledInit() {
     m_robotContainer.subStateMachine.setRobotState(RobotState.NONE);
     m_robotContainer.subStateMachine.setTargetState(TargetState.PREP_NONE);
+    m_robotContainer.setMegaTag2(false);
 
     Shooter.hasZeroed = false;
     Elevator.hasZeroed = false;
@@ -81,6 +82,7 @@ public void disabledExit() {
 
   @Override
   public void autonomousInit() {
+    m_robotContainer.setMegaTag2(true);
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
     bothSubsystemsZeroed = Shooter.hasZeroed && Elevator.hasZeroed;
 
@@ -106,6 +108,7 @@ public void autonomousExit() {
   @Override
   public void teleopInit() {
     bothSubsystemsZeroed = Shooter.hasZeroed && Elevator.hasZeroed;
+    m_robotContainer.setMegaTag2(true);
 
     RobotContainer.checkForManualZeroing().cancel();
 

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -12,6 +12,7 @@
 
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.units.Units;
 import edu.wpi.first.wpilibj.GenericHID.RumbleType;
 import edu.wpi.first.wpilibj.PowerDistribution;
@@ -26,6 +27,7 @@
 import frc.robot.Constants.constLEDs;
 import frc.robot.Constants.constShooter;
 import frc.robot.RobotMap.mapControllers;
+import frc.robot.RobotPreferences.prefVision;
 import frc.robot.commands.AddVisionMeasurement;
 import frc.robot.commands.Drive;
 import frc.robot.commands.GamePieceRumble;
@@ -411,6 +413,23 @@ public static Command AddVisionMeasurement() {
         .withInterruptBehavior(Command.InterruptionBehavior.kCancelIncoming).ignoringDisable(true);
   }
 
+  public void setMegaTag2(boolean setMegaTag2) {
+
+    if (setMegaTag2) {
+      subDrivetrain.swervePoseEstimator.setVisionMeasurementStdDevs(VecBuilder.fill(
+          prefVision.megaTag2StdDevsPosition.getValue(),
+          prefVision.megaTag2StdDevsPosition.getValue(),
+          prefVision.megaTag2StdDevsHeading.getValue()));
+    } else {
+      // Use MegaTag 1
+      subDrivetrain.swervePoseEstimator.setVisionMeasurementStdDevs(VecBuilder.fill(
+          prefVision.megaTag1StdDevsPosition.getValue(),
+          prefVision.megaTag1StdDevsPosition.getValue(),
+          prefVision.megaTag1StdDevsHeading.getValue()));
+    }
+    subLimelight.setMegaTag2(setMegaTag2);
+  }
+
   // -- PDH --
   /**
    * Updates the values supplied to the PDH to SmartDashboard. Should be called

diff --git a/src/main/java/frc/robot/RobotPreferences.java b/src/main/java/frc/robot/RobotPreferences.java
@@ -85,23 +85,18 @@ public static final class prefElevator {
   }
 
   public static final class prefVision {
-    /**
-     * <p>
-     * Pose estimator standard deviation for vision data using Multi-tag
-     * <p>
-     * <b>Units:</b> Meters
-     */
-    public static final SN_DoublePreference multiTagStdDevsPosition = new SN_DoublePreference(
-        "multiTagStdDevsPosition", 0.7);
+    public static final SN_DoublePreference megaTag2StdDevsPosition = new SN_DoublePreference(
+        "megaTag2StdDevsPosition", 0.7);
+
+    public static final SN_DoublePreference megaTag2StdDevsHeading = new SN_DoublePreference(
+        "megaTag2StdDevsHeading", 9999999);
+
+    public static final SN_DoublePreference megaTag1StdDevsPosition = new SN_DoublePreference(
+        "megaTag1StdDevsPosition", 0.3);
+
+    public static final SN_DoublePreference megaTag1StdDevsHeading = new SN_DoublePreference(
+        "megaTag1StdDevsHeading", 0.1);
 
-    /**
-     * <p>
-     * Pose estimator standard deviation for vision data using Multi-Tag
-     * </p>
-     * <b>Units:</b> Radians
-     */
-    public static final SN_DoublePreference multiTagStdDevsHeading = new SN_DoublePreference(
-        "multiTagStdDevsHeading", 9999999);
   }
 
   public static final class prefIntake {

diff --git a/src/main/java/frc/robot/commands/Autos/WingOnly.java b/src/main/java/frc/robot/commands/Autos/WingOnly.java
@@ -63,10 +63,6 @@ public WingOnly(StateMachine subStateMachine, Climber subClimber, Drivetrain sub
     shootSequence = () -> new ShootSequence(subStateMachine, subClimber, subDrivetrain, subElevator, subIntake,
         subLEDs, subTransfer, subShooter, readyToShoot);
     addCommands(
-        // Resetting pose
-        Commands.runOnce(() -> subDrivetrain.resetPoseToPose(
-            getInitialPose().get())),
-
         // -- PRELOAD --
         Commands.runOnce(() -> subStateMachine.setTargetState(TargetState.PREP_VISION)),
 

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -79,9 +79,9 @@ public Drivetrain() {
             prefDrivetrain.measurementStdDevsPosition.getValue(),
             prefDrivetrain.measurementStdDevsHeading.getValue()),
         VecBuilder.fill(
-            prefVision.multiTagStdDevsPosition.getValue(),
-            prefVision.multiTagStdDevsPosition.getValue(),
-            prefVision.multiTagStdDevsHeading.getValue()),
+            prefVision.megaTag2StdDevsPosition.getValue(),
+            prefVision.megaTag2StdDevsPosition.getValue(),
+            prefVision.megaTag2StdDevsHeading.getValue()),
         new PIDConstants(prefDrivetrain.autoDriveP,
             prefDrivetrain.autoDriveI,
             prefDrivetrain.autoDriveD),

diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java
@@ -17,6 +17,7 @@
 
 public class Limelight extends SubsystemBase {
   PoseEstimate lastEstimate = new PoseEstimate();
+  private boolean useMegaTag2 = false;
 
   public Limelight() {
   }
@@ -25,6 +26,10 @@ public PoseEstimate getPoseEstimate() {
     return lastEstimate;
   }
 
+  public void setMegaTag2(boolean useMegaTag2) {
+    this.useMegaTag2 = useMegaTag2;
+  }
+
   /**
    * Determines if a given pose estimate should be rejected.
    * 
@@ -34,6 +39,11 @@ public PoseEstimate getPoseEstimate() {
    * @return True if the estimate should be rejected
    */
   public boolean rejectUpdate(PoseEstimate poseEstimate, Measure<Velocity<Angle>> gyroRate) {
+    // We only use MegaTag 1 in disabled, so we have full faith in our position
+    if (!useMegaTag2 && (poseEstimate.tagCount != 0)) {
+      return false;
+    }
+
     // Angular velocity is too high to have accurate vision
     if (gyroRate.compareTo(constLimelight.MAX_ANGULAR_VELOCITY) > 0) {
       return true;
@@ -56,7 +66,13 @@ public boolean rejectUpdate(PoseEstimate poseEstimate, Measure<Velocity<Angle>>
 
   @Override
   public void periodic() {
-    PoseEstimate currentEstimate = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("limelight");
+    PoseEstimate currentEstimate;
+    if (useMegaTag2) {
+      currentEstimate = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("limelight");
+    } else {
+      currentEstimate = LimelightHelpers.getBotPoseEstimate_wpiBlue("limelight");
+    }
+
     if (currentEstimate != null) {
       lastEstimate = currentEstimate;
     }