Getting in Range of the Target
The following example is from the PhotonLib example repository (Java/C++).
Knowledge and Equipment Needed
Everything required in Aiming at a Target.
Large space where your robot can move around freely
Code
In FRC, a mechanism usually has to be a certain distance away from its target in order to be effective and score. In the previous example, we showed how to aim your robot at the target. Now we will show how to move to a certain distance from the target.
For proper functionality of just this example, ensure that your robot is pointed towards the target.
While the operator holds down a button, the robot will drive towards the target and get in range.
This example uses P term of the PID loop and PhotonLib and the distance function of PhotonUtils.
Warning
The PhotonLib utility to calculate distance depends on the camera being at a different vertical height than the target. If this is not the case, a different method for estimating distance, such as target width or area, should be used. In general, this method becomes more accurate as range decreases and as the height difference increases.
Note
There is no strict minimum delta-height necessary for this method to be applicable, just a requirement that a delta exists.
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
47 // Angle between horizontal and the camera.
48 final double CAMERA_PITCH_RADIANS = Units.degreesToRadians(0);
49
50 // How far from the target we want to be
51 final double GOAL_RANGE_METERS = Units.feetToMeters(3);
52
53 // Change this to match the name of your camera
54 PhotonCamera camera = new PhotonCamera("photonvision");
55
56 // PID constants should be tuned per robot
57 final double P_GAIN = 0.1;
58 final double D_GAIN = 0.0;
59 PIDController controller = new PIDController(P_GAIN, 0, D_GAIN);
60
61 XboxController xboxController;
62
63 // Drive motors
64 PWMVictorSPX leftMotor = new PWMVictorSPX(0);
65 PWMVictorSPX rightMotor = new PWMVictorSPX(1);
66 DifferentialDrive drive = new DifferentialDrive(leftMotor, rightMotor);
67
68 @Override
69 public void robotInit() {
70 xboxController = new XboxController(0);
71 }
72
73 @Override
74 public void teleopPeriodic() {
75 double forwardSpeed;
76 double rotationSpeed = xboxController.getLeftX();
77
78 if (xboxController.getAButton()) {
79 // Vision-alignment mode
80 // Query the latest result from PhotonVision
81 var result = camera.getLatestResult();
82
83 if (result.hasTargets()) {
84 // First calculate range
85 double range =
86 PhotonUtils.calculateDistanceToTargetMeters(
87 CAMERA_HEIGHT_METERS,
88 TARGET_HEIGHT_METERS,
89 CAMERA_PITCH_RADIANS,
90 Units.degreesToRadians(result.getBestTarget().getPitch()));
91
92 // Use this range as the measurement we give to the PID controller.
93 // -1.0 required to ensure positive PID controller effort _increases_ range
94 forwardSpeed = -controller.calculate(range, GOAL_RANGE_METERS);
95 } else {
96 // If we have no targets, stay still.
97 forwardSpeed = 0;
98 }
99 } else {
100 // Manual Driver Mode
101 forwardSpeed = -xboxController.getRightY();
102 }
103
104 // Use our forward/turn speeds to control the drivetrain
105 drive.arcadeDrive(forwardSpeed, rotationSpeed);
106 }
107}
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 P_GAIN = 0.1;
55 const double D_GAIN = 0.0;
56 frc2::PIDController controller{P_GAIN, 0.0, D_GAIN};
57
58 // Change this to match the name of your camera
59 photonlib::PhotonCamera camera{"photonvision"};
60
61 frc::XboxController xboxController{0};
62
63 // Drive motors
64 frc::PWMVictorSPX leftMotor{0};
65 frc::PWMVictorSPX rightMotor{1};
66 frc::DifferentialDrive drive{leftMotor, rightMotor};
67};
25#include "Robot.h"
26
27#include <photonlib/PhotonUtils.h>
28
29void Robot::TeleopPeriodic() {
30 double forwardSpeed;
31 double rotationSpeed = xboxController.GetLeftX();
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 // 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 forwardSpeed =
46 -controller.Calculate(range.value(), GOAL_RANGE_METERS.value());
47 } else {
48 // If we have no targets, stay still.
49 forwardSpeed = 0;
50 }
51 } else {
52 // Manual Driver Mode
53 forwardSpeed = -xboxController.GetRightY();
54 }
55
56 // Use our forward/turn speeds to control the drivetrain
57 drive.ArcadeDrive(forwardSpeed, rotationSpeed);
58}
Hint
The accuracy of the measurement of the camera’s pitch (CAMERA_PITCH_RADIANS
in the above example), as well as the camera’s FOV, will determine the overall accuracy of this method.