MegaTag ile Lokalizasyon

1. Limelight'ınıza bir saha haritası yüklendiğinden emin olun.
2. Pipeline 0'ı Apriltag Pipeline olarak yapılandırın ve 3D işlevselliğini etkinleştirin
3. Limelight'ınızın robotunuzun merkezine göre pozisyonunu Limelight arayüzünde yapılandırın
4. Aşağıdaki koda bakın. Limelight tahminlerini yalnızca birden fazla etiket görünür olduğunda dahil ediyoruz. Ölçülen rotasyonun standart sapmasını büyük bir sayıya ayarlıyoruz çünkü doğru robot yönelimi için IMU'muza güvenebiliriz.
5. Yaygın yol planlama araçları mavi köşe sabit saha orijini stratejisini benimsediği için her zaman botpose_wpiblue ölçümünü okuyoruz.
6. Mümkünse megatag 2 kullanın.

Tam örnek projeyi buradan görebilirsiniz

  public void updateOdometry() {
    m_poseEstimator.update(
        m_gyro.getRotation2d(),
        new SwerveModulePosition[] {
          m_frontLeft.getPosition(),
          m_frontRight.getPosition(),
          m_backLeft.getPosition(),
          m_backRight.getPosition()
        });


    boolean useMegaTag2 = true; // MegaTag1 kullanmak için false olarak ayarlayın
    boolean doRejectUpdate = false;
    if(useMegaTag2 == false)
    {
      LimelightHelpers.PoseEstimate mt1 = LimelightHelpers.getBotPoseEstimate_wpiBlue("limelight");
      
      if(mt1.tagCount == 1 && mt1.rawFiducials.length == 1)
      {
        if(mt1.rawFiducials[0].ambiguity > .7)
        {
          doRejectUpdate = true;
        }
        if(mt1.rawFiducials[0].distToCamera > 3)
        {
          doRejectUpdate = true;
        }
      }
      if(mt1.tagCount == 0)
      {
        doRejectUpdate = true;
      }

      if(!doRejectUpdate)
      {
        m_poseEstimator.setVisionMeasurementStdDevs(VecBuilder.fill(.5,.5,9999999));
        m_poseEstimator.addVisionMeasurement(
            mt1.pose,
            mt1.timestampSeconds);
      }
    }
    else if (useMegaTag2 == true)
    {
      LimelightHelpers.SetRobotOrientation("limelight", m_poseEstimator.getEstimatedPosition().getRotation().getDegrees(), 0, 0, 0, 0, 0);
      LimelightHelpers.PoseEstimate mt2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("limelight");
      if(Math.abs(m_gyro.getRate()) > 720) // açısal hızımız saniyede 720 dereceden büyükse, görüş güncellemelerini yoksay
      {
        doRejectUpdate = true;
      }
      if(mt2.tagCount == 0)
      {
        doRejectUpdate = true;
      }
      if(!doRejectUpdate)
      {
        m_poseEstimator.setVisionMeasurementStdDevs(VecBuilder.fill(.7,.7,9999999));
        m_poseEstimator.addVisionMeasurement(
            mt2.pose,
            mt2.timestampSeconds);
      }
    }
  }