التوطين باستخدام MegaTag

1. تأكد من تحميل خريطة الحقل إلى جهاز Limelight الخاص بك.
2. قم بتكوين خط الأنابيب 0 كخط أنابيب Apriltag وتمكين وظائف الأبعاد الثلاثية.
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);
      }
    }
  }