AimAtTarget working

PhotonVision · Sep 6, 2024 · 8c0b01f · 8c0b01f
1 parent 40200dc
commit 8c0b01f
Show file tree

Hide file tree

Showing 5 changed files with 41 additions and 11 deletions.
diff --git 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
@@ -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
@@ -27,7 +27,8 @@
 #include <frc/TimedRobot.h>
 #include <frc/XboxController.h>
 
-#include "Vision.h"
+#include "VisionSim.h"
+#include "Constants.h"
 #include <photon/PhotonCamera.h>
 
 #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/...les/aimattarget/src/main/include/Vision.h → .../aimattarget/src/main/include/VisionSim.h b/...les/aimattarget/src/main/include/Vision.h → .../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<photon::VisionSystemSim>("main");
@@ -50,10 +50,10 @@ class Vision {
 
       cameraProp = std::make_unique<photon::SimCameraProperties>();
 
-      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
@@ -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<photon::VisionSystemSim> visionSim;
   std::unique_ptr<photon::SimCameraProperties> cameraProp;
   std::shared_ptr<photon::PhotonCameraSim> cameraSim;