Merge branch 'main' of github.com:FairportRobotics/potential-octo-eureka

.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

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') {

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")
 

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<EstimatedRobotPose> frozenPose;
+    boolean frozen = false;
 
     /*
-        Example of Transform3d: 
+     * Example of Transform3d:
-
+     * 
-        Transform3d robotToCam = new Transform3d(new Translation3d(0.5, 0.0, 0.5), new Rotation3d(0,0,0)); 
+     * Transform3d robotToCam = new Transform3d(new Translation3d(0.5, 0.0, 0.5),
-        Cam mounted facing forward, half a meter forward of center, half a meter up from center.
+     * 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<EstimatedRobotPose> 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;
     }
 
 
-}   
+}

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<AprilTagsDetectorPipeline> 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<AprilTagsDetectorPipeline>(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<Pose3d> 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<AprilTagsDetectorPipeline> {
+        @Override
+        public void copyPipelineOutputs(AprilTagsDetectorPipeline pipeline) {
+            estimatedPose = pipeline.getEstimatedPose();
+        }
+    }
+
+}

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 );
         }

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) {