Aiming at a Target
The following example is from the PhotonLib example repository (Java).
Knowledge and Equipment Needed
A Robot
A camera mounted rigidly to the robot’s frame, cenetered and pointed forward.
A coprocessor running PhotonVision with an AprilTag or Aurco 2D Pipeline.
A printout of AprilTag 7, mounted on a rigid and flat surface.
Code
Now that you have properly set up your vision system and have tuned a pipeline, you can now aim your robot at an AprilTag using the data from PhotonVision. The yaw of the target is the critical piece of data that will be needed first.
Yaw is reported to the roboRIO over Network Tables. PhotonLib, our vender dependency, is the easiest way to access this data. The documentation for the Network Tables API can be found here and the documentation for PhotonLib here.
In this example, while the operator holds a button down, the robot will turn towards the AprilTag using the P term of a PID loop. To learn more about how PID loops work, how WPILib implements them, and more, visit Advanced Controls (PID) and PID Control in WPILib.
77 @Override
78 public void teleopPeriodic() {
79 // Calculate drivetrain commands from Joystick values
80 double forward = -controller.getLeftY() * Constants.Swerve.kMaxLinearSpeed;
81 double strafe = -controller.getLeftX() * Constants.Swerve.kMaxLinearSpeed;
82 double turn = -controller.getRightX() * Constants.Swerve.kMaxAngularSpeed;
83
84 // Read in relevant data from the Camera
85 boolean targetVisible = false;
86 double targetYaw = 0.0;
87 var results = camera.getAllUnreadResults();
88 if (!results.isEmpty()) {
89 // Camera processed a new frame since last
90 // Get the last one in the list.
91 var result = results.get(results.size() - 1);
92 if (result.hasTargets()) {
93 // At least one AprilTag was seen by the camera
94 for (var target : result.getTargets()) {
95 if (target.getFiducialId() == 7) {
96 // Found Tag 7, record its information
97 targetYaw = target.getYaw();
98 targetVisible = true;
99 }
100 }
101 }
102 }
103
104 // Auto-align when requested
105 if (controller.getAButton() && targetVisible) {
106 // Driver wants auto-alignment to tag 7
107 // And, tag 7 is in sight, so we can turn toward it.
108 // Override the driver's turn command with an automatic one that turns toward the tag.
109 turn = -1.0 * targetYaw * VISION_TURN_kP * Constants.Swerve.kMaxAngularSpeed;
110 }
111
112 // Command drivetrain motors based on target speeds
113 drivetrain.drive(forward, strafe, turn);
114
115 // Put debug information to the dashboard
116 SmartDashboard.putBoolean("Vision Target Visible", targetVisible);
117 }
25#pragma once
26
27#include <photon/PhotonCamera.h>
28
29#include <frc/TimedRobot.h>
30#include <frc/XboxController.h>
31
32#include "Constants.h"
33#include "VisionSim.h"
34#include "subsystems/SwerveDrive.h"
35
36class Robot : public frc::TimedRobot {
37 public:
38 void RobotInit() override;
39 void RobotPeriodic() override;
40 void DisabledInit() override;
41 void DisabledPeriodic() override;
42 void DisabledExit() override;
43 void AutonomousInit() override;
44 void AutonomousPeriodic() override;
45 void AutonomousExit() override;
46 void TeleopInit() override;
47 void TeleopPeriodic() override;
48 void TeleopExit() override;
49 void TestInit() override;
50 void TestPeriodic() override;
51 void TestExit() override;
52 void SimulationPeriodic() override;
53
54 private:
55 photon::PhotonCamera camera{constants::Vision::kCameraName};
56 SwerveDrive drivetrain{};
57 VisionSim vision{&camera};
58 frc::XboxController controller{0};
59 static constexpr double VISION_TURN_kP = 0.01;
60};
56void Robot::TeleopPeriodic() {
57 // Calculate drivetrain commands from Joystick values
58 auto forward =
59 -1.0 * controller.GetLeftY() * constants::Swerve::kMaxLinearSpeed;
60 auto strafe =
61 -1.0 * controller.GetLeftX() * constants::Swerve::kMaxLinearSpeed;
62 auto turn =
63 -1.0 * controller.GetRightX() * constants::Swerve::kMaxAngularSpeed;
64
65 bool targetVisible = false;
66 double targetYaw = 0.0;
67 auto results = camera.GetAllUnreadResults();
68 if (results.size() > 0) {
69 // Camera processed a new frame since last
70 // Get the last one in the list.
71 auto result = results[results.size() - 1];
72 if (result.HasTargets()) {
73 // At least one AprilTag was seen by the camera
74 for (auto& target : result.GetTargets()) {
75 if (target.GetFiducialId() == 7) {
76 // Found Tag 7, record its information
77 targetYaw = target.GetYaw();
78 targetVisible = true;
79 }
80 }
81 }
82 }
83
84 // Auto-align when requested
85 if (controller.GetAButton() && targetVisible) {
86 // Driver wants auto-alignment to tag 7
87 // And, tag 7 is in sight, so we can turn toward it.
88 // Override the driver's turn command with an automatic one that turns
89 // toward the tag.
90 turn =
91 -1.0 * targetYaw * VISION_TURN_kP * constants::Swerve::kMaxAngularSpeed;
92 }
93
94 // Command drivetrain motors based on target speeds
95 drivetrain.Drive(forward, strafe, turn);
96}
46 def teleopPeriodic(self) -> None:
47 xSpeed = -1.0 * self.controller.getLeftY() * drivetrain.kMaxSpeed
48 ySpeed = -1.0 * self.controller.getLeftX() * drivetrain.kMaxSpeed
49 rot = -1.0 * self.controller.getRightX() * drivetrain.kMaxAngularSpeed
50
51 # Get information from the camera
52 targetYaw = 0.0
53 targetVisible = False
54 results = self.cam.getAllUnreadResults()
55 if len(results) > 0:
56 result = results[-1] # take the most recent result the camera had
57 for target in result.getTargets():
58 if target.getFiducialId() == 7:
59 # Found tag 7, record its information
60 targetVisible = True
61 targetYaw = target.getYaw()
62
63 if self.controller.getAButton() and targetVisible:
64 # Driver wants auto-alignment to tag 7
65 # And, tag 7 is in sight, so we can turn toward it.
66 # Override the driver's turn command with an automatic one that turns toward the tag.
67 rot = -1.0 * targetYaw * VISION_TURN_kP * drivetrain.kMaxAngularSpeed
68
69 self.swerve.drive(xSpeed, ySpeed, rot, True, self.getPeriod())
70