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.
Code
Now that you know how to aim toward the AprilTag, let’s also drive the correct distance from the AprilTag.
To do this, we’ll use the pitch of the target in the camera image and trigonometry to figure out how far away the robot is from the AprilTag. Then, like before, we’ll use the P term of a PID controller to drive the robot to the correct distance.
84 // Calculate drivetrain commands from Joystick values
85 double forward = -controller.getLeftY() * Constants.Swerve.kMaxLinearSpeed;
86 double strafe = -controller.getLeftX() * Constants.Swerve.kMaxLinearSpeed;
87 double turn = -controller.getRightX() * Constants.Swerve.kMaxAngularSpeed;
88
89 // Read in relevant data from the Camera
90 boolean targetVisible = false;
91 double targetYaw = 0.0;
92 double targetRange = 0.0;
93 var results = camera.getAllUnreadResults();
94 if (!results.isEmpty()) {
95 // Camera processed a new frame since last
96 // Get the last one in the list.
97 var result = results.get(results.size() - 1);
98 if (result.hasTargets()) {
99 // At least one AprilTag was seen by the camera
100 for (var target : result.getTargets()) {
101 if (target.getFiducialId() == 7) {
102 // Found Tag 7, record its information
103 targetYaw = target.getYaw();
104 targetRange =
105 PhotonUtils.calculateDistanceToTargetMeters(
106 0.5, // Measured with a tape measure, or in CAD.
107 1.435, // From 2024 game manual for ID 7
108 Units.degreesToRadians(-30.0), // Measured with a protractor, or in CAD.
109 Units.degreesToRadians(target.getPitch()));
110
111 targetVisible = true;
112 }
113 }
114 }
115 }
116
117 // Auto-align when requested
118 if (controller.getAButton() && targetVisible) {
119 // Driver wants auto-alignment to tag 7
120 // And, tag 7 is in sight, so we can turn toward it.
121 // Override the driver's turn and fwd/rev command with an automatic one
122 // That turns toward the tag, and gets the range right.
123 turn =
124 (VISION_DES_ANGLE_deg - targetYaw) * VISION_TURN_kP * Constants.Swerve.kMaxAngularSpeed;
125 forward =
126 (VISION_DES_RANGE_m - targetRange) * VISION_STRAFE_kP * Constants.Swerve.kMaxLinearSpeed;
127 }
128
129 // Command drivetrain motors based on target speeds
130 drivetrain.drive(forward, strafe, turn);
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 auto VISION_TURN_kP = 0.01;
60 static constexpr auto VISION_DES_ANGLE = 0.0_deg;
61 static constexpr auto VISION_STRAFE_kP = 0.5;
62 static constexpr auto VISION_DES_RANGE = 1.25_m;
63};
58void Robot::TeleopPeriodic() {
59 // Calculate drivetrain commands from Joystick values
60 auto forward =
61 -1.0 * controller.GetLeftY() * constants::Swerve::kMaxLinearSpeed;
62 auto strafe =
63 -1.0 * controller.GetLeftX() * constants::Swerve::kMaxLinearSpeed;
64 auto turn =
65 -1.0 * controller.GetRightX() * constants::Swerve::kMaxAngularSpeed;
66
67 bool targetVisible = false;
68 units::degree_t targetYaw = 0.0_deg;
69 units::meter_t targetRange = 0.0_m;
70 auto results = camera.GetAllUnreadResults();
71 if (results.size() > 0) {
72 // Camera processed a new frame since last
73 // Get the last one in the list.
74 auto result = results[results.size() - 1];
75 if (result.HasTargets()) {
76 // At least one AprilTag was seen by the camera
77 for (auto& target : result.GetTargets()) {
78 if (target.GetFiducialId() == 7) {
79 // Found Tag 7, record its information
80 targetYaw = units::degree_t{target.GetYaw()};
81 targetRange = photon::PhotonUtils::CalculateDistanceToTarget(
82 0.5_m, // Measured with a tape measure, or in CAD
83 1.435_m, // From 2024 game manual for ID 7
84 -30.0_deg, // Measured witha protractor, or in CAD
85 units::degree_t{target.GetPitch()});
86 targetVisible = true;
87 }
88 }
89 }
90 }
91
92 // Auto-align when requested
93 if (controller.GetAButton() && targetVisible) {
94 // Driver wants auto-alignment to tag 7
95 // And, tag 7 is in sight, so we can turn toward it.
96 // Override the driver's turn command with an automatic one that turns
97 // toward the tag and gets the range right.
98 turn = (VISION_DES_ANGLE - targetYaw).value() * VISION_TURN_kP *
99 constants::Swerve::kMaxAngularSpeed;
100 forward = (VISION_DES_RANGE - targetRange).value() * VISION_STRAFE_kP *
101 constants::Swerve::kMaxLinearSpeed;
102 }
103
104 // Command drivetrain motors based on target speeds
105 drivetrain.Drive(forward, strafe, turn);
106}
44 self.controller = wpilib.XboxController(0)
45 self.swerve = drivetrain.Drivetrain()
46 self.cam = PhotonCamera("YOUR CAMERA NAME")
47
48 def robotPeriodic(self) -> None:
49 self.swerve.updateOdometry()
50 self.swerve.log()
51
52 def teleopPeriodic(self) -> None:
53 xSpeed = -1.0 * self.controller.getLeftY() * drivetrain.kMaxSpeed
54 ySpeed = -1.0 * self.controller.getLeftX() * drivetrain.kMaxSpeed
55 rot = -1.0 * self.controller.getRightX() * drivetrain.kMaxAngularSpeed
56
57 # Get information from the camera
58 targetYaw = 0.0
59 targetRange = 0.0
60 targetVisible = False
61 results = self.cam.getAllUnreadResults()
62 if len(results) > 0:
63 result = results[-1] # take the most recent result the camera had
64 # At least one apriltag was seen by the camera
65 for target in result.getTargets():
66 if target.getFiducialId() == 7:
67 # Found tag 7, record its information
68 targetVisible = True
69 targetYaw = target.getYaw()
70 heightDelta = CAM_MOUNT_HEIGHT_m - TAG_7_MOUNT_HEIGHT_m
71 angleDelta = math.radians(CAM_MOUNT_PITCH_deg - target.getPitch())
72 targetRange = heightDelta / math.tan(angleDelta)
73
74 if self.controller.getAButton() and targetVisible:
75 # Driver wants auto-alignment to tag 7
76 # And, tag 7 is in sight, so we can turn toward it.
77 # Override the driver's turn and x-vel command with
78 # an automatic one that turns toward the tag
79 # and puts us at the right distance
80 rot = (
81 (VISION_DES_ANGLE_deg - targetYaw)
82 * VISION_TURN_kP
83 * drivetrain.kMaxAngularSpeed
84 )
85 xSpeed = (
86 (VISION_DES_RANGE_m - targetRange)
87 * VISION_STRAFE_kP
88 * drivetrain.kMaxSpeed
89 )
90
91 self.swerve.drive(xSpeed, ySpeed, rot, True, self.getPeriod())
92
93 def _simulationPeriodic(self) -> None:
94 self.swerve.simulationPeriodic()
95 return super()._simulationPeriodic()