diff --git a/src/main/cpp/AprilTagPoseEstimator.cpp b/src/main/cpp/AprilTagPoseEstimator.cpp deleted file mode 100644 index ca3a053..0000000 --- a/src/main/cpp/AprilTagPoseEstimator.cpp +++ /dev/null @@ -1,39 +0,0 @@ -#include "AprilTagPoseEstimator.h" -#include - -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 -Pose2d AprilTagPoseEstimator::getPose(Pose3d aprilTagPosReading, int aprilTagNum) { - units::meter_t x = aprilTagPosReading.X(); - units::meter_t y = aprilTagPosReading.Y(); - units::radian_t gamma = aprilTagPosReading.Rotation().Z(); - - Pose3d aprilTagPos = aprilTagFieldLayout.GetTagPose(aprilTagNum).value(); - - // apriltag rotation - units::radian_t aprilTagRot = aprilTagPos.Rotation().ToRotation2d().Radians(); - - // calculate rotation sine and cosine values - units::radian_t rotAngle = aprilTagRot - Rotation2d(90_deg).Radians() + gamma; - double cosVal = std::cos(unit_cast(rotAngle)); - double sinVal = std::sin(unit_cast(rotAngle)); - - // calculated translated x and y values (robot pose relative to apriltag with correct coordinates) - units::meter_t translatedX = x * cosVal - y * sinVal; - units::meter_t translatedY = x * sinVal + y * cosVal; - - // add april tag location coordinates to get robot position - units::meter_t robotX = translatedX + aprilTagPos.X(); - units::meter_t robotY = translatedY + aprilTagPos.Y(); - units::radian_t robotRot = aprilTagRot + gamma - Rotation2d(180_deg).Radians(); - - return Pose2d{robotX, robotY, robotRot}; -} diff --git a/src/main/cpp/AprilTags.cpp b/src/main/cpp/AprilTags.cpp deleted file mode 100644 index a131124..0000000 --- a/src/main/cpp/AprilTags.cpp +++ /dev/null @@ -1,124 +0,0 @@ -#include "AprilTags.h" -#include -#include -#include -#include "Constants.h" - - -using namespace units::length; -using namespace frc; - -frc::AprilTagFieldLayout AprilTags::getAprilTagFieldLayout() { - // if blue alliance or invalid - if (DriverStation::GetAlliance() != DriverStation::Alliance::kRed) { - AprilTag tag1(1, Pose3d(Pose2d( - AprilTagsConstants::FIELD_LENGTH - AprilTagsConstants::GRID_APRILTAG_X, - AprilTagsConstants::GRID_APRILTAG_LOW_Y, - Rotation2d(180_deg) - ))); - AprilTag tag2(2, Pose3d(Pose2d( - AprilTagsConstants::FIELD_LENGTH - AprilTagsConstants::GRID_APRILTAG_X, - AprilTagsConstants::GRID_APRILTAG_MID_Y, - Rotation2d(180_deg) - ))); - AprilTag tag3(3, Pose3d(Pose2d( - AprilTagsConstants::FIELD_LENGTH - AprilTagsConstants::GRID_APRILTAG_X, - AprilTagsConstants::GRID_APRILTAG_TOP_Y, - Rotation2d(180_deg) - ))); - AprilTag tag4(4, Pose3d(Pose2d( - AprilTagsConstants::FIELD_LENGTH - AprilTagsConstants::DOUBLE_SUBSTATION_APRILTAG_X, - AprilTagsConstants::DOUBLE_SUBSTATION_APRILTAG_Y, - Rotation2d(180_deg) - ))); - AprilTag tag5(5, Pose3d(Pose2d( - AprilTagsConstants::DOUBLE_SUBSTATION_APRILTAG_X, - AprilTagsConstants::DOUBLE_SUBSTATION_APRILTAG_Y, - Rotation2d(0_deg) - ))); - AprilTag tag6(6, Pose3d(Pose2d( - AprilTagsConstants::GRID_APRILTAG_X, - AprilTagsConstants::GRID_APRILTAG_TOP_Y, - Rotation2d(0_deg) - ))); - AprilTag tag7(7, Pose3d(Pose2d( - AprilTagsConstants::GRID_APRILTAG_X, - AprilTagsConstants::GRID_APRILTAG_MID_Y, - Rotation2d(0_deg) - ))); - AprilTag tag8(8, Pose3d(Pose2d( - AprilTagsConstants::GRID_APRILTAG_X, - AprilTagsConstants::GRID_APRILTAG_LOW_Y, - Rotation2d(0_deg) - ))); - std::vector aprilTags; - aprilTags.push_back(tag1); - aprilTags.push_back(tag2); - aprilTags.push_back(tag3); - aprilTags.push_back(tag4); - aprilTags.push_back(tag5); - aprilTags.push_back(tag6); - aprilTags.push_back(tag7); - aprilTags.push_back(tag8); - AprilTagFieldLayout layout( - aprilTags, - AprilTagsConstants::FIELD_LENGTH, AprilTagsConstants::FIELD_WIDTH - ); - return layout; - } else { // if red alliance - AprilTag tag1(1, Pose3d(Pose2d( - AprilTagsConstants::GRID_APRILTAG_X, - AprilTagsConstants::GRID_APRILTAG_LOW_Y, - Rotation2d(0_deg) - ))); - AprilTag tag2(2, Pose3d(Pose2d( - AprilTagsConstants::GRID_APRILTAG_X, - AprilTagsConstants::GRID_APRILTAG_MID_Y, - Rotation2d(0_deg) - ))); - AprilTag tag3(3, Pose3d(Pose2d( - AprilTagsConstants::GRID_APRILTAG_X, - AprilTagsConstants::GRID_APRILTAG_TOP_Y, - Rotation2d(0_deg) - ))); - AprilTag tag4(4, Pose3d(Pose2d( - AprilTagsConstants::DOUBLE_SUBSTATION_APRILTAG_X, - AprilTagsConstants::DOUBLE_SUBSTATION_APRILTAG_Y, - Rotation2d(0_deg) - ))); - AprilTag tag5(5, Pose3d(Pose2d( - AprilTagsConstants::FIELD_LENGTH - AprilTagsConstants::DOUBLE_SUBSTATION_APRILTAG_X, - AprilTagsConstants::DOUBLE_SUBSTATION_APRILTAG_Y, - Rotation2d(180_deg) - ))); - AprilTag tag6(6, Pose3d(Pose2d( - AprilTagsConstants::FIELD_LENGTH - AprilTagsConstants::GRID_APRILTAG_X, - AprilTagsConstants::GRID_APRILTAG_TOP_Y, - Rotation2d(180_deg) - ))); - AprilTag tag7(7, Pose3d(Pose2d( - AprilTagsConstants::FIELD_LENGTH - AprilTagsConstants::GRID_APRILTAG_X, - AprilTagsConstants::GRID_APRILTAG_MID_Y, - Rotation2d(180_deg) - ))); - AprilTag tag8(8, Pose3d(Pose2d( - AprilTagsConstants::FIELD_LENGTH - AprilTagsConstants::GRID_APRILTAG_X, - AprilTagsConstants::GRID_APRILTAG_LOW_Y, - Rotation2d(180_deg) - ))); - std::vector aprilTags; - aprilTags.push_back(tag1); - aprilTags.push_back(tag2); - aprilTags.push_back(tag3); - aprilTags.push_back(tag4); - aprilTags.push_back(tag5); - aprilTags.push_back(tag6); - aprilTags.push_back(tag7); - aprilTags.push_back(tag8); - AprilTagFieldLayout layout( - aprilTags, - AprilTagsConstants::FIELD_LENGTH, AprilTagsConstants::FIELD_WIDTH - ); - return layout; - } -} \ No newline at end of file diff --git a/src/main/include/AprilTagPoseEstimator.h b/src/main/include/AprilTagPoseEstimator.h deleted file mode 100644 index 6b07b8a..0000000 --- a/src/main/include/AprilTagPoseEstimator.h +++ /dev/null @@ -1,19 +0,0 @@ -#include -#include -#include -#include "AprilTags.h" - - -class AprilTagPoseEstimator { -public: - /** - * Returns the robot position based on an april tag position reading. - * @param aprilTagPosReading april tag position reading @see AprilTagPoseEstimator.cpp for more details about this argument - * @param aprilTagNum april tag number - * @return robot position - */ - frc::Pose2d getPose(frc::Pose3d aprilTagPosReading, int aprilTagNum); - -private: - frc::AprilTagFieldLayout aprilTagFieldLayout = AprilTags::getAprilTagFieldLayout(); -}; \ No newline at end of file diff --git a/src/main/include/AprilTags.h b/src/main/include/AprilTags.h deleted file mode 100644 index b821b47..0000000 --- a/src/main/include/AprilTags.h +++ /dev/null @@ -1,10 +0,0 @@ -#include - -class AprilTags { -public: - /** - * Returns the april tag field layout of the 2023 Charged Up game. - * @return april tag field layout - */ - static frc::AprilTagFieldLayout getAprilTagFieldLayout(); -}; \ No newline at end of file