Combining Aiming and Getting in Range
The following example is from the PhotonLib example repository (Java/C++).
Knowledge and Equipment Needed
Everything required in Aiming at a Target and Getting in Range of the Target.
Code
Now that you know how to both aim and get in range of the target, it is time to combine them both at the same time. This example will take the previous two code examples and make them into one function using the same tools as before. With this example, you now have all the knowledge you need to use PhotonVision on your robot in any game.
42public class Robot extends TimedRobot {
43 // Constants such as camera and target height stored. Change per robot and goal!
44 final double CAMERA_HEIGHT_METERS = Units.inchesToMeters(24);
45 final double TARGET_HEIGHT_METERS = Units.feetToMeters(5);
46 // Angle between horizontal and the camera.
47 final double CAMERA_PITCH_RADIANS = Units.degreesToRadians(0);
48
49 // How far from the target we want to be
50 final double GOAL_RANGE_METERS = Units.feetToMeters(3);
51
52 // Change this to match the name of your camera
53 PhotonCamera camera = new PhotonCamera("photonvision");
54
55 // PID constants should be tuned per robot
56 final double LINEAR_P = 0.1;
57 final double LINEAR_D = 0.0;
58 PIDController forwardController = new PIDController(LINEAR_P, 0, LINEAR_D);
59
60 final double ANGULAR_P = 0.1;
61 final double ANGULAR_D = 0.0;
62 PIDController turnController = new PIDController(ANGULAR_P, 0, ANGULAR_D);
63
64 XboxController xboxController = new XboxController(0);
65
66 // Drive motors
67 PWMVictorSPX leftMotor = new PWMVictorSPX(0);
68 PWMVictorSPX rightMotor = new PWMVictorSPX(1);
69 DifferentialDrive drive = new DifferentialDrive(leftMotor, rightMotor);
70
71 @Override
72 public void teleopPeriodic() {
73 double forwardSpeed;
74 double rotationSpeed;
75
76 if (xboxController.getAButton()) {
77 // Vision-alignment mode
78 // Query the latest result from PhotonVision
79 var result = camera.getLatestResult();
80
81 if (result.hasTargets()) {
82 // First calculate range
83 double range =
84 PhotonUtils.calculateDistanceToTargetMeters(
85 CAMERA_HEIGHT_METERS,
86 TARGET_HEIGHT_METERS,
87 CAMERA_PITCH_RADIANS,
88 Units.degreesToRadians(result.getBestTarget().getPitch()));
89
90 // Use this range as the measurement we give to the PID controller.
91 // -1.0 required to ensure positive PID controller effort _increases_ range
92 forwardSpeed = -forwardController.calculate(range, GOAL_RANGE_METERS);
93
94 // Also calculate angular power
95 // -1.0 required to ensure positive PID controller effort _increases_ yaw
96 rotationSpeed = -turnController.calculate(result.getBestTarget().getYaw(), 0);
97 } else {
98 // If we have no targets, stay still.
99 forwardSpeed = 0;
100 rotationSpeed = 0;
101 }
102 } else {
103 // Manual Driver Mode
104 forwardSpeed = -xboxController.getRightY();
105 rotationSpeed = xboxController.getLeftX();
106 }
107
108 // Use our forward/turn speeds to control the drivetrain
109 drive.arcadeDrive(forwardSpeed, rotationSpeed);
110 }
111}
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 // Constants such as camera and target height stored. Change per robot and
43 // goal!
44 const units::meter_t CAMERA_HEIGHT = 24_in;
45 const units::meter_t TARGET_HEIGHT = 5_ft;
46
47 // Angle between horizontal and the camera.
48 const units::radian_t CAMERA_PITCH = 0_deg;
49
50 // How far from the target we want to be
51 const units::meter_t GOAL_RANGE_METERS = 3_ft;
52
53 // PID constants should be tuned per robot
54 const double LINEAR_P = 0.1;
55 const double LINEAR_D = 0.0;
56 frc2::PIDController forwardController{LINEAR_P, 0.0, LINEAR_D};
57
58 const double ANGULAR_P = 0.1;
59 const double ANGULAR_D = 0.0;
60 frc2::PIDController turnController{ANGULAR_P, 0.0, ANGULAR_D};
61
62 // Change this to match the name of your camera
63 photonlib::PhotonCamera camera{"photonvision"};
64
65 frc::XboxController xboxController{0};
66
67 // Drive motors
68 frc::PWMVictorSPX leftMotor{0};
69 frc::PWMVictorSPX rightMotor{1};
70 frc::DifferentialDrive drive{leftMotor, rightMotor};
71};
25#include "Robot.h"
26
27#include <photonlib/PhotonUtils.h>
28
29void Robot::TeleopPeriodic() {
30 double forwardSpeed;
31 double rotationSpeed;
32
33 if (xboxController.GetAButton()) {
34 // Vision-alignment mode
35 // Query the latest result from PhotonVision
36 const auto& result = camera.GetLatestResult();
37
38 if (result.HasTargets()) {
39 // First calculate range
40 units::meter_t range = photonlib::PhotonUtils::CalculateDistanceToTarget(
41 CAMERA_HEIGHT, TARGET_HEIGHT, CAMERA_PITCH,
42 units::degree_t{result.GetBestTarget().GetPitch()});
43
44 // Use this range as the measurement we give to the PID controller.
45 // -1.0 required to ensure positive PID controller effort _increases_
46 // range
47 forwardSpeed = -forwardController.Calculate(range.value(),
48 GOAL_RANGE_METERS.value());
49
50 // Also calculate angular power
51 // -1.0 required to ensure positive PID controller effort _increases_ yaw
52 rotationSpeed =
53 -turnController.Calculate(result.GetBestTarget().GetYaw(), 0);
54 } else {
55 // If we have no targets, stay still.
56 forwardSpeed = 0;
57 rotationSpeed = 0;
58 }
59 } else {
60 // Manual Driver Mode
61 forwardSpeed = -xboxController.GetRightY();
62 rotationSpeed = xboxController.GetLeftX();
63 }
64
65 // Use our forward/turn speeds to control the drivetrain
66 drive.ArcadeDrive(forwardSpeed, rotationSpeed);
67}