FairportRobotics
/
RoboLib
mirror of https://github.com/FairportRobotics/RoboLib.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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

This commit is contained in:
Tyler Wilcox 2023-03-28 10:35:11 -04:00
parent bfde970427 26ffc6b68b
commit 4e5588d2a8
7 changed files with 178 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
.github/workflows/gradle-publish.yml vendored
View File

@ -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

19
lib/build.gradle
View File

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

2
lib/publish.gradle
View File

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

72
lib/src/main/java/com/fairportrobotics/frc/poe/CameraTracking/RobotFieldPosition.java
View File

@ -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:
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<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;
}
}
}

92
lib/src/main/java/com/fairportrobotics/frc/poe/CameraTracking/WPILIB/WPI_AprilTagPoseEstimator.java Normal file
View File

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

2
lib/src/main/java/com/fairportrobotics/frc/poe/controllers/lighting/ArduinoLightingController.java
View File

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

2
lib/src/main/java/com/fairportrobotics/frc/poe/sensors/colorsensors/TCS34725.java
View File

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