diff --git a/src/main/cpp/AprilTagPoseEstimator.cpp b/src/main/cpp/AprilTagPoseEstimator.cpp index ca3a053..c989a45 100644 --- a/src/main/cpp/AprilTagPoseEstimator.cpp +++ b/src/main/cpp/AprilTagPoseEstimator.cpp @@ -4,13 +4,13 @@ using namespace frc; // this function expects to get the april tag the april tag pose reading in the following format: -// x - 1st value from jetson -// y - 2nd value from jetson -// z - 3rd value from jetson -// roll (x, 1st value in rotation3d) - 4th value from jetson -// pitch (y, 2nd value in rotation3d) - 5th value from jetson -// yaw (z, 3rd value in rotation3d) - 6th value from jetson -// aprilTagNum: april tag number +// x - 3rd value from jetson +// y - 4th value from jetson +// z - doesn't matter, can be left 0 +// roll (x, 1st value in rotation3d) - doesn't matter, can be left 0 +// pitch (y, 2nd value in rotation3d) - doesn't matter, can be left 0 +// yaw (z, 3rd value in rotation3d) - 4th value from jetson +// aprilTagNum: april tag number - 2nd value from jetson Pose2d AprilTagPoseEstimator::getPose(Pose3d aprilTagPosReading, int aprilTagNum) { units::meter_t x = aprilTagPosReading.X(); units::meter_t y = aprilTagPosReading.Y();