Ok so I am trying to get some pose estimation with the robot going. We have it tracking april tags and giving us good pose2d from that.
We are using the Pheonix 6 Swerve base code that is generated using the swerve generator.
It appears to me in the Telemetry that it publishes the robots Pose to the networktables as a double array. I am unable to retrieve that Pose.
I also see that there is an addVisionMeasurement method which leads me to believe there is tracking of the robots Odometry in the drivetrain already I am just struggling to retrieve the Pose2d.
I would like to addVisionMeasurement when we see a good target and the pose isnt way off so we can use that pose for other alignment things in our program.
Forgive my lack of proper terminology as I am not a programmer by trade 