Aiming at a Target
The following example is from the PhotonLib example repository (Java/C++).
Knowledge and Equipment Needed
Robot with a vision system running PhotonVision
Target
Ability to track a target by properly tuning a pipeline
Code
Now that you have properly set up your vision system and have tuned a pipeline, you can now aim your robot/turret at the target using the data from PhotonVision. This data is reported over NetworkTables and includes: latency, whether there is a target detected or not, pitch, yaw, area, skew, and target pose relative to the robot. This data will be used/manipulated by our vendor dependency, PhotonLib. The documentation for the Network Tables API can be found here and the documentation for PhotonLib here.
For this simple example, only yaw is needed.
In this example, while the operator holds a button down, the robot will turn towards the goal 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.
41public class Robot extends TimedRobot {
42 // Constants such as camera and target height stored. Change per robot and goal!
43 final double CAMERA_HEIGHT_METERS = Units.inchesToMeters(24);
44 final double TARGET_HEIGHT_METERS = Units.feetToMeters(5);
45 // Angle between horizontal and the camera.
46 final double CAMERA_PITCH_RADIANS = Units.degreesToRadians(0);
47
48 // How far from the target we want to be
49 final double GOAL_RANGE_METERS = Units.feetToMeters(3);
50
51 // Change this to match the name of your camera
52 PhotonCamera camera = new PhotonCamera("photonvision");
53
54 // PID constants should be tuned per robot
55 final double LINEAR_P = 0.1;
56 final double LINEAR_D = 0.0;
57 PIDController forwardController = new PIDController(LINEAR_P, 0, LINEAR_D);
58
59 final double ANGULAR_P = 0.1;
60 final double ANGULAR_D = 0.0;
61 PIDController turnController = new PIDController(ANGULAR_P, 0, ANGULAR_D);
62
63 XboxController xboxController = new XboxController(0);
64
65 // Drive motors
66 PWMVictorSPX leftMotor = new PWMVictorSPX(0);
67 PWMVictorSPX rightMotor = new PWMVictorSPX(1);
68 DifferentialDrive drive = new DifferentialDrive(leftMotor, rightMotor);
69
70 @Override
71 public void teleopPeriodic() {
72 double forwardSpeed;
73 double rotationSpeed;
74
75 forwardSpeed = -xboxController.getRightY();
76
77 if (xboxController.getAButton()) {
78 // Vision-alignment mode
79 // Query the latest result from PhotonVision
80 var result = camera.getLatestResult();
81
82 if (result.hasTargets()) {
83 // Calculate angular turn power
84 // -1.0 required to ensure positive PID controller effort _increases_ yaw
85 rotationSpeed = -turnController.calculate(result.getBestTarget().getYaw(), 0);
86 } else {
87 // If we have no targets, stay still.
88 rotationSpeed = 0;
89 }
90 } else {
91 // Manual Driver Mode
92 rotationSpeed = xboxController.getLeftX();
93 }
94
95 // Use our forward/turn speeds to control the drivetrain
96 drive.arcadeDrive(forwardSpeed, rotationSpeed);
97 }
98}
27#include <photonlib/PhotonCamera.h>
28
29#include <frc/TimedRobot.h>
30#include <frc/XboxController.h>
31#include <frc/controller/PIDController.h>
32#include <frc/drive/DifferentialDrive.h>
33#include <frc/motorcontrol/PWMVictorSPX.h>
34#include <units/angle.h>
35#include <units/length.h>
36
37class Robot : public frc::TimedRobot {
38 public:
39 void TeleopPeriodic() override;
40
41 private:
42 // Change this to match the name of your camera
43 photonlib::PhotonCamera camera{"photonvision"};
44 // PID constants should be tuned per robot
45 frc2::PIDController controller{.1, 0, 0};
46
47 frc::XboxController xboxController{0};
48
49 // Drive motors
50 frc::PWMVictorSPX leftMotor{0};
51 frc::PWMVictorSPX rightMotor{1};
52 frc::DifferentialDrive drive{leftMotor, rightMotor};
53};
25#include "Robot.h"
26
27#include <photonlib/PhotonUtils.h>
28
29void Robot::TeleopPeriodic() {
30 double forwardSpeed = -xboxController.GetRightY();
31 double rotationSpeed;
32
33 if (xboxController.GetAButton()) {
34 // Vision-alignment mode
35 // Query the latest result from PhotonVision
36 photonlib::PhotonPipelineResult result = camera.GetLatestResult();
37
38 if (result.HasTargets()) {
39 // Rotation speed is the output of the PID controller
40 rotationSpeed = -controller.Calculate(result.GetBestTarget().GetYaw(), 0);
41 } else {
42 // If we have no targets, stay still.
43 rotationSpeed = 0;
44 }
45 } else {
46 // Manual Driver Mode
47 rotationSpeed = xboxController.GetLeftX();
48 }
49
50 // Use our forward/turn speeds to control the drivetrain
51 drive.ArcadeDrive(forwardSpeed, rotationSpeed);
52}