MegaTag के साथ स्थानीयकरण
- सुनिश्चित करें कि आपके Limelight में एक फील्ड मैप अपलोड किया गया है।
- पाइपलाइन 0 को एक Apriltag पाइपलाइन के रूप में कॉन्फ़िगर करें और 3D फंक्शनैलिटी को सक्षम करें।
- Limelight UI में अपने रोबोट के केंद्र के सापेक्ष अपने Limelight की स्थिति को कॉन्फ़िगर करें।
- नीचे दिए गए कोड को देखें। हम Limelight के अनुमानों को केवल तभी शामिल करते हैं जब एक से अधिक टैग दिखाई देते हैं। हम मापित रोटेशन के स्टैंडर्ड डेविएशन को एक बड़ी संख्या पर सेट करते हैं क्योंकि हम अपने IMU पर भरोसा कर सकते हैं कि वह हमें सही रोबोट हेडिंग दे।
- हम हमेशा botpose_wpiblue माप पढ़ते हैं क्योंकि सामान्य पाथप्लानिंग टूल्स ने ब्लू-कॉर्नर-ऑलवेज़ फील्ड ओरिजिन स्ट्रैटेजी को अपनाया है।
- यदि संभव हो तो 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; //MegaTag1 का उपयोग करने के लिए false पर सेट करें
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);
}
}
}