Combining Aiming and Getting in Range

Knowledge and Equipment Needed

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.

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

Java

C++ (Header)

C++ (Source)

  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
/**
 * Copyright (C) 2018-2020 Photon Vision.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <https://www.gnu.org/licenses/>.
 */

package frc.robot;

import edu.wpi.first.wpilibj.GenericHID;
import org.photonvision.PhotonCamera;
import org.photonvision.PhotonUtils;

import edu.wpi.first.wpilibj.PWMVictorSPX;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.util.Units;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the TimedRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the build.gradle file in the
 * project.
 */
public class Robot extends TimedRobot {
   // Constants such as camera and target height stored. Change per robot and goal!
   final double CAMERA_HEIGHT_METERS = Units.inchesToMeters(24);
   final double TARGET_HEIGHT_METERS = Units.feetToMeters(5);
   // Angle between horizontal and the camera.
   final double CAMERA_PITCH_RADIANS = Units.degreesToRadians(0);

   // How far from the target we want to be
   final double GOAL_RANGE_METERS = Units.feetToMeters(3);

   // Change this to match the name of your camera
   PhotonCamera camera = new PhotonCamera("photonvision");

   // PID constants should be tuned per robot
   final double LINEAR_P = 0.1;
   final double LINEAR_D = 0.0;
   PIDController forwardController = new PIDController(LINEAR_P, 0, LINEAR_D);

   final double ANGULAR_P = 0.1;
   final double ANGULAR_D = 0.0;
   PIDController turnController = new PIDController(ANGULAR_P, 0, ANGULAR_D);

   XboxController xboxController = new XboxController(0);

   // Drive motors
   PWMVictorSPX leftMotor = new PWMVictorSPX(0);
   PWMVictorSPX rightMotor = new PWMVictorSPX(1);
   DifferentialDrive drive = new DifferentialDrive(leftMotor, rightMotor);

   @Override
   public void teleopPeriodic() {
      double forwardSpeed;
      double rotationSpeed;

      if (xboxController.getAButton()) {
         // Vision-alignment mode
         // Query the latest result from PhotonVision
         var result = camera.getLatestResult();

         if (result.hasTargets()) {
            // First calculate range
            double range = PhotonUtils.calculateDistanceToTargetMeters(CAMERA_HEIGHT_METERS, TARGET_HEIGHT_METERS,
                  CAMERA_PITCH_RADIANS, result.getBestTarget().getPitch());

            // Use this range as the measurement we give to the PID controller.
            forwardSpeed = forwardController.calculate(range, GOAL_RANGE_METERS);

            // Also calculate angular power
            rotationSpeed = turnController.calculate(result.getBestTarget().getYaw(), 0);
         } else {
            // If we have no targets, stay still.
            forwardSpeed = 0;
            rotationSpeed = 0;
         }
      } else {
         // Manual Driver Mode
         forwardSpeed = xboxController.getY(GenericHID.Hand.kRight);
         rotationSpeed = xboxController.getX(GenericHID.Hand.kLeft);
      }

      // Use our forward/turn speeds to control the drivetrain
      drive.arcadeDrive(forwardSpeed, rotationSpeed);
   }
}

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
/**
 * Copyright (C) 2018-2020 Photon Vision.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <https://www.gnu.org/licenses/>.
 */

#pragma once

#include <photonlib/PhotonCamera.h>

#include <frc/PWMVictorSPX.h>
#include <frc/TimedRobot.h>
#include <frc/XboxController.h>
#include <frc/controller/PIDController.h>
#include <frc/drive/DifferentialDrive.h>
#include <units/angle.h>
#include <units/length.h>

class Robot : public frc::TimedRobot {
public:
  void TeleopPeriodic() override;

private:
  // Constants such as camera and target height stored. Change per robot and
  // goal!
  const units::meter_t CAMERA_HEIGHT = 24_in;
  const units::meter_t TARGET_HEIGHT = 5_ft;

  // Angle between horizontal and the camera.
  const units::radian_t CAMERA_PITCH = 0_deg;

  // How far from the target we want to be
  const units::meter_t GOAL_RANGE_METERS = 3_ft;

  // PID constants should be tuned per robot
  const double LINEAR_P = 0.1;
  const double LINEAR_D = 0.0;
  frc2::PIDController forwardController{LINEAR_P, 0.0, LINEAR_D};

  const double ANGULAR_P = 0.1;
  const double ANGULAR_D = 0.0;
  frc2::PIDController turnController{ANGULAR_P, 0.0, ANGULAR_D};

  // Change this to match the name of your camera
  photonlib::PhotonCamera camera{"photonvision"};

  frc::XboxController xboxController{0};

  // Drive motors
  frc::PWMVictorSPX leftMotor{0};
  frc::PWMVictorSPX rightMotor{1};
  frc::DifferentialDrive drive{leftMotor, rightMotor};
};

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
/**
 * Copyright (C) 2018-2020 Photon Vision.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <https://www.gnu.org/licenses/>.
 */

#include "Robot.h"

#include <photonlib/PhotonUtils.h>

void Robot::TeleopPeriodic() {
  double forwardSpeed;
  double rotationSpeed;

  if (xboxController.GetAButton()) {
    // Vision-alignment mode
    // Query the latest result from PhotonVision
    const auto &result = camera.GetLatestResult();

    if (result.HasTargets()) {
      // First calculate range
      units::meter_t range =
          photonlib::PhotonUtils::CalculateDistanceToTarget(
              CAMERA_HEIGHT, TARGET_HEIGHT, CAMERA_PITCH,
              units::degree_t{result.GetBestTarget().GetPitch()});

      // Use this range as the measurement we give to the PID controller.
      forwardSpeed = forwardController.Calculate(
          range.to<double>(), GOAL_RANGE_METERS.to<double>());

      // Also calculate angular power
      rotationSpeed =
          turnController.Calculate(result.GetBestTarget().GetYaw(), 0);
    } else {
      // If we have no targets, stay still.
      forwardSpeed = 0;
      rotationSpeed = 0;
    }
  } else {
    // Manual Driver Mode
    forwardSpeed =
        xboxController.GetY(frc::GenericHID::JoystickHand::kRightHand);
    rotationSpeed =
        xboxController.GetX(frc::GenericHID::JoystickHand::kLeftHand);
  }

  // Use our forward/turn speeds to control the drivetrain
  drive.ArcadeDrive(forwardSpeed, rotationSpeed);
}

#ifndef RUNNING_FRC_TESTS
int main() { return frc::StartRobot<Robot>(); }
#endif