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

1. וודא שמפת השדה הועלתה ל-Limelight שלך.
2. הגדר את pipeline 0 כ-Apriltag Pipeline והפעל פונקציונליות תלת-מימדית
3. הגדר את המיקום של ה-Limelight ביחס למרכז הרובוט שלך בממשק המשתמש של Limelight
4. ראה את הקוד למטה. אנחנו משלבים את הערכות ה-Limelight רק כאשר יותר מתג אחד נראה. אנחנו מגדירים את סטיית התקן של הסיבוב הנמדד למספר גדול מכיוון שאנחנו יכולים לסמוך על ה-IMU שלנו שייתן לנו את כיוון הרובוט הנכון.
5. אנחנו תמיד קוראים את מדידת botpose_wpiblue מכיוון שכלי תכנון מסלול נפוצים אימצו אסטרטגיה של מקור שדה תמיד בפינה הכחולה.
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);
      }
    }
  }