Add Photonlib (#231)

Merges Photonlib into Photonvision, along with the Photonlib code examples. Also creates a new PhotonTargeting library teams can depend on.

photonlib-java-examples/build.gradle

@@ -0,0 +1,35 @@
+apply plugin: 'java'
+
+String wpilibVersion = "2021.1.2"
+String opencvVersion = "3.4.7-5"
+String ejmlVersion = "0.38"
+String jacksonVersion = "2.10.0"
+
+repositories {
+    maven {
+        url = 'https://frcmaven.wpi.edu:443/artifactory/release'
+    }
+}
+
+dependencies {
+    implementation project(':photon-lib')
+    implementation project(':photon-targeting')
+
+    implementation "edu.wpi.first.wpilibj:wpilibj-java:${wpilibVersion}"
+    implementation "edu.wpi.first.wpimath:wpimath-java:${wpilibVersion}"
+    implementation "edu.wpi.first.ntcore:ntcore-java:${wpilibVersion}"
+    implementation "edu.wpi.first.wpiutil:wpiutil-java:${wpilibVersion}"
+    implementation "edu.wpi.first.thirdparty.frc2021.opencv:opencv-java:${opencvVersion}"
+    implementation "edu.wpi.first.cscore:cscore-java:${wpilibVersion}"
+    implementation "edu.wpi.first.cameraserver:cameraserver-java:${wpilibVersion}"
+    implementation "edu.wpi.first.hal:hal-java:${wpilibVersion}"
+    implementation "org.ejml:ejml-simple:${ejmlVersion}"
+    implementation "com.fasterxml.jackson.core:jackson-annotations:${jacksonVersion}"
+    implementation "com.fasterxml.jackson.core:jackson-core:${jacksonVersion}"
+    implementation "com.fasterxml.jackson.core:jackson-databind:${jacksonVersion}"
+}
+
+ext {
+    exampleDirectory = new File("$projectDir/src/main/java/edu/wpi/first/wpilibj/examples/")
+    exampleFile = new File("$projectDir/src/main/java/edu/wpi/first/wpilibj/examples/examples.json")
+}

photonlib-java-examples/settings.gradle

@@ -0,0 +1 @@
+rootproject.name = 'photonlib-java-examples'

photonlib-java-examples/src/main/java/org/photonlib/examples/aimandrange/Main.java

@@ -0,0 +1,38 @@
+/*
+ * Copyright (C) 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 org.photonlib.examples.aimandrange;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+* you are doing, do not modify this file except to change the parameter class to the startRobot
+* call.
+*/
+public final class Main {
+    private Main() {}
+
+    /**
+    * Main initialization function. Do not perform any initialization here.
+    *
+    * <p>If you change your main robot class, change the parameter type.
+    */
+    public static void main(String... args) {
+        RobotBase.startRobot(Robot::new);
+    }
+}

photonlib-java-examples/src/main/java/org/photonlib/examples/aimandrange/Robot.java

@@ -0,0 +1,103 @@
+/*
+ * Copyright (C) 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 org.photonlib.examples.aimandrange;
+
+import edu.wpi.first.wpilibj.GenericHID;
+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;
+import org.photonvision.PhotonCamera;
+import org.photonvision.PhotonUtils;
+
+/**
+* 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);
+    }
+}

photonlib-java-examples/src/main/java/org/photonlib/examples/aimattarget/Main.java

@@ -0,0 +1,38 @@
+/*
+ * Copyright (C) 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 org.photonlib.examples.aimattarget;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+* you are doing, do not modify this file except to change the parameter class to the startRobot
+* call.
+*/
+public final class Main {
+    private Main() {}
+
+    /**
+    * Main initialization function. Do not perform any initialization here.
+    *
+    * <p>If you change your main robot class, change the parameter type.
+    */
+    public static void main(String... args) {
+        RobotBase.startRobot(Robot::new);
+    }
+}

photonlib-java-examples/src/main/java/org/photonlib/examples/aimattarget/Robot.java

@@ -0,0 +1,103 @@
+/*
+ * Copyright (C) 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 org.photonlib.examples.aimattarget;
+
+import edu.wpi.first.wpilibj.GenericHID;
+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;
+import org.photonvision.PhotonCamera;
+import org.photonvision.PhotonUtils;
+
+/**
+* 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);
+    }
+}

photonlib-java-examples/src/main/java/org/photonlib/examples/examples.json

@@ -0,0 +1,38 @@
+[
+  {
+    "name": "AimAtTarget",
+    "description": "Aim at a target",
+    "tags": [],
+    "gradlebase": "java",
+    "language": "java",
+    "commandversion": 1,
+    "mainclass": "Main",
+    "packagetoreplace": null,
+    "dependencies": [],
+    "foldername": "aimattarget"
+  },
+  {
+    "name": "GetInRange",
+    "description": "Get in range of a target",
+    "tags": [],
+    "gradlebase": "java",
+    "language": "java",
+    "commandversion": 1,
+    "mainclass": "Main",
+    "packagetoreplace": null,
+    "dependencies": [],
+    "foldername": "getinrange"
+  },
+  {
+    "name": "AimAndRange",
+    "description": "Aim at a target while at a desired range",
+    "tags": [],
+    "gradlebase": "java",
+    "language": "java",
+    "commandversion": 1,
+    "mainclass": "Main",
+    "packagetoreplace": null,
+    "dependencies": [],
+    "foldername": "aimandrange"
+  }
+]

photonlib-java-examples/src/main/java/org/photonlib/examples/getinrange/Main.java

@@ -0,0 +1,38 @@
+/*
+ * Copyright (C) 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 org.photonlib.examples.getinrange;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+* you are doing, do not modify this file except to change the parameter class to the startRobot
+* call.
+*/
+public final class Main {
+    private Main() {}
+
+    /**
+    * Main initialization function. Do not perform any initialization here.
+    *
+    * <p>If you change your main robot class, change the parameter type.
+    */
+    public static void main(String... args) {
+        RobotBase.startRobot(Robot::new);
+    }
+}

photonlib-java-examples/src/main/java/org/photonlib/examples/getinrange/Robot.java

@@ -0,0 +1,100 @@
+/*
+ * Copyright (C) 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 org.photonlib.examples.getinrange;
+
+import edu.wpi.first.wpilibj.GenericHID;
+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;
+import org.photonvision.PhotonCamera;
+import org.photonvision.PhotonUtils;
+
+/**
+* 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 P_GAIN = 0.1;
+    final double D_GAIN = 0.0;
+    PIDController controller = new PIDController(P_GAIN, 0, D_GAIN);
+
+    XboxController xboxController;
+
+    // Drive motors
+    PWMVictorSPX leftMotor = new PWMVictorSPX(0);
+    PWMVictorSPX rightMotor = new PWMVictorSPX(1);
+    DifferentialDrive drive = new DifferentialDrive(leftMotor, rightMotor);
+
+    @Override
+    public void robotInit() {
+        xboxController = new XboxController(0);
+    }
+
+    @Override
+    public void teleopPeriodic() {
+        double forwardSpeed;
+        double rotationSpeed = xboxController.getX(GenericHID.Hand.kLeft);
+
+        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 = controller.calculate(range, GOAL_RANGE_METERS);
+            } else {
+                // If we have no targets, stay still.
+                forwardSpeed = 0;
+            }
+        } else {
+            // Manual Driver Mode
+            forwardSpeed = xboxController.getY(GenericHID.Hand.kRight);
+        }
+
+        // Use our forward/turn speeds to control the drivetrain
+        drive.arcadeDrive(forwardSpeed, rotationSpeed);
+    }
+}