Getting in Range of the Target

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 it’s target in order to be effective and score. In the last example, we showed how to aim your robot at the target. Now you will learn how to move to a certain distance from the target. In order to properly complete this example, ensure that your robot is pointed towards the target, but this will not be necessary in the future. This example is similar to the previous one and will also be using the P term of the PID loop and PhotonLib, specifically the distance function of PhotonUtils. While the operator holds down a button, the robot will drive towards the target and get in range.

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.

The following example is from the PhotonLib example repository (Java/C++).

 1/*
 2 * Copyright (C) Photon Vision.
 3 *
 4 * This program is free software: you can redistribute it and/or modify
 5 * it under the terms of the GNU General Public License as published by
 6 * the Free Software Foundation, either version 3 of the License, or
 7 * (at your option) any later version.
 8 *
 9 * This program is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12 * GNU General Public License for more details.
13 *
14 * You should have received a copy of the GNU General Public License
15 * along with this program.  If not, see <https://www.gnu.org/licenses/>.
16 */
17package org.photonlib.examples.getinrange;
18
19import edu.wpi.first.math.controller.PIDController;
20import edu.wpi.first.math.util.Units;
21import edu.wpi.first.wpilibj.TimedRobot;
22import edu.wpi.first.wpilibj.XboxController;
23import edu.wpi.first.wpilibj.drive.DifferentialDrive;
24import edu.wpi.first.wpilibj.motorcontrol.PWMVictorSPX;
25import org.photonvision.PhotonCamera;
26import org.photonvision.PhotonUtils;
27
28/**
29 * The VM is configured to automatically run this class, and to call the functions corresponding to
30 * each mode, as described in the TimedRobot documentation. If you change the name of this class or
31 * the package after creating this project, you must also update the build.gradle file in the
32 * project.
33 */
34public class Robot extends TimedRobot {
35    // Constants such as camera and target height stored. Change per robot and goal!
36    final double CAMERA_HEIGHT_METERS = Units.inchesToMeters(24);
37    final double TARGET_HEIGHT_METERS = Units.feetToMeters(5);
38
39    // Angle between horizontal and the camera.
40    final double CAMERA_PITCH_RADIANS = Units.degreesToRadians(0);
41
42    // How far from the target we want to be
43    final double GOAL_RANGE_METERS = Units.feetToMeters(3);
44
45    // Change this to match the name of your camera
46    PhotonCamera camera = new PhotonCamera("photonvision");
47
48    // PID constants should be tuned per robot
49    final double P_GAIN = 0.1;
50    final double D_GAIN = 0.0;
51    PIDController controller = new PIDController(P_GAIN, 0, D_GAIN);
52
53    XboxController xboxController;
54
55    // Drive motors
56    PWMVictorSPX leftMotor = new PWMVictorSPX(0);
57    PWMVictorSPX rightMotor = new PWMVictorSPX(1);
58    DifferentialDrive drive = new DifferentialDrive(leftMotor, rightMotor);
59
60    @Override
61    public void robotInit() {
62        xboxController = new XboxController(0);
63    }
64
65    @Override
66    public void teleopPeriodic() {
67        double forwardSpeed;
68        double rotationSpeed = xboxController.getLeftX();
69
70        if (xboxController.getAButton()) {
71            // Vision-alignment mode
72            // Query the latest result from PhotonVision
73            var result = camera.getLatestResult();
74
75            if (result.hasTargets()) {
76                // First calculate range
77                double range =
78                        PhotonUtils.calculateDistanceToTargetMeters(
79                                CAMERA_HEIGHT_METERS,
80                                TARGET_HEIGHT_METERS,
81                                CAMERA_PITCH_RADIANS,
82                                Units.degreesToRadians(result.getBestTarget().getPitch()));
83
84                // Use this range as the measurement we give to the PID controller.
85                // -1.0 required to ensure positive PID controller effort _increases_ range
86                forwardSpeed = -controller.calculate(range, GOAL_RANGE_METERS);
87            } else {
88                // If we have no targets, stay still.
89                forwardSpeed = 0;
90            }
91        } else {
92            // Manual Driver Mode
93            forwardSpeed = -xboxController.getRightY();
94        }
95
96        // Use our forward/turn speeds to control the drivetrain
97        drive.arcadeDrive(forwardSpeed, rotationSpeed);
98    }
99}
 1/*
 2 * Copyright (C) Photon Vision.
 3 *
 4 * This program is free software: you can redistribute it and/or modify
 5 * it under the terms of the GNU General Public License as published by
 6 * the Free Software Foundation, either version 3 of the License, or
 7 * (at your option) any later version.
 8 *
 9 * This program is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12 * GNU General Public License for more details.
13 *
14 * You should have received a copy of the GNU General Public License
15 * along with this program.  If not, see <https://www.gnu.org/licenses/>.
16 */
17
18#pragma once
19
20#include <photonlib/PhotonCamera.h>
21
22#include <frc/TimedRobot.h>
23#include <frc/XboxController.h>
24#include <frc/controller/PIDController.h>
25#include <frc/drive/DifferentialDrive.h>
26#include <frc/motorcontrol/PWMVictorSPX.h>
27#include <units/angle.h>
28#include <units/length.h>
29
30class Robot : public frc::TimedRobot {
31 public:
32  void TeleopPeriodic() override;
33
34 private:
35  // Constants such as camera and target height stored. Change per robot and
36  // goal!
37  const units::meter_t CAMERA_HEIGHT = 24_in;
38  const units::meter_t TARGET_HEIGHT = 5_ft;
39
40  // Angle between horizontal and the camera.
41  const units::radian_t CAMERA_PITCH = 0_deg;
42
43  // How far from the target we want to be
44  const units::meter_t GOAL_RANGE_METERS = 3_ft;
45
46  // PID constants should be tuned per robot
47  const double P_GAIN = 0.1;
48  const double D_GAIN = 0.0;
49  frc2::PIDController controller{P_GAIN, 0.0, D_GAIN};
50
51  // Change this to match the name of your camera
52  photonlib::PhotonCamera camera{"photonvision"};
53
54  frc::XboxController xboxController{0};
55
56  // Drive motors
57  frc::PWMVictorSPX leftMotor{0};
58  frc::PWMVictorSPX rightMotor{1};
59  frc::DifferentialDrive drive{leftMotor, rightMotor};
60};
 1/*
 2 * Copyright (C) Photon Vision.
 3 *
 4 * This program is free software: you can redistribute it and/or modify
 5 * it under the terms of the GNU General Public License as published by
 6 * the Free Software Foundation, either version 3 of the License, or
 7 * (at your option) any later version.
 8 *
 9 * This program is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12 * GNU General Public License for more details.
13 *
14 * You should have received a copy of the GNU General Public License
15 * along with this program.  If not, see <https://www.gnu.org/licenses/>.
16 */
17
18#include "Robot.h"
19
20#include <photonlib/PhotonUtils.h>
21
22void Robot::TeleopPeriodic() {
23  double forwardSpeed;
24  double rotationSpeed = xboxController.GetLeftX();
25
26  if (xboxController.GetAButton()) {
27    // Vision-alignment mode
28    // Query the latest result from PhotonVision
29    photonlib::PhotonPipelineResult result = camera.GetLatestResult();
30
31    if (result.HasTargets()) {
32      // First calculate range
33      units::meter_t range = photonlib::PhotonUtils::CalculateDistanceToTarget(
34          CAMERA_HEIGHT, TARGET_HEIGHT, CAMERA_PITCH,
35          units::degree_t{result.GetBestTarget().GetPitch()});
36
37      // Use this range as the measurement we give to the PID controller.
38      forwardSpeed =
39          -controller.Calculate(range.value(), GOAL_RANGE_METERS.value());
40    } else {
41      // If we have no targets, stay still.
42      forwardSpeed = 0;
43    }
44  } else {
45    // Manual Driver Mode
46    forwardSpeed = -xboxController.GetRightY();
47  }
48
49  // Use our forward/turn speeds to control the drivetrain
50  drive.ArcadeDrive(forwardSpeed, rotationSpeed);
51}
52
53#ifndef RUNNING_FRC_TESTS
54int main() { return frc::StartRobot<Robot>(); }
55#endif

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.