לוקליזציה עם MegaTag

1. ודאו שמפת שדה הועלתה ל-Limelight שלכם.
2. הגדירו pipeline 0 כ-Apriltag Pipeline והפעילו פונקציונליות 3D
3. הגדירו את מיקום ה-Limelight ביחס למרכז הרובוט שלכם בממשק המשתמש של Limelight
4. ראו את הקוד למטה. אנחנו משלבים את ההערכות של Limelight רק כאשר יותר מתג אחד נראה. אנחנו מגדירים את סטיית התקן של הסיבוב הנמדד למספר גדול כי אנחנו יכולים לסמוך על ה-IMU שלנו שייתן לנו את כיוון הרובוט הנכון.
5. אנחנו תמיד קוראים את המדידה botpose_wpiblue כי כלי pathplanning נפוצים אימצו אסטרטגיית מקור שדה בפינה הכחולה תמיד.
6. השתמשו ב-megatag 2 אם אפשר.

ראו את פרויקט הדוגמה המלא כאן

  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; // הגדירו ל-false כדי להשתמש ב-MegaTag1
    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) // אם המהירות הזוויתית שלנו גדולה מ-720 מעלות לשנייה, התעלמו מעדכוני ראייה
      {
        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);
      }
    }
  }