MegaTag ile Lokalizasyon

1. Limelight'ınıza bir saha haritasının yüklendiğinden emin olun.
2. Pipeline 0'ı bir Apriltag Pipeline'ı olarak yapılandırın ve 3D işlevselliğini etkinleştirin.
3. Limelight'ınızın robotunuzun merkezine göre konumunu Limelight kullanıcı arayüzünde yapılandırın.
4. Aşağıdaki kodu inceleyin. Limelight'ın tahminlerini yalnızca birden fazla etiket görünür olduğunda dahil ediyoruz. Ölçülen dönüşün standart sapmasını büyük bir sayıya ayarlıyoruz çünkü IMU'muzun bize doğru robot yönünü vereceğine güvenebiliriz.
5. Her zaman botpose_wpiblue ölçümünü okuyoruz çünkü yaygın yol planlama araçları mavi-köşe-her zaman saha orijini stratejisini benimsemiştir.
6. Mümkünse megatag 2'yi 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) // eğer açısal hızımız saniyede 720 dereceden fazlaysa, görüş güncellemelerini görmezden gel
      {
        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);
      }
    }
  }