diff --git a/.github/workflows/gradle-publish.yml b/.github/workflows/gradle-publish.yml index 8ec3ee3..3feef94 100644 --- a/.github/workflows/gradle-publish.yml +++ b/.github/workflows/gradle-publish.yml @@ -10,6 +10,8 @@ name: Gradle Package on: release: types: [created] + push: + branches: ["main"] jobs: build: @@ -22,10 +24,10 @@ jobs: steps: - uses: actions/checkout@v3 - - name: Set up JDK 11 + - name: Set up JDK 17 uses: actions/setup-java@v3 with: - java-version: '11' + java-version: '17' distribution: 'temurin' server-id: github # Value of the distributionManagement/repository/id field of the pom.xml settings-path: ${{ github.workspace }} # location for the settings.xml file diff --git a/lib/build.gradle b/lib/build.gradle index 689664a..5b2288f 100644 --- a/lib/build.gradle +++ b/lib/build.gradle @@ -9,7 +9,7 @@ plugins { // Apply the java-library plugin for API and implementation separation. id 'java' - id 'edu.wpi.first.GradleRIO' version '2023.2.1' + id 'edu.wpi.first.GradleRIO' version '2023.+' } repositories { @@ -21,6 +21,14 @@ repositories { maven {url "https://maven.photonvision.org/repository/snapshots"} maven {url "https://maven.photonvision.org/repository/internal"} maven {url "https://maven.ctr-electronics.com/release/"} + + maven { + url = uri("https://maven.pkg.github.com/Mechanical-Advantage/AdvantageKit") + credentials { + username = "Mechanical-Advantage-Bot" + password = "\u0067\u0068\u0070\u005f\u006e\u0056\u0051\u006a\u0055\u004f\u004c\u0061\u0079\u0066\u006e\u0078\u006e\u0037\u0051\u0049\u0054\u0042\u0032\u004c\u004a\u006d\u0055\u0070\u0073\u0031\u006d\u0037\u004c\u005a\u0030\u0076\u0062\u0070\u0063\u0051" + } + } } dependencies { @@ -43,10 +51,19 @@ dependencies { implementation "com.fasterxml.jackson.core:jackson-core:2.10.0" implementation "com.fasterxml.jackson.core:jackson-databind:2.10.0" implementation 'edu.wpi.first.thirdparty.frc2022.opencv:opencv-java:4.5.2-1' + + // CTRE Phoenix implementation 'com.ctre.phoenix:api-java:5.30.3' implementation 'com.ctre.phoenix:wpiapi-java:5.30.3' + + // PhotonVision implementation "org.photonvision:PhotonLib-java:v2023.+" implementation "org.photonvision:PhotonTargeting-java:v2023.+" + + // AdvantageKit + implementation "org.littletonrobotics.akit.junction:wpilib-shim:2.2.1" + implementation "org.littletonrobotics.akit.junction:junction-core:2.2.1" + implementation "org.littletonrobotics.akit.conduit:conduit-api:2.2.1" } tasks.named('test') { diff --git a/lib/publish.gradle b/lib/publish.gradle index bd49a07..76eaee3 100644 --- a/lib/publish.gradle +++ b/lib/publish.gradle @@ -4,7 +4,7 @@ apply plugin: 'maven-publish' ext.licenseFile = files("$rootDir/LICENSE.txt") -def pubVersion = '2023.1.0' +def pubVersion = '2023.3.2' def outputsFolder = file("$buildDir/allOutputs") diff --git a/lib/src/main/java/com/fairportrobotics/frc/poe/CameraTracking/RobotFieldPosition.java b/lib/src/main/java/com/fairportrobotics/frc/poe/CameraTracking/RobotFieldPosition.java index 2c62ef5..45a8566 100644 --- a/lib/src/main/java/com/fairportrobotics/frc/poe/CameraTracking/RobotFieldPosition.java +++ b/lib/src/main/java/com/fairportrobotics/frc/poe/CameraTracking/RobotFieldPosition.java @@ -9,30 +9,80 @@ import org.photonvision.PhotonPoseEstimator.PoseStrategy; import org.photonvision.EstimatedRobotPose; import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.apriltag.AprilTagFieldLayout.OriginPosition; +import edu.wpi.first.apriltag.AprilTagFieldLayout.OriginPosition; import edu.wpi.first.math.geometry.Transform3d; -public class RobotFieldPosition { +public class RobotFieldPosition { AprilTagFieldLayout layout; PhotonPoseEstimator photonPoseEstimator; + Optional frozenPose; + boolean frozen = false; /* - Example of Transform3d: - - Transform3d robotToCam = new Transform3d(new Translation3d(0.5, 0.0, 0.5), new Rotation3d(0,0,0)); - Cam mounted facing forward, half a meter forward of center, half a meter up from center. - */ + * Example of Transform3d: + * + * Transform3d robotToCam = new Transform3d(new Translation3d(0.5, 0.0, 0.5), + * new Rotation3d(0,0,0)); + * Cam mounted facing forward, half a meter forward of center, half a meter up + * from center. + */ public RobotFieldPosition(String camera, Transform3d robotToCam, String layoutResourceFile) throws IOException { - layout = AprilTagFieldLayout.loadFromResource(layoutResourceFile); - + PhotonCamera cam = new PhotonCamera(camera); photonPoseEstimator = new PhotonPoseEstimator(layout, PoseStrategy.LOWEST_AMBIGUITY, cam, robotToCam); - } + } + + public RobotFieldPosition(String camera, Transform3d robotToCam, String layoutResourceFile, PoseStrategy strategy) throws IOException { + layout = AprilTagFieldLayout.loadFromResource(layoutResourceFile); + + PhotonCamera cam = new PhotonCamera(camera); + photonPoseEstimator = new PhotonPoseEstimator(layout, strategy, cam, robotToCam); + } + + public RobotFieldPosition(String camera, Transform3d robotToCam, AprilTagFields fields) throws IOException { + layout = AprilTagFieldLayout.loadFromResource(fields.m_resourceFile); + + layout.setOrigin(OriginPosition.kBlueAllianceWallRightSide); + + PhotonCamera cam = new PhotonCamera(camera); + photonPoseEstimator = new PhotonPoseEstimator(layout, PoseStrategy.LOWEST_AMBIGUITY, cam, robotToCam); + } + + public RobotFieldPosition(String camera, Transform3d robotToCam, AprilTagFields fields, PoseStrategy strategy) throws IOException { + layout = AprilTagFieldLayout.loadFromResource(fields.m_resourceFile); + + PhotonCamera cam = new PhotonCamera(camera); + photonPoseEstimator = new PhotonPoseEstimator(layout, strategy, cam, robotToCam); + } + + public RobotFieldPosition(String camera, Transform3d robotToCam, AprilTagFieldLayout layout, PoseStrategy strategy) throws IOException { + this.layout = layout; + + PhotonCamera cam = new PhotonCamera(camera); + photonPoseEstimator = new PhotonPoseEstimator(layout, strategy, cam, robotToCam); + } public Optional getEstimatedGlobalPose() { - return photonPoseEstimator.update(); + return frozen ? frozenPose : photonPoseEstimator.update(); + } + + public void setLastPose(Pose2d pose) { + photonPoseEstimator.setLastPose(pose); + } + + // For when you want it to stay the same across lines + public void freeze() { + frozen = true; + frozenPose = photonPoseEstimator.update(); + } + + public void unfreeze() { + frozen = false; } -} \ No newline at end of file +} \ No newline at end of file diff --git a/lib/src/main/java/com/fairportrobotics/frc/poe/CameraTracking/WPILIB/WPI_AprilTagPoseEstimator.java b/lib/src/main/java/com/fairportrobotics/frc/poe/CameraTracking/WPILIB/WPI_AprilTagPoseEstimator.java new file mode 100644 index 0000000..c275981 --- /dev/null +++ b/lib/src/main/java/com/fairportrobotics/frc/poe/CameraTracking/WPILIB/WPI_AprilTagPoseEstimator.java @@ -0,0 +1,92 @@ +package com.fairportrobotics.frc.poe.CameraTracking.WPILIB; + +import java.io.IOException; +import java.util.Optional; + +import org.opencv.core.Mat; + +import edu.wpi.first.apriltag.AprilTagDetection; +import edu.wpi.first.apriltag.AprilTagDetector; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.apriltag.AprilTagPoseEstimator; +import edu.wpi.first.apriltag.AprilTagPoseEstimator.Config; +import edu.wpi.first.cscore.VideoCamera; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.vision.VisionPipeline; +import edu.wpi.first.vision.VisionRunner; + +public class WPI_AprilTagPoseEstimator { + + private final AprilTagDetector aprilTagDetector; + private final AprilTagPoseEstimator aprilTagPoseEstimator; + private AprilTagFieldLayout aprilTagFieldLayout; + private Pose3d estimatedPose = new Pose3d(); + + private VisionRunner visionRunner; + + public WPI_AprilTagPoseEstimator(VideoCamera imageSource) { + aprilTagDetector = new AprilTagDetector(); + aprilTagDetector.addFamily("tag16h5"); + AprilTagPoseEstimator.Config poseEstimatorConfig = new Config(0.203, 0, 0, 0, 0); + aprilTagPoseEstimator = new AprilTagPoseEstimator(poseEstimatorConfig); + + try { + aprilTagFieldLayout = new AprilTagFieldLayout(AprilTagFields.kDefaultField.m_resourceFile); + } catch (IOException e) { + e.printStackTrace(); + } + + visionRunner = new VisionRunner(imageSource, new AprilTagsDetectorPipeline(), new VisionSystemCallback()); + } + + public void runDetectorPipeline(){ + new Thread(() -> { + try{ + visionRunner.runForever(); + } catch (Exception e){ + e.printStackTrace(); + } + }).start(); + } + + public Pose3d getEstimatedGlobalPose() { + return estimatedPose; + } + + private final class AprilTagsDetectorPipeline implements VisionPipeline { + + private Pose3d estPose = new Pose3d(); + + @Override + public void process(Mat image) { + AprilTagDetection[] aprilTags = aprilTagDetector.detect(image); + + for (AprilTagDetection aprilTag : aprilTags) { + Transform3d tagRelativeToRobot = aprilTagPoseEstimator.estimate(aprilTag); + Optional tagRelativeToFieldOptional = aprilTagFieldLayout.getTagPose(aprilTag.getId()); + + if (tagRelativeToFieldOptional.isPresent()) { + Pose3d tagRelativeToFieldPose = tagRelativeToFieldOptional.get(); + Pose3d estimatedFieldPos = tagRelativeToFieldPose.plus(tagRelativeToRobot); + // Logger.getInstance().recordOutput("April Tag Pose", tagRelativeToFieldPose); + estPose = estimatedFieldPos; + } + } + } + + public Pose3d getEstimatedPose(){ + return estPose; + } + + } + + private final class VisionSystemCallback implements VisionRunner.Listener { + @Override + public void copyPipelineOutputs(AprilTagsDetectorPipeline pipeline) { + estimatedPose = pipeline.getEstimatedPose(); + } + } + +} diff --git a/lib/src/main/java/com/fairportrobotics/frc/poe/controllers/lighting/ArduinoLightingController.java b/lib/src/main/java/com/fairportrobotics/frc/poe/controllers/lighting/ArduinoLightingController.java index d42bfcc..bc3b121 100644 --- a/lib/src/main/java/com/fairportrobotics/frc/poe/controllers/lighting/ArduinoLightingController.java +++ b/lib/src/main/java/com/fairportrobotics/frc/poe/controllers/lighting/ArduinoLightingController.java @@ -23,7 +23,7 @@ public class ArduinoLightingController { Matcher m = colorPattern.matcher(colorValue); return m.find(); } - private int executeColor(String colorValue, String command) { + public int executeColor(String colorValue, String command) { if ( validateColor(colorValue) ) { return bus.writeString( colorValue + command ); } diff --git a/lib/src/main/java/com/fairportrobotics/frc/poe/sensors/colorsensors/TCS34725.java b/lib/src/main/java/com/fairportrobotics/frc/poe/sensors/colorsensors/TCS34725.java index 0f76a61..15b34d8 100644 --- a/lib/src/main/java/com/fairportrobotics/frc/poe/sensors/colorsensors/TCS34725.java +++ b/lib/src/main/java/com/fairportrobotics/frc/poe/sensors/colorsensors/TCS34725.java @@ -8,7 +8,7 @@ import edu.wpi.first.wpilibj.I2C; public class TCS34725 { /** */ - class TCS34725_RGB { + public class TCS34725_RGB { private int r, g, b; public TCS34725_RGB(int r, int g, int b) {