Localisation avec MegaTag

1. Assurez-vous qu'une carte de terrain est téléchargée dans votre Limelight.
2. Configurez le pipeline 0 comme un Pipeline Apriltag et activez la fonctionnalité 3D.
3. Configurez la pose de votre Limelight par rapport au centre de votre robot dans l'interface utilisateur Limelight.
4. Voir le code ci-dessous. Nous n'incorporons les estimations de Limelight que lorsque plus d'un tag est visible. Nous définissons l'écart-type de la rotation mesurée à un grand nombre car nous pouvons faire confiance à notre IMU pour nous donner le bon cap du robot.
5. Nous lisons toujours la mesure botpose_wpiblue car les outils courants de planification de trajectoire ont adopté une stratégie d'origine du terrain toujours dans le coin bleu.
6. Utilisez megatag 2 si possible.

Voir le projet complet ici

  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; //mettre à false pour utiliser 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) // si notre vitesse angulaire est supérieure à 720 degrés par seconde, ignorer les mises à jour de vision
      {
        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);
      }
    }
  }