Closed agasser closed 8 months ago
Here's an example how usage would look:
var estimate = LimelightHelpers.getBotPoseEstimate_wpiBlue("");
poseEstimator.addVisionMeasurement(estimate.pose, estimate.timestampSeconds);
Without this PR, the usage would look something like this:
var poseEntry = LimelightHelpers.getLimelightNTTableEntry("limelight", "botpose_wpiblue");
var poseArray = poseEntry.getDoubleArray(new double[0]);
var timestamp = poseEntry.getLastChange() / 1e6 - poseArray[6] / 1e3;
var pose = new Pose2d(
new Translation2d(poseArray[0], poseArray[1]),
new Rotation2d(Units.degreesToRadians(poseArray[5])));
poseEstimator.addVisionMeasurement(pose, timestamp);
cool!
This feature implements #21
I do not have access to a Limelight right now, so this is untested.