Aller au contenu principal

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. Consultez le code ci-dessous. Nous n'intégrons les estimations de Limelight que lorsque plus d'un tag est visible. Nous définissons l'écart-type de la rotation mesurée à un nombre élevé 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 de terrain toujours dans le coin bleu.
  6. Utilisez megatag 2 si possible.

Consultez le projet d'exemple 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, ignorez 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);
}
}
}