diff --git a/photonlib-cpp-examples/aimattarget/src/main/cpp/Robot.cpp b/photonlib-cpp-examples/aimattarget/src/main/cpp/Robot.cpp index 56a7b6c0d7..140af4563c 100644 --- a/photonlib-cpp-examples/aimattarget/src/main/cpp/Robot.cpp +++ b/photonlib-cpp-examples/aimattarget/src/main/cpp/Robot.cpp @@ -62,9 +62,37 @@ void Robot::TeleopPeriodic() { auto turn = -1.0 * controller.GetRightX() * constants::Swerve::kMaxAngularSpeed; + bool targetVisible = false; + double targetYaw = 0.0; + auto results = camera.GetAllUnreadResults(); + if (results.size() > 0) { + // Camera processed a new frame since last + // Get the last one in the list. + auto result = results[results.size() - 1]; + if (result.HasTargets()) { + // At least one AprilTag was seen by the camera + for (auto& target : result.GetTargets()) { + if (target.GetFiducialId() == 7) { + // Found Tag 7, record its information + targetYaw = target.GetYaw(); + targetVisible = true; + } + } + } + } + + // Auto-align when requested + if (controller.GetAButton() && targetVisible) { + // Driver wants auto-alignment to tag 7 + // And, tag 7 is in sight, so we can turn toward it. + // Override the driver's turn command with an automatic one that turns + // toward the tag. + turn = + -1.0 * targetYaw * VISION_TURN_kP * constants::Swerve::kMaxAngularSpeed; + } + // Command drivetrain motors based on target speeds drivetrain.Drive(forward, strafe, turn); - } void Robot::TeleopExit() {} diff --git a/photonlib-cpp-examples/aimattarget/src/main/include/Constants.h b/photonlib-cpp-examples/aimattarget/src/main/include/Constants.h index 98e1d5b1db..6a20d67991 100644 --- a/photonlib-cpp-examples/aimattarget/src/main/include/Constants.h +++ b/photonlib-cpp-examples/aimattarget/src/main/include/Constants.h @@ -38,7 +38,7 @@ namespace Vision { inline constexpr std::string_view kCameraName{"YOUR CAMERA NAME"}; inline const frc::Transform3d kRobotToCam{ frc::Translation3d{0.5_m, 0.0_m, 0.5_m}, - frc::Rotation3d{0_rad, 0_rad, 0_rad}}; + frc::Rotation3d{0_rad, -30_deg, 0_rad}}; inline const frc::AprilTagFieldLayout kTagLayout{ frc::LoadAprilTagLayoutField(frc::AprilTagField::k2024Crescendo)}; diff --git a/photonlib-cpp-examples/aimattarget/src/main/include/Robot.h b/photonlib-cpp-examples/aimattarget/src/main/include/Robot.h index d9eedc9097..b60bcd678d 100644 --- a/photonlib-cpp-examples/aimattarget/src/main/include/Robot.h +++ b/photonlib-cpp-examples/aimattarget/src/main/include/Robot.h @@ -27,7 +27,8 @@ #include #include -#include "Vision.h" +#include "VisionSim.h" +#include "Constants.h" #include #include "subsystems/SwerveDrive.h" @@ -51,8 +52,9 @@ class Robot : public frc::TimedRobot { void SimulationPeriodic() override; private: - photon::PhotonCamera camera{"photonvision"}; + photon::PhotonCamera camera{constants::Vision::kCameraName}; SwerveDrive drivetrain{}; - Vision vision{&camera}; + VisionSim vision{&camera}; frc::XboxController controller{0}; + static constexpr double VISION_TURN_kP = 0.01; }; diff --git a/photonlib-cpp-examples/aimattarget/src/main/include/Vision.h b/photonlib-cpp-examples/aimattarget/src/main/include/VisionSim.h similarity index 93% rename from photonlib-cpp-examples/aimattarget/src/main/include/Vision.h rename to photonlib-cpp-examples/aimattarget/src/main/include/VisionSim.h index 1efb300048..3d4a407e42 100644 --- a/photonlib-cpp-examples/aimattarget/src/main/include/Vision.h +++ b/photonlib-cpp-examples/aimattarget/src/main/include/VisionSim.h @@ -39,9 +39,9 @@ #include "Constants.h" -class Vision { +class VisionSim { public: - Vision(photon::PhotonCamera* camera) { + VisionSim(photon::PhotonCamera* camera) { if (frc::RobotBase::IsSimulation()) { visionSim = std::make_unique("main"); @@ -50,10 +50,10 @@ class Vision { cameraProp = std::make_unique(); - cameraProp->SetCalibration(960, 720, frc::Rotation2d{90_deg}); + cameraProp->SetCalibration(320, 240, frc::Rotation2d{90_deg}); cameraProp->SetCalibError(.35, .10); - cameraProp->SetFPS(15_Hz); - cameraProp->SetAvgLatency(50_ms); + cameraProp->SetFPS(70_Hz); + cameraProp->SetAvgLatency(30_ms); cameraProp->SetLatencyStdDev(15_ms); cameraSim = diff --git a/photonlib-cpp-examples/poseest/src/main/include/Vision.h b/photonlib-cpp-examples/poseest/src/main/include/Vision.h index 200bc43b59..af3713389e 100644 --- a/photonlib-cpp-examples/poseest/src/main/include/Vision.h +++ b/photonlib-cpp-examples/poseest/src/main/include/Vision.h @@ -142,7 +142,7 @@ class Vision { constants::Vision::kTagLayout, photon::PoseStrategy::MULTI_TAG_PNP_ON_COPROCESSOR, constants::Vision::kRobotToCam}; - photon::PhotonCamera camera{"photonvision"}; + photon::PhotonCamera camera{constants::Vision::kCameraName}; std::unique_ptr visionSim; std::unique_ptr cameraProp; std::shared_ptr cameraSim;