From 9f248b0b571a9be0127fd59afe7f86d77d18f409 Mon Sep 17 00:00:00 2001 From: IanTapply22 Date: Sat, 11 Nov 2023 20:19:01 -0500 Subject: [PATCH 01/14] Cretaed build, format, and lint actions --- .github/workflows/build.yml | 29 +++++++++++++++++++++++++++++ .github/workflows/format.yml | 18 ++++++++++++++++++ .github/workflows/lint.yml | 23 +++++++++++++++++++++++ 3 files changed, 70 insertions(+) create mode 100644 .github/workflows/build.yml create mode 100644 .github/workflows/format.yml create mode 100644 .github/workflows/lint.yml diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml new file mode 100644 index 0000000..ffa7966 --- /dev/null +++ b/.github/workflows/build.yml @@ -0,0 +1,29 @@ +name: Build + +# Controls when the action will run. Triggers the workflow on push to all branches. +on: [ push ] + +jobs: + + build: + # The type of runner that the job will run on + runs-on: ubuntu-latest + + # This grabs the WPILib docker container + container: wpilib/roborio-cross-ubuntu:2023-22.04 + + steps: + # Checks-out your repository under $GITHUB_WORKSPACE, so your job can access it + - uses: actions/checkout@v3 + + # Declares the repository safe and not under dubious ownership. + - name: Add repository to git safe directories + run: git config --global --add safe.directory $GITHUB_WORKSPACE + + # Grant execute permission for gradlew + - name: Grant execute permission for gradlew + run: chmod +x gradlew + + # Runs a single command using the runners shell + - name: Compile and run tests on robot code + run: ./gradlew build \ No newline at end of file diff --git a/.github/workflows/format.yml b/.github/workflows/format.yml new file mode 100644 index 0000000..b1306c4 --- /dev/null +++ b/.github/workflows/format.yml @@ -0,0 +1,18 @@ +name: Format + +on: + push: + branches: [ main ] # only format when code gets pushed to main + +jobs: + + formatting: + # The type of runner that the job will run on + runs-on: ubuntu-latest + + steps: + # Checks-out your repository under $GITHUB_WORKSPACE, so your job can access it + - uses: actions/checkout@v3 + - uses: axel-op/googlejavaformat-action@v3 + with: + args: "--skip-sorting-imports --replace" \ No newline at end of file diff --git a/.github/workflows/lint.yml b/.github/workflows/lint.yml new file mode 100644 index 0000000..e56211b --- /dev/null +++ b/.github/workflows/lint.yml @@ -0,0 +1,23 @@ +name: Lint + +# Controls when the action will run. Triggers the workflow on push to all branches. +on: [ push ] + +jobs: + + lint: + # The type of runner that the job will run on + runs-on: ubuntu-latest + + steps: + # Checks-out your repository under $GITHUB_WORKSPACE, so your job can access it + - uses: actions/checkout@v3 + with: + fetch-depth: 0 + - uses: actions/setup-java@v3 + with: + distribution: 'zulu' + java-version: 17 + # Executes linting + # If this fails, run ./gradlew spotlessApply locally to fix formatting + - run: ./gradlew spotlessCheck \ No newline at end of file From 4a5e1ffe4a28187c7970645d7ac487ba8edafe1d Mon Sep 17 00:00:00 2001 From: IanTapply22 Date: Sat, 11 Nov 2023 20:23:51 -0500 Subject: [PATCH 02/14] Added spotless apply to gradle --- build.gradle | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/build.gradle b/build.gradle index b233ef1..f6de22f 100644 --- a/build.gradle +++ b/build.gradle @@ -1,5 +1,6 @@ plugins { id "java" + id 'com.diffplug.spotless' version "6.12.0" id "edu.wpi.first.GradleRIO" version "2023.4.3" } @@ -8,6 +9,20 @@ targetCompatibility = JavaVersion.VERSION_11 def ROBOT_MAIN_CLASS = "frc.robot.Main" +spotless { + java { + target fileTree('.') { + include '**/*.java' + exclude '**/build/**', '**/build-*/**' + } + toggleOffOn() + googleJavaFormat() + removeUnusedImports() + trimTrailingWhitespace() + endWithNewline() + } +} + // Define my targets (RoboRIO) and artifacts (deployable files) // This is added by GradleRIO's backing project DeployUtils. deploy { From 1b9f93ef974fcc8a67e497b4ded10bbf4bbad43e Mon Sep 17 00:00:00 2001 From: IanTapply22 Date: Sat, 11 Nov 2023 20:31:40 -0500 Subject: [PATCH 03/14] Added space to end of workflows --- .github/workflows/build.yml | 2 +- .github/workflows/format.yml | 2 +- .github/workflows/lint.yml | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index ffa7966..4c603d7 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -26,4 +26,4 @@ jobs: # Runs a single command using the runners shell - name: Compile and run tests on robot code - run: ./gradlew build \ No newline at end of file + run: ./gradlew build diff --git a/.github/workflows/format.yml b/.github/workflows/format.yml index b1306c4..6c1190c 100644 --- a/.github/workflows/format.yml +++ b/.github/workflows/format.yml @@ -15,4 +15,4 @@ jobs: - uses: actions/checkout@v3 - uses: axel-op/googlejavaformat-action@v3 with: - args: "--skip-sorting-imports --replace" \ No newline at end of file + args: "--skip-sorting-imports --replace" diff --git a/.github/workflows/lint.yml b/.github/workflows/lint.yml index e56211b..1780478 100644 --- a/.github/workflows/lint.yml +++ b/.github/workflows/lint.yml @@ -20,4 +20,4 @@ jobs: java-version: 17 # Executes linting # If this fails, run ./gradlew spotlessApply locally to fix formatting - - run: ./gradlew spotlessCheck \ No newline at end of file + - run: ./gradlew spotlessCheck From cf9ffaedd2f0d78cd758a24956c073f99fb9b8d3 Mon Sep 17 00:00:00 2001 From: IanTapply22 Date: Sat, 11 Nov 2023 20:45:35 -0500 Subject: [PATCH 04/14] Merged lint and build actions together Specified format to only work for PRs and on main and 2023-beta --- .../workflows/{build.yml => build-lint.yml} | 21 ++++++++++++++++- .github/workflows/format.yml | 3 +-- .github/workflows/lint.yml | 23 ------------------- 3 files changed, 21 insertions(+), 26 deletions(-) rename .github/workflows/{build.yml => build-lint.yml} (57%) delete mode 100644 .github/workflows/lint.yml diff --git a/.github/workflows/build.yml b/.github/workflows/build-lint.yml similarity index 57% rename from .github/workflows/build.yml rename to .github/workflows/build-lint.yml index 4c603d7..c668b40 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build-lint.yml @@ -1,10 +1,11 @@ -name: Build +name: Build and Lint # Controls when the action will run. Triggers the workflow on push to all branches. on: [ push ] jobs: + # Build our code to see if it can be deployed without errors build: # The type of runner that the job will run on runs-on: ubuntu-latest @@ -27,3 +28,21 @@ jobs: # Runs a single command using the runners shell - name: Compile and run tests on robot code run: ./gradlew build + + # Lints our code to find any format violations + lint: + # The type of runner that the job will run on + runs-on: ubuntu-latest + + steps: + # Checks-out your repository under $GITHUB_WORKSPACE, so your job can access it + - uses: actions/checkout@v3 + with: + fetch-depth: 0 + - uses: actions/setup-java@v3 + with: + distribution: 'zulu' + java-version: 17 + # Executes linting + # If this fails, run ./gradlew spotlessApply locally to fix formatting + - run: ./gradlew spotlessCheck diff --git a/.github/workflows/format.yml b/.github/workflows/format.yml index 6c1190c..7810423 100644 --- a/.github/workflows/format.yml +++ b/.github/workflows/format.yml @@ -1,8 +1,7 @@ name: Format on: - push: - branches: [ main ] # only format when code gets pushed to main + pull_request: [ main, 2023-beta ] jobs: diff --git a/.github/workflows/lint.yml b/.github/workflows/lint.yml deleted file mode 100644 index 1780478..0000000 --- a/.github/workflows/lint.yml +++ /dev/null @@ -1,23 +0,0 @@ -name: Lint - -# Controls when the action will run. Triggers the workflow on push to all branches. -on: [ push ] - -jobs: - - lint: - # The type of runner that the job will run on - runs-on: ubuntu-latest - - steps: - # Checks-out your repository under $GITHUB_WORKSPACE, so your job can access it - - uses: actions/checkout@v3 - with: - fetch-depth: 0 - - uses: actions/setup-java@v3 - with: - distribution: 'zulu' - java-version: 17 - # Executes linting - # If this fails, run ./gradlew spotlessApply locally to fix formatting - - run: ./gradlew spotlessCheck From 8ae78ec1c6cfc64c37db9558678c3e384703bf3c Mon Sep 17 00:00:00 2001 From: IanTapply22 Date: Sat, 11 Nov 2023 23:39:20 -0500 Subject: [PATCH 05/14] Fixed branch name typo --- .github/workflows/format.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/format.yml b/.github/workflows/format.yml index 7810423..afc8e9d 100644 --- a/.github/workflows/format.yml +++ b/.github/workflows/format.yml @@ -1,7 +1,7 @@ name: Format on: - pull_request: [ main, 2023-beta ] + pull_request: [ main, 2024-beta ] jobs: From cdeedbef94d43384e82e6da252ed52478dc26191 Mon Sep 17 00:00:00 2001 From: IanTapply22 Date: Sun, 12 Nov 2023 11:34:34 -0500 Subject: [PATCH 06/14] Fixed invalid format in format workfow --- .github/workflows/format.yml | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/.github/workflows/format.yml b/.github/workflows/format.yml index afc8e9d..cc37b36 100644 --- a/.github/workflows/format.yml +++ b/.github/workflows/format.yml @@ -1,7 +1,12 @@ name: Format on: - pull_request: [ main, 2024-beta ] + pull_request: + types: + - open + branches: + - main + - 2024-beta jobs: From 08a8b0e74021ff8684cb47caa37d9a8b0a512544 Mon Sep 17 00:00:00 2001 From: IanTapply22 Date: Sun, 12 Nov 2023 11:55:25 -0500 Subject: [PATCH 07/14] Remove PR type specification --- .github/workflows/format.yml | 2 -- 1 file changed, 2 deletions(-) diff --git a/.github/workflows/format.yml b/.github/workflows/format.yml index cc37b36..9893f8f 100644 --- a/.github/workflows/format.yml +++ b/.github/workflows/format.yml @@ -2,8 +2,6 @@ name: Format on: pull_request: - types: - - open branches: - main - 2024-beta From 3ba23435ce10170df6e2bbfeabb4b8664830f1ff Mon Sep 17 00:00:00 2001 From: IanTapply22 Date: Sun, 12 Nov 2023 12:01:18 -0500 Subject: [PATCH 08/14] Fixed google auto format not working in pull requests --- .github/workflows/format.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/format.yml b/.github/workflows/format.yml index 9893f8f..20cda79 100644 --- a/.github/workflows/format.yml +++ b/.github/workflows/format.yml @@ -17,4 +17,5 @@ jobs: - uses: actions/checkout@v3 - uses: axel-op/googlejavaformat-action@v3 with: + red: ${{ github.event.pull_request.head.ref }} args: "--skip-sorting-imports --replace" From ef0b4357ced00ef77efb31480179bce9f138d8a9 Mon Sep 17 00:00:00 2001 From: IanTapply22 Date: Sun, 12 Nov 2023 12:02:34 -0500 Subject: [PATCH 09/14] Fixed typo --- .github/workflows/format.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/format.yml b/.github/workflows/format.yml index 20cda79..549b1df 100644 --- a/.github/workflows/format.yml +++ b/.github/workflows/format.yml @@ -17,5 +17,5 @@ jobs: - uses: actions/checkout@v3 - uses: axel-op/googlejavaformat-action@v3 with: - red: ${{ github.event.pull_request.head.ref }} + ref: ${{ github.event.pull_request.head.ref }} args: "--skip-sorting-imports --replace" From fe9c7ccf945efe022e8c7de417531071d931408e Mon Sep 17 00:00:00 2001 From: IanTapply22 Date: Sun, 12 Nov 2023 12:05:32 -0500 Subject: [PATCH 10/14] Moved ref to checkout --- .github/workflows/format.yml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.github/workflows/format.yml b/.github/workflows/format.yml index 549b1df..251b269 100644 --- a/.github/workflows/format.yml +++ b/.github/workflows/format.yml @@ -15,7 +15,8 @@ jobs: steps: # Checks-out your repository under $GITHUB_WORKSPACE, so your job can access it - uses: actions/checkout@v3 - - uses: axel-op/googlejavaformat-action@v3 with: ref: ${{ github.event.pull_request.head.ref }} + - uses: axel-op/googlejavaformat-action@v3 + with: args: "--skip-sorting-imports --replace" From 37140cd3c4de10d030882ee5f7ebae0fa74699bd Mon Sep 17 00:00:00 2001 From: github-actions <> Date: Sun, 12 Nov 2023 17:05:52 +0000 Subject: [PATCH 11/14] Google Java Format --- .../frc/robot/CommandSwerveDrivetrain.java | 120 +- src/main/java/frc/robot/LimelightHelpers.java | 1241 ++++++++--------- src/main/java/frc/robot/Robot.java | 9 +- src/main/java/frc/robot/RobotContainer.java | 50 +- src/main/java/frc/robot/Telemetry.java | 203 +-- .../frc/robot/generated/TunerConstants.java | 206 +-- 6 files changed, 949 insertions(+), 880 deletions(-) diff --git a/src/main/java/frc/robot/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/CommandSwerveDrivetrain.java index 884b426..5211b54 100644 --- a/src/main/java/frc/robot/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/CommandSwerveDrivetrain.java @@ -20,66 +20,80 @@ import edu.wpi.first.wpilibj2.command.Subsystem; /** - * Class that extends the Phoenix SwerveDrivetrain class and implements subsystem - * so it can be used in command-based projects easily. + * Class that extends the Phoenix SwerveDrivetrain class and implements subsystem so it can be used + * in command-based projects easily. */ public class CommandSwerveDrivetrain extends SwerveDrivetrain implements Subsystem { - private SwerveRequest.ApplyChassisSpeeds autoRequest = new SwerveRequest.ApplyChassisSpeeds(); - public CommandSwerveDrivetrain(SwerveDrivetrainConstants driveTrainConstants, double OdometryUpdateFrequency, SwerveModuleConstants... modules) { - super(driveTrainConstants, OdometryUpdateFrequency, modules); - configurePathPlanner(); - } - public CommandSwerveDrivetrain(SwerveDrivetrainConstants driveTrainConstants, SwerveModuleConstants... modules) { - super(driveTrainConstants, modules); - configurePathPlanner(); - } + private SwerveRequest.ApplyChassisSpeeds autoRequest = new SwerveRequest.ApplyChassisSpeeds(); - private void configurePathPlanner() { - AutoBuilder.configureHolonomic( - ()->this.getState().Pose, // Supplier of current robot pose - this::seedFieldRelative, // Consumer for seeding pose against auto - this::getCurrentRobotChassisSpeeds, - (speeds)->this.setControl(autoRequest.withSpeeds(speeds)), // Consumer of ChassisSpeeds to drive the robot - new HolonomicPathFollowerConfig(new PIDConstants(10, 0, 0), - new PIDConstants(10, 0, 0), - 1, - 1, - new ReplanningConfig(), - 0.004), - this); // Subsystem for requirements - } + public CommandSwerveDrivetrain( + SwerveDrivetrainConstants driveTrainConstants, + double OdometryUpdateFrequency, + SwerveModuleConstants... modules) { + super(driveTrainConstants, OdometryUpdateFrequency, modules); + configurePathPlanner(); + } - public Command applyRequest(Supplier requestSupplier) { - return new RunCommand(()->{this.setControl(requestSupplier.get());}, this); - } + public CommandSwerveDrivetrain( + SwerveDrivetrainConstants driveTrainConstants, SwerveModuleConstants... modules) { + super(driveTrainConstants, modules); + configurePathPlanner(); + } - public Command getAutoPath(String pathName) { - return new PathPlannerAuto(pathName); - } + private void configurePathPlanner() { + AutoBuilder.configureHolonomic( + () -> this.getState().Pose, // Supplier of current robot pose + this::seedFieldRelative, // Consumer for seeding pose against auto + this::getCurrentRobotChassisSpeeds, + (speeds) -> + this.setControl( + autoRequest.withSpeeds(speeds)), // Consumer of ChassisSpeeds to drive the robot + new HolonomicPathFollowerConfig( + new PIDConstants(10, 0, 0), + new PIDConstants(10, 0, 0), + 1, + 1, + new ReplanningConfig(), + 0.004), + this); // Subsystem for requirements + } - @Override - public void simulationPeriodic() { - /* Assume */ - updateSimState(0.02, 12); - } - /** - * Takes the specified location and makes it the current pose for - * field-relative maneuvers - * - * @param location Pose to make the current pose at. - */ - @Override - public void seedFieldRelative(Pose2d location) { - try { - m_stateLock.writeLock().lock(); + public Command applyRequest(Supplier requestSupplier) { + return new RunCommand( + () -> { + this.setControl(requestSupplier.get()); + }, + this); + } - m_odometry.resetPosition(Rotation2d.fromDegrees(m_pigeon2.getYaw().getValue()), m_modulePositions, location); - } finally { - m_stateLock.writeLock().unlock(); - } - } + public Command getAutoPath(String pathName) { + return new PathPlannerAuto(pathName); + } + + @Override + public void simulationPeriodic() { + /* Assume */ + updateSimState(0.02, 12); + } - public ChassisSpeeds getCurrentRobotChassisSpeeds() { - return m_kinematics.toChassisSpeeds(getState().ModuleStates); + /** + * Takes the specified location and makes it the current pose for field-relative maneuvers + * + * @param location Pose to make the current pose at. + */ + @Override + public void seedFieldRelative(Pose2d location) { + try { + m_stateLock.writeLock().lock(); + + m_odometry.resetPosition( + Rotation2d.fromDegrees(m_pigeon2.getYaw().getValue()), m_modulePositions, location); + } finally { + m_stateLock.writeLock().unlock(); } + } + + public ChassisSpeeds getCurrentRobotChassisSpeeds() { + return m_kinematics.toChassisSpeeds(getState().ModuleStates); + } } diff --git a/src/main/java/frc/robot/LimelightHelpers.java b/src/main/java/frc/robot/LimelightHelpers.java index 7aae8fb..9e162e8 100644 --- a/src/main/java/frc/robot/LimelightHelpers.java +++ b/src/main/java/frc/robot/LimelightHelpers.java @@ -1,4 +1,4 @@ -//LimelightHelpers v1.2.1 (March 1, 2023) +// LimelightHelpers v1.2.1 (March 1, 2023) package frc.robot; @@ -28,753 +28,740 @@ public class LimelightHelpers { - public static class LimelightTarget_Retro { - - @JsonProperty("t6c_ts") - private double[] cameraPose_TargetSpace; - - @JsonProperty("t6r_fs") - private double[] robotPose_FieldSpace; - - @JsonProperty("t6r_ts") - private double[] robotPose_TargetSpace; - - @JsonProperty("t6t_cs") - private double[] targetPose_CameraSpace; - - @JsonProperty("t6t_rs") - private double[] targetPose_RobotSpace; - - public Pose3d getCameraPose_TargetSpace() - { - return toPose3D(cameraPose_TargetSpace); - } - public Pose3d getRobotPose_FieldSpace() - { - return toPose3D(robotPose_FieldSpace); - } - public Pose3d getRobotPose_TargetSpace() - { - return toPose3D(robotPose_TargetSpace); - } - public Pose3d getTargetPose_CameraSpace() - { - return toPose3D(targetPose_CameraSpace); - } - public Pose3d getTargetPose_RobotSpace() - { - return toPose3D(targetPose_RobotSpace); - } - - public Pose2d getCameraPose_TargetSpace2D() - { - return toPose2D(cameraPose_TargetSpace); - } - public Pose2d getRobotPose_FieldSpace2D() - { - return toPose2D(robotPose_FieldSpace); - } - public Pose2d getRobotPose_TargetSpace2D() - { - return toPose2D(robotPose_TargetSpace); - } - public Pose2d getTargetPose_CameraSpace2D() - { - return toPose2D(targetPose_CameraSpace); - } - public Pose2d getTargetPose_RobotSpace2D() - { - return toPose2D(targetPose_RobotSpace); - } - - @JsonProperty("ta") - public double ta; - - @JsonProperty("tx") - public double tx; - - @JsonProperty("txp") - public double tx_pixels; - - @JsonProperty("ty") - public double ty; - - @JsonProperty("typ") - public double ty_pixels; - - @JsonProperty("ts") - public double ts; - - public LimelightTarget_Retro() { - cameraPose_TargetSpace = new double[6]; - robotPose_FieldSpace = new double[6]; - robotPose_TargetSpace = new double[6]; - targetPose_CameraSpace = new double[6]; - targetPose_RobotSpace = new double[6]; - } - - } - - public static class LimelightTarget_Fiducial { - - @JsonProperty("fID") - public double fiducialID; - - @JsonProperty("fam") - public String fiducialFamily; - - @JsonProperty("t6c_ts") - private double[] cameraPose_TargetSpace; - - @JsonProperty("t6r_fs") - private double[] robotPose_FieldSpace; - - @JsonProperty("t6r_ts") - private double[] robotPose_TargetSpace; - - @JsonProperty("t6t_cs") - private double[] targetPose_CameraSpace; - - @JsonProperty("t6t_rs") - private double[] targetPose_RobotSpace; - - public Pose3d getCameraPose_TargetSpace() - { - return toPose3D(cameraPose_TargetSpace); - } - public Pose3d getRobotPose_FieldSpace() - { - return toPose3D(robotPose_FieldSpace); - } - public Pose3d getRobotPose_TargetSpace() - { - return toPose3D(robotPose_TargetSpace); - } - public Pose3d getTargetPose_CameraSpace() - { - return toPose3D(targetPose_CameraSpace); - } - public Pose3d getTargetPose_RobotSpace() - { - return toPose3D(targetPose_RobotSpace); - } - - public Pose2d getCameraPose_TargetSpace2D() - { - return toPose2D(cameraPose_TargetSpace); - } - public Pose2d getRobotPose_FieldSpace2D() - { - return toPose2D(robotPose_FieldSpace); - } - public Pose2d getRobotPose_TargetSpace2D() - { - return toPose2D(robotPose_TargetSpace); - } - public Pose2d getTargetPose_CameraSpace2D() - { - return toPose2D(targetPose_CameraSpace); - } - public Pose2d getTargetPose_RobotSpace2D() - { - return toPose2D(targetPose_RobotSpace); - } - - @JsonProperty("ta") - public double ta; + public static class LimelightTarget_Retro { - @JsonProperty("tx") - public double tx; + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; - @JsonProperty("txp") - public double tx_pixels; + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; - @JsonProperty("ty") - public double ty; + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; - @JsonProperty("typ") - public double ty_pixels; + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; - @JsonProperty("ts") - public double ts; - - public LimelightTarget_Fiducial() { - cameraPose_TargetSpace = new double[6]; - robotPose_FieldSpace = new double[6]; - robotPose_TargetSpace = new double[6]; - targetPose_CameraSpace = new double[6]; - targetPose_RobotSpace = new double[6]; - } - } - - public static class LimelightTarget_Barcode { + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; + public Pose3d getCameraPose_TargetSpace() { + return toPose3D(cameraPose_TargetSpace); } - public static class LimelightTarget_Classifier { - - @JsonProperty("class") - public String className; - - @JsonProperty("classID") - public double classID; - - @JsonProperty("conf") - public double confidence; - - @JsonProperty("zone") - public double zone; - - @JsonProperty("tx") - public double tx; - - @JsonProperty("txp") - public double tx_pixels; - - @JsonProperty("ty") - public double ty; - - @JsonProperty("typ") - public double ty_pixels; - - public LimelightTarget_Classifier() { - } + public Pose3d getRobotPose_FieldSpace() { + return toPose3D(robotPose_FieldSpace); } - public static class LimelightTarget_Detector { - - @JsonProperty("class") - public String className; - - @JsonProperty("classID") - public double classID; - - @JsonProperty("conf") - public double confidence; - - @JsonProperty("ta") - public double ta; - - @JsonProperty("tx") - public double tx; - - @JsonProperty("txp") - public double tx_pixels; - - @JsonProperty("ty") - public double ty; + public Pose3d getRobotPose_TargetSpace() { + return toPose3D(robotPose_TargetSpace); + } - @JsonProperty("typ") - public double ty_pixels; + public Pose3d getTargetPose_CameraSpace() { + return toPose3D(targetPose_CameraSpace); + } - public LimelightTarget_Detector() { - } + public Pose3d getTargetPose_RobotSpace() { + return toPose3D(targetPose_RobotSpace); } - public static class Results { + public Pose2d getCameraPose_TargetSpace2D() { + return toPose2D(cameraPose_TargetSpace); + } - @JsonProperty("pID") - public double pipelineID; + public Pose2d getRobotPose_FieldSpace2D() { + return toPose2D(robotPose_FieldSpace); + } - @JsonProperty("tl") - public double latency_pipeline; + public Pose2d getRobotPose_TargetSpace2D() { + return toPose2D(robotPose_TargetSpace); + } - @JsonProperty("cl") - public double latency_capture; + public Pose2d getTargetPose_CameraSpace2D() { + return toPose2D(targetPose_CameraSpace); + } - public double latency_jsonParse; + public Pose2d getTargetPose_RobotSpace2D() { + return toPose2D(targetPose_RobotSpace); + } - @JsonProperty("ts") - public double timestamp_LIMELIGHT_publish; + @JsonProperty("ta") + public double ta; - @JsonProperty("ts_rio") - public double timestamp_RIOFPGA_capture; + @JsonProperty("tx") + public double tx; - @JsonProperty("v") - @JsonFormat(shape = Shape.NUMBER) - public boolean valid; + @JsonProperty("txp") + public double tx_pixels; - @JsonProperty("botpose") - public double[] botpose; + @JsonProperty("ty") + public double ty; - @JsonProperty("botpose_wpired") - public double[] botpose_wpired; + @JsonProperty("typ") + public double ty_pixels; - @JsonProperty("botpose_wpiblue") - public double[] botpose_wpiblue; + @JsonProperty("ts") + public double ts; - @JsonProperty("t6c_rs") - public double[] camerapose_robotspace; + public LimelightTarget_Retro() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; + } + } - public Pose3d getBotPose3d() { - return toPose3D(botpose); - } - - public Pose3d getBotPose3d_wpiRed() { - return toPose3D(botpose_wpired); - } - - public Pose3d getBotPose3d_wpiBlue() { - return toPose3D(botpose_wpiblue); - } + public static class LimelightTarget_Fiducial { - public Pose2d getBotPose2d() { - return toPose2D(botpose); - } - - public Pose2d getBotPose2d_wpiRed() { - return toPose2D(botpose_wpired); - } - - public Pose2d getBotPose2d_wpiBlue() { - return toPose2D(botpose_wpiblue); - } + @JsonProperty("fID") + public double fiducialID; - @JsonProperty("Retro") - public LimelightTarget_Retro[] targets_Retro; + @JsonProperty("fam") + public String fiducialFamily; - @JsonProperty("Fiducial") - public LimelightTarget_Fiducial[] targets_Fiducials; + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; - @JsonProperty("Classifier") - public LimelightTarget_Classifier[] targets_Classifier; + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; - @JsonProperty("Detector") - public LimelightTarget_Detector[] targets_Detector; + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; - @JsonProperty("Barcode") - public LimelightTarget_Barcode[] targets_Barcode; + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; - public Results() { - botpose = new double[6]; - botpose_wpired = new double[6]; - botpose_wpiblue = new double[6]; - camerapose_robotspace = new double[6]; - targets_Retro = new LimelightTarget_Retro[0]; - targets_Fiducials = new LimelightTarget_Fiducial[0]; - targets_Classifier = new LimelightTarget_Classifier[0]; - targets_Detector = new LimelightTarget_Detector[0]; - targets_Barcode = new LimelightTarget_Barcode[0]; + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; - } + public Pose3d getCameraPose_TargetSpace() { + return toPose3D(cameraPose_TargetSpace); } - public static class LimelightResults { - @JsonProperty("Results") - public Results targetingResults; - - public LimelightResults() { - targetingResults = new Results(); - } + public Pose3d getRobotPose_FieldSpace() { + return toPose3D(robotPose_FieldSpace); } - private static ObjectMapper mapper; - - /** - * Print JSON Parse time to the console in milliseconds - */ - static boolean profileJSON = false; - - static final String sanitizeName(String name) { - if (name == "" || name == null) { - return "limelight"; - } - return name; + public Pose3d getRobotPose_TargetSpace() { + return toPose3D(robotPose_TargetSpace); } - private static Pose3d toPose3D(double[] inData){ - if(inData.length < 6) - { - System.err.println("Bad LL 3D Pose Data!"); - return new Pose3d(); - } - return new Pose3d( - new Translation3d(inData[0], inData[1], inData[2]), - new Rotation3d(Units.degreesToRadians(inData[3]), Units.degreesToRadians(inData[4]), - Units.degreesToRadians(inData[5]))); + public Pose3d getTargetPose_CameraSpace() { + return toPose3D(targetPose_CameraSpace); } - private static Pose2d toPose2D(double[] inData){ - if(inData.length < 6) - { - System.err.println("Bad LL 2D Pose Data!"); - return new Pose2d(); - } - Translation2d tran2d = new Translation2d(inData[0], inData[1]); - Rotation2d r2d = new Rotation2d(Units.degreesToRadians(inData[5])); - return new Pose2d(tran2d, r2d); + public Pose3d getTargetPose_RobotSpace() { + return toPose3D(targetPose_RobotSpace); } - public static NetworkTable getLimelightNTTable(String tableName) { - return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); + public Pose2d getCameraPose_TargetSpace2D() { + return toPose2D(cameraPose_TargetSpace); } - public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName) { - return getLimelightNTTable(tableName).getEntry(entryName); + public Pose2d getRobotPose_FieldSpace2D() { + return toPose2D(robotPose_FieldSpace); } - public static double getLimelightNTDouble(String tableName, String entryName) { - return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0); + public Pose2d getRobotPose_TargetSpace2D() { + return toPose2D(robotPose_TargetSpace); } - public static void setLimelightNTDouble(String tableName, String entryName, double val) { - getLimelightNTTableEntry(tableName, entryName).setDouble(val); + public Pose2d getTargetPose_CameraSpace2D() { + return toPose2D(targetPose_CameraSpace); } - public static void setLimelightNTDoubleArray(String tableName, String entryName, double[] val) { - getLimelightNTTableEntry(tableName, entryName).setDoubleArray(val); + public Pose2d getTargetPose_RobotSpace2D() { + return toPose2D(targetPose_RobotSpace); } - public static double[] getLimelightNTDoubleArray(String tableName, String entryName) { - return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]); - } + @JsonProperty("ta") + public double ta; - public static String getLimelightNTString(String tableName, String entryName) { - return getLimelightNTTableEntry(tableName, entryName).getString(""); - } + @JsonProperty("tx") + public double tx; - public static URL getLimelightURLString(String tableName, String request) { - String urlString = "http://" + sanitizeName(tableName) + ".local:5807/" + request; - URL url; - try { - url = new URL(urlString); - return url; - } catch (MalformedURLException e) { - System.err.println("bad LL URL"); - } - return null; - } - ///// - ///// + @JsonProperty("txp") + public double tx_pixels; - public static double getTX(String limelightName) { - return getLimelightNTDouble(limelightName, "tx"); - } + @JsonProperty("ty") + public double ty; - public static double getTY(String limelightName) { - return getLimelightNTDouble(limelightName, "ty"); - } + @JsonProperty("typ") + public double ty_pixels; - public static double getTA(String limelightName) { - return getLimelightNTDouble(limelightName, "ta"); - } + @JsonProperty("ts") + public double ts; - public static double getLatency_Pipeline(String limelightName) { - return getLimelightNTDouble(limelightName, "tl"); + public LimelightTarget_Fiducial() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; } + } - public static double getLatency_Capture(String limelightName) { - return getLimelightNTDouble(limelightName, "cl"); - } + public static class LimelightTarget_Barcode {} - public static double getCurrentPipelineIndex(String limelightName) { - return getLimelightNTDouble(limelightName, "getpipe"); - } + public static class LimelightTarget_Classifier { - public static String getJSONDump(String limelightName) { - return getLimelightNTString(limelightName, "json"); - } + @JsonProperty("class") + public String className; - /** - * Switch to getBotPose - * - * @param limelightName - * @return - */ - @Deprecated - public static double[] getBotpose(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose"); - } + @JsonProperty("classID") + public double classID; - /** - * Switch to getBotPose_wpiRed - * - * @param limelightName - * @return - */ - @Deprecated - public static double[] getBotpose_wpiRed(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); - } + @JsonProperty("conf") + public double confidence; - /** - * Switch to getBotPose_wpiBlue - * - * @param limelightName - * @return - */ - @Deprecated - public static double[] getBotpose_wpiBlue(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); - } + @JsonProperty("zone") + public double zone; - public static double[] getBotPose(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose"); - } + @JsonProperty("tx") + public double tx; - public static double[] getBotPose_wpiRed(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); - } + @JsonProperty("txp") + public double tx_pixels; - public static double[] getBotPose_wpiBlue(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); - } + @JsonProperty("ty") + public double ty; - public static double[] getBotPose_TargetSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); - } + @JsonProperty("typ") + public double ty_pixels; - public static double[] getCameraPose_TargetSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); - } + public LimelightTarget_Classifier() {} + } - public static double[] getTargetPose_CameraSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); - } + public static class LimelightTarget_Detector { - public static double[] getTargetPose_RobotSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); - } + @JsonProperty("class") + public String className; - public static double[] getTargetColor(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "tc"); - } + @JsonProperty("classID") + public double classID; - public static double getFiducialID(String limelightName) { - return getLimelightNTDouble(limelightName, "tid"); - } + @JsonProperty("conf") + public double confidence; - public static double getNeuralClassID(String limelightName) { - return getLimelightNTDouble(limelightName, "tclass"); - } + @JsonProperty("ta") + public double ta; - ///// - ///// + @JsonProperty("tx") + public double tx; - public static Pose3d getBotPose3d(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose"); - return toPose3D(poseArray); - } + @JsonProperty("txp") + public double tx_pixels; - public static Pose3d getBotPose3d_wpiRed(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpired"); - return toPose3D(poseArray); - } + @JsonProperty("ty") + public double ty; - public static Pose3d getBotPose3d_wpiBlue(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); - return toPose3D(poseArray); - } + @JsonProperty("typ") + public double ty_pixels; - public static Pose3d getBotPose3d_TargetSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); - return toPose3D(poseArray); - } + public LimelightTarget_Detector() {} + } - public static Pose3d getCameraPose3d_TargetSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); - return toPose3D(poseArray); - } + public static class Results { - public static Pose3d getTargetPose3d_CameraSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); - return toPose3D(poseArray); - } + @JsonProperty("pID") + public double pipelineID; - public static Pose3d getTargetPose3d_RobotSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); - return toPose3D(poseArray); - } + @JsonProperty("tl") + public double latency_pipeline; - public static Pose3d getCameraPose3d_RobotSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_robotspace"); - return toPose3D(poseArray); - } + @JsonProperty("cl") + public double latency_capture; - /** - * Gets the Pose2d for easy use with Odometry vision pose estimator - * (addVisionMeasurement) - * - * @param limelightName - * @return - */ - public static Pose2d getBotPose2d_wpiBlue(String limelightName) { + public double latency_jsonParse; - double[] result = getBotPose_wpiBlue(limelightName); - return toPose2D(result); - } + @JsonProperty("ts") + public double timestamp_LIMELIGHT_publish; - /** - * Gets the Pose2d for easy use with Odometry vision pose estimator - * (addVisionMeasurement) - * - * @param limelightName - * @return - */ - public static Pose2d getBotPose2d_wpiRed(String limelightName) { + @JsonProperty("ts_rio") + public double timestamp_RIOFPGA_capture; - double[] result = getBotPose_wpiRed(limelightName); - return toPose2D(result); + @JsonProperty("v") + @JsonFormat(shape = Shape.NUMBER) + public boolean valid; - } + @JsonProperty("botpose") + public double[] botpose; - /** - * Gets the Pose2d for easy use with Odometry vision pose estimator - * (addVisionMeasurement) - * - * @param limelightName - * @return - */ - public static Pose2d getBotPose2d(String limelightName) { + @JsonProperty("botpose_wpired") + public double[] botpose_wpired; - double[] result = getBotPose(limelightName); - return toPose2D(result); + @JsonProperty("botpose_wpiblue") + public double[] botpose_wpiblue; - } + @JsonProperty("t6c_rs") + public double[] camerapose_robotspace; - public static boolean getTV(String limelightName) { - return 1.0 == getLimelightNTDouble(limelightName, "tv"); + public Pose3d getBotPose3d() { + return toPose3D(botpose); } - ///// - ///// - - public static void setPipelineIndex(String limelightName, int pipelineIndex) { - setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); + public Pose3d getBotPose3d_wpiRed() { + return toPose3D(botpose_wpired); } - /** - * The LEDs will be controlled by Limelight pipeline settings, and not by robot - * code. - */ - public static void setLEDMode_PipelineControl(String limelightName) { - setLimelightNTDouble(limelightName, "ledMode", 0); + public Pose3d getBotPose3d_wpiBlue() { + return toPose3D(botpose_wpiblue); } - public static void setLEDMode_ForceOff(String limelightName) { - setLimelightNTDouble(limelightName, "ledMode", 1); + public Pose2d getBotPose2d() { + return toPose2D(botpose); } - public static void setLEDMode_ForceBlink(String limelightName) { - setLimelightNTDouble(limelightName, "ledMode", 2); + public Pose2d getBotPose2d_wpiRed() { + return toPose2D(botpose_wpired); } - public static void setLEDMode_ForceOn(String limelightName) { - setLimelightNTDouble(limelightName, "ledMode", 3); + public Pose2d getBotPose2d_wpiBlue() { + return toPose2D(botpose_wpiblue); } - public static void setStreamMode_Standard(String limelightName) { - setLimelightNTDouble(limelightName, "stream", 0); - } + @JsonProperty("Retro") + public LimelightTarget_Retro[] targets_Retro; - public static void setStreamMode_PiPMain(String limelightName) { - setLimelightNTDouble(limelightName, "stream", 1); - } + @JsonProperty("Fiducial") + public LimelightTarget_Fiducial[] targets_Fiducials; - public static void setStreamMode_PiPSecondary(String limelightName) { - setLimelightNTDouble(limelightName, "stream", 2); - } + @JsonProperty("Classifier") + public LimelightTarget_Classifier[] targets_Classifier; - public static void setCameraMode_Processor(String limelightName) { - setLimelightNTDouble(limelightName, "camMode", 0); - } - public static void setCameraMode_Driver(String limelightName) { - setLimelightNTDouble(limelightName, "camMode", 1); - } + @JsonProperty("Detector") + public LimelightTarget_Detector[] targets_Detector; + @JsonProperty("Barcode") + public LimelightTarget_Barcode[] targets_Barcode; - /** - * Sets the crop window. The crop window in the UI must be completely open for - * dynamic cropping to work. - */ - public static void setCropWindow(String limelightName, double cropXMin, double cropXMax, double cropYMin, double cropYMax) { - double[] entries = new double[4]; - entries[0] = cropXMin; - entries[1] = cropXMax; - entries[2] = cropYMin; - entries[3] = cropYMax; - setLimelightNTDoubleArray(limelightName, "crop", entries); + public Results() { + botpose = new double[6]; + botpose_wpired = new double[6]; + botpose_wpiblue = new double[6]; + camerapose_robotspace = new double[6]; + targets_Retro = new LimelightTarget_Retro[0]; + targets_Fiducials = new LimelightTarget_Fiducial[0]; + targets_Classifier = new LimelightTarget_Classifier[0]; + targets_Detector = new LimelightTarget_Detector[0]; + targets_Barcode = new LimelightTarget_Barcode[0]; } + } + + public static class LimelightResults { + @JsonProperty("Results") + public Results targetingResults; - public static void setCameraPose_RobotSpace(String limelightName, double forward, double side, double up, double roll, double pitch, double yaw) { - double[] entries = new double[6]; - entries[0] = forward; - entries[1] = side; - entries[2] = up; - entries[3] = roll; - entries[4] = pitch; - entries[5] = yaw; - setLimelightNTDoubleArray(limelightName, "camerapose_robotspace_set", entries); + public LimelightResults() { + targetingResults = new Results(); } + } - ///// - ///// + private static ObjectMapper mapper; - public static void setPythonScriptData(String limelightName, double[] outgoingPythonData) { - setLimelightNTDoubleArray(limelightName, "llrobot", outgoingPythonData); - } + /** Print JSON Parse time to the console in milliseconds */ + static boolean profileJSON = false; - public static double[] getPythonScriptData(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "llpython"); + static final String sanitizeName(String name) { + if (name == "" || name == null) { + return "limelight"; } + return name; + } - ///// - ///// - - /** - * Asynchronously take snapshot. - */ - public static CompletableFuture takeSnapshot(String tableName, String snapshotName) { - return CompletableFuture.supplyAsync(() -> { - return SYNCH_TAKESNAPSHOT(tableName, snapshotName); - }); + private static Pose3d toPose3D(double[] inData) { + if (inData.length < 6) { + System.err.println("Bad LL 3D Pose Data!"); + return new Pose3d(); } + return new Pose3d( + new Translation3d(inData[0], inData[1], inData[2]), + new Rotation3d( + Units.degreesToRadians(inData[3]), + Units.degreesToRadians(inData[4]), + Units.degreesToRadians(inData[5]))); + } - private static boolean SYNCH_TAKESNAPSHOT(String tableName, String snapshotName) { - URL url = getLimelightURLString(tableName, "capturesnapshot"); - try { - HttpURLConnection connection = (HttpURLConnection) url.openConnection(); - connection.setRequestMethod("GET"); - if (snapshotName != null && snapshotName != "") { - connection.setRequestProperty("snapname", snapshotName); - } - - int responseCode = connection.getResponseCode(); - if (responseCode == 200) { - return true; - } else { - System.err.println("Bad LL Request"); - } - } catch (IOException e) { - System.err.println(e.getMessage()); - } - return false; - } - - /** - * Parses Limelight's JSON results dump into a LimelightResults Object - */ - public static LimelightResults getLatestResults(String limelightName) { - - long start = System.nanoTime(); - LimelightHelpers.LimelightResults results = new LimelightHelpers.LimelightResults(); - if (mapper == null) { - mapper = new ObjectMapper().configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false); - } - - try { - results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class); - } catch (JsonProcessingException e) { - System.err.println("lljson error: " + e.getMessage()); - } - - long end = System.nanoTime(); - double millis = (end - start) * .000001; - results.targetingResults.latency_jsonParse = millis; - if (profileJSON) { - System.out.printf("lljson: %.2f\r\n", millis); - } - - return results; + private static Pose2d toPose2D(double[] inData) { + if (inData.length < 6) { + System.err.println("Bad LL 2D Pose Data!"); + return new Pose2d(); } + Translation2d tran2d = new Translation2d(inData[0], inData[1]); + Rotation2d r2d = new Rotation2d(Units.degreesToRadians(inData[5])); + return new Pose2d(tran2d, r2d); + } + + public static NetworkTable getLimelightNTTable(String tableName) { + return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); + } + + public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName) { + return getLimelightNTTable(tableName).getEntry(entryName); + } + + public static double getLimelightNTDouble(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0); + } + + public static void setLimelightNTDouble(String tableName, String entryName, double val) { + getLimelightNTTableEntry(tableName, entryName).setDouble(val); + } + + public static void setLimelightNTDoubleArray(String tableName, String entryName, double[] val) { + getLimelightNTTableEntry(tableName, entryName).setDoubleArray(val); + } + + public static double[] getLimelightNTDoubleArray(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]); + } + + public static String getLimelightNTString(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getString(""); + } + + public static URL getLimelightURLString(String tableName, String request) { + String urlString = "http://" + sanitizeName(tableName) + ".local:5807/" + request; + URL url; + try { + url = new URL(urlString); + return url; + } catch (MalformedURLException e) { + System.err.println("bad LL URL"); + } + return null; + } + + ///// + ///// + + public static double getTX(String limelightName) { + return getLimelightNTDouble(limelightName, "tx"); + } + + public static double getTY(String limelightName) { + return getLimelightNTDouble(limelightName, "ty"); + } + + public static double getTA(String limelightName) { + return getLimelightNTDouble(limelightName, "ta"); + } + + public static double getLatency_Pipeline(String limelightName) { + return getLimelightNTDouble(limelightName, "tl"); + } + + public static double getLatency_Capture(String limelightName) { + return getLimelightNTDouble(limelightName, "cl"); + } + + public static double getCurrentPipelineIndex(String limelightName) { + return getLimelightNTDouble(limelightName, "getpipe"); + } + + public static String getJSONDump(String limelightName) { + return getLimelightNTString(limelightName, "json"); + } + + /** + * Switch to getBotPose + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + /** + * Switch to getBotPose_wpiRed + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + /** + * Switch to getBotPose_wpiBlue + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + public static double[] getBotPose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + public static double[] getBotPose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + public static double[] getBotPose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + public static double[] getBotPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + } + + public static double[] getCameraPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + } + + public static double[] getTargetPose_CameraSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + } + + public static double[] getTargetPose_RobotSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + } + + public static double[] getTargetColor(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "tc"); + } + + public static double getFiducialID(String limelightName) { + return getLimelightNTDouble(limelightName, "tid"); + } + + public static double getNeuralClassID(String limelightName) { + return getLimelightNTDouble(limelightName, "tclass"); + } + + ///// + ///// + + public static Pose3d getBotPose3d(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose"); + return toPose3D(poseArray); + } + + public static Pose3d getBotPose3d_wpiRed(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + return toPose3D(poseArray); + } + + public static Pose3d getBotPose3d_wpiBlue(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + return toPose3D(poseArray); + } + + public static Pose3d getBotPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + return toPose3D(poseArray); + } + + public static Pose3d getCameraPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + return toPose3D(poseArray); + } + + public static Pose3d getTargetPose3d_CameraSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + return toPose3D(poseArray); + } + + public static Pose3d getTargetPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + return toPose3D(poseArray); + } + + public static Pose3d getCameraPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_robotspace"); + return toPose3D(poseArray); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiBlue(String limelightName) { + + double[] result = getBotPose_wpiBlue(limelightName); + return toPose2D(result); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiRed(String limelightName) { + + double[] result = getBotPose_wpiRed(limelightName); + return toPose2D(result); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d(String limelightName) { + + double[] result = getBotPose(limelightName); + return toPose2D(result); + } + + public static boolean getTV(String limelightName) { + return 1.0 == getLimelightNTDouble(limelightName, "tv"); + } + + ///// + ///// + + public static void setPipelineIndex(String limelightName, int pipelineIndex) { + setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); + } + + /** The LEDs will be controlled by Limelight pipeline settings, and not by robot code. */ + public static void setLEDMode_PipelineControl(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 0); + } + + public static void setLEDMode_ForceOff(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 1); + } + + public static void setLEDMode_ForceBlink(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 2); + } + + public static void setLEDMode_ForceOn(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 3); + } + + public static void setStreamMode_Standard(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 0); + } + + public static void setStreamMode_PiPMain(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 1); + } + + public static void setStreamMode_PiPSecondary(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 2); + } + + public static void setCameraMode_Processor(String limelightName) { + setLimelightNTDouble(limelightName, "camMode", 0); + } + + public static void setCameraMode_Driver(String limelightName) { + setLimelightNTDouble(limelightName, "camMode", 1); + } + + /** + * Sets the crop window. The crop window in the UI must be completely open for dynamic cropping to + * work. + */ + public static void setCropWindow( + String limelightName, double cropXMin, double cropXMax, double cropYMin, double cropYMax) { + double[] entries = new double[4]; + entries[0] = cropXMin; + entries[1] = cropXMax; + entries[2] = cropYMin; + entries[3] = cropYMax; + setLimelightNTDoubleArray(limelightName, "crop", entries); + } + + public static void setCameraPose_RobotSpace( + String limelightName, + double forward, + double side, + double up, + double roll, + double pitch, + double yaw) { + double[] entries = new double[6]; + entries[0] = forward; + entries[1] = side; + entries[2] = up; + entries[3] = roll; + entries[4] = pitch; + entries[5] = yaw; + setLimelightNTDoubleArray(limelightName, "camerapose_robotspace_set", entries); + } + + ///// + ///// + + public static void setPythonScriptData(String limelightName, double[] outgoingPythonData) { + setLimelightNTDoubleArray(limelightName, "llrobot", outgoingPythonData); + } + + public static double[] getPythonScriptData(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "llpython"); + } + + ///// + ///// + + /** Asynchronously take snapshot. */ + public static CompletableFuture takeSnapshot(String tableName, String snapshotName) { + return CompletableFuture.supplyAsync( + () -> { + return SYNCH_TAKESNAPSHOT(tableName, snapshotName); + }); + } + + private static boolean SYNCH_TAKESNAPSHOT(String tableName, String snapshotName) { + URL url = getLimelightURLString(tableName, "capturesnapshot"); + try { + HttpURLConnection connection = (HttpURLConnection) url.openConnection(); + connection.setRequestMethod("GET"); + if (snapshotName != null && snapshotName != "") { + connection.setRequestProperty("snapname", snapshotName); + } + + int responseCode = connection.getResponseCode(); + if (responseCode == 200) { + return true; + } else { + System.err.println("Bad LL Request"); + } + } catch (IOException e) { + System.err.println(e.getMessage()); + } + return false; + } + + /** Parses Limelight's JSON results dump into a LimelightResults Object */ + public static LimelightResults getLatestResults(String limelightName) { + + long start = System.nanoTime(); + LimelightHelpers.LimelightResults results = new LimelightHelpers.LimelightResults(); + if (mapper == null) { + mapper = + new ObjectMapper().configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false); + } + + try { + results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class); + } catch (JsonProcessingException e) { + System.err.println("lljson error: " + e.getMessage()); + } + + long end = System.nanoTime(); + double millis = (end - start) * .000001; + results.targetingResults.latency_jsonParse = millis; + if (profileJSON) { + System.out.printf("lljson: %.2f\r\n", millis); + } + + return results; + } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1d1e383..8bafc7b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,7 +4,6 @@ package frc.robot; - import com.ctre.phoenix6.SignalLogger; import edu.wpi.first.math.geometry.Pose2d; @@ -28,10 +27,11 @@ public void robotInit() { SignalLogger.start(); } + @Override public void robotPeriodic() { - CommandScheduler.getInstance().run(); - if(UseLimelight) { + CommandScheduler.getInstance().run(); + if (UseLimelight) { var lastResult = LimelightHelpers.getLatestResults("limelight").targetingResults; Pose2d llPose = lastResult.getBotPose2d_wpiBlue(); @@ -77,8 +77,7 @@ public void teleopInit() { public void teleopPeriodic() {} @Override - public void teleopExit() { - } + public void teleopExit() {} @Override public void testInit() { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cabed9b..4d4825f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -20,10 +20,15 @@ public class RobotContainer { /* Setting up bindings for necessary control of the swerve drive platform */ CommandXboxController joystick = new CommandXboxController(0); // My joystick CommandSwerveDrivetrain drivetrain = TunerConstants.DriveTrain; // My drivetrain - SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric().withIsOpenLoop(true).withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1); // I want field-centric - // driving in open loop + SwerveRequest.FieldCentric drive = + new SwerveRequest.FieldCentric() + .withIsOpenLoop(true) + .withDeadband(MaxSpeed * 0.1) + .withRotationalDeadband(MaxAngularRate * 0.1); // I want field-centric + // driving in open loop SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); - SwerveRequest.RobotCentric forwardStraight = new SwerveRequest.RobotCentric().withIsOpenLoop(true); + SwerveRequest.RobotCentric forwardStraight = + new SwerveRequest.RobotCentric().withIsOpenLoop(true); SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); /* Path follower */ @@ -35,23 +40,42 @@ public class RobotContainer { private void configureBindings() { drivetrain.setDefaultCommand( // Drivetrain will execute this command periodically - drivetrain.applyRequest(() -> drive.withVelocityX(-joystick.getLeftY() * MaxSpeed) // Drive forward with - // negative Y (forward) - .withVelocityY(-joystick.getLeftX() * MaxSpeed) // Drive left with negative X (left) - .withRotationalRate(-joystick.getRightX() * MaxAngularRate) // Drive counterclockwise with negative X (left) - ).ignoringDisable(true)); + drivetrain + .applyRequest( + () -> + drive + .withVelocityX(-joystick.getLeftY() * MaxSpeed) // Drive forward with + // negative Y (forward) + .withVelocityY( + -joystick.getLeftX() * MaxSpeed) // Drive left with negative X (left) + .withRotationalRate( + -joystick.getRightX() + * MaxAngularRate) // Drive counterclockwise with negative X (left) + ) + .ignoringDisable(true)); joystick.a().whileTrue(drivetrain.applyRequest(() -> brake)); - joystick.b().whileTrue(drivetrain - .applyRequest(() -> point.withModuleDirection(new Rotation2d(-joystick.getLeftY(), -joystick.getLeftX())))); + joystick + .b() + .whileTrue( + drivetrain.applyRequest( + () -> + point.withModuleDirection( + new Rotation2d(-joystick.getLeftY(), -joystick.getLeftX())))); // if (Utils.isSimulation()) { // drivetrain.seedFieldRelative(new Pose2d(new Translation2d(), Rotation2d.fromDegrees(90))); // } drivetrain.registerTelemetry(logger::telemeterize); - joystick.pov(0).whileTrue(drivetrain.applyRequest(()->forwardStraight.withVelocityX(0.5).withVelocityY(0))); - joystick.pov(180).whileTrue(drivetrain.applyRequest(()->forwardStraight.withVelocityX(-0.5).withVelocityY(0))); + joystick + .pov(0) + .whileTrue( + drivetrain.applyRequest(() -> forwardStraight.withVelocityX(0.5).withVelocityY(0))); + joystick + .pov(180) + .whileTrue( + drivetrain.applyRequest(() -> forwardStraight.withVelocityX(-0.5).withVelocityY(0))); } public RobotContainer() { @@ -60,7 +84,7 @@ public RobotContainer() { public Command getAutonomousCommand() { /* First put the drivetrain into auto run mode, then run the auto */ - return new InstantCommand(()->{}); + return new InstantCommand(() -> {}); } public boolean seedPoseButtonDown() { diff --git a/src/main/java/frc/robot/Telemetry.java b/src/main/java/frc/robot/Telemetry.java index 3fb3ded..16e9089 100644 --- a/src/main/java/frc/robot/Telemetry.java +++ b/src/main/java/frc/robot/Telemetry.java @@ -19,101 +19,114 @@ import edu.wpi.first.wpilibj.util.Color8Bit; public class Telemetry { - private final double MaxSpeed; - private int logEntry; - private int odomEntry; - - /** - * Construct a telemetry object, with the specified max speed of the robot - * - * @param maxSpeed Maximum speed in meters per second - */ - public Telemetry(double maxSpeed) { - MaxSpeed = maxSpeed; - logEntry = DataLogManager.getLog().start("odometry", "double[]"); - odomEntry = DataLogManager.getLog().start("odom period", "double"); + private final double MaxSpeed; + private int logEntry; + private int odomEntry; + + /** + * Construct a telemetry object, with the specified max speed of the robot + * + * @param maxSpeed Maximum speed in meters per second + */ + public Telemetry(double maxSpeed) { + MaxSpeed = maxSpeed; + logEntry = DataLogManager.getLog().start("odometry", "double[]"); + odomEntry = DataLogManager.getLog().start("odom period", "double"); + } + + /* What to publish over networktables for telemetry */ + NetworkTableInstance inst = NetworkTableInstance.getDefault(); + + /* Robot pose for field positioning */ + NetworkTable table = inst.getTable("Pose"); + DoubleArrayPublisher fieldPub = table.getDoubleArrayTopic("robotPose").publish(); + StringPublisher fieldTypePub = table.getStringTopic(".type").publish(); + + /* Robot speeds for general checking */ + NetworkTable driveStats = inst.getTable("Drive"); + DoublePublisher velocityX = driveStats.getDoubleTopic("Velocity X").publish(); + DoublePublisher velocityY = driveStats.getDoubleTopic("Velocity Y").publish(); + DoublePublisher speed = driveStats.getDoubleTopic("Speed").publish(); + DoublePublisher odomPeriod = driveStats.getDoubleTopic("Odometry Period").publish(); + + /* Keep a reference of the last pose to calculate the speeds */ + Pose2d m_lastPose = new Pose2d(); + double lastTime = Utils.getCurrentTimeSeconds(); + + /* Mechanisms to represent the swerve module states */ + Mechanism2d[] m_moduleMechanisms = + new Mechanism2d[] { + new Mechanism2d(1, 1), new Mechanism2d(1, 1), new Mechanism2d(1, 1), new Mechanism2d(1, 1), + }; + /* A direction and length changing ligament for speed representation */ + MechanismLigament2d[] m_moduleSpeeds = + new MechanismLigament2d[] { + m_moduleMechanisms[0] + .getRoot("RootSpeed", 0.5, 0.5) + .append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[1] + .getRoot("RootSpeed", 0.5, 0.5) + .append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[2] + .getRoot("RootSpeed", 0.5, 0.5) + .append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[3] + .getRoot("RootSpeed", 0.5, 0.5) + .append(new MechanismLigament2d("Speed", 0.5, 0)), + }; + /* A direction changing and length constant ligament for module direction */ + MechanismLigament2d[] m_moduleDirections = + new MechanismLigament2d[] { + m_moduleMechanisms[0] + .getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[1] + .getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[2] + .getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[3] + .getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + }; + + /* Accept the swerve drive state and telemeterize it to smartdashboard */ + public void telemeterize(SwerveDriveState state) { + /* Telemeterize the pose */ + Pose2d pose = state.Pose; + fieldTypePub.set("Field2d"); + fieldPub.set(new double[] {pose.getX(), pose.getY(), pose.getRotation().getDegrees()}); + + /* Telemeterize the robot's general speeds */ + double currentTime = Utils.getCurrentTimeSeconds(); + double diffTime = currentTime - lastTime; + lastTime = currentTime; + Translation2d distanceDiff = pose.minus(m_lastPose).getTranslation(); + m_lastPose = pose; + + Translation2d velocities = distanceDiff.div(diffTime); + + speed.set(velocities.getNorm()); + velocityX.set(velocities.getX()); + velocityY.set(velocities.getY()); + odomPeriod.set(1.0 / state.OdometryPeriod); + + /* Telemeterize the module's states */ + for (int i = 0; i < 4; ++i) { + m_moduleSpeeds[i].setAngle(state.ModuleStates[i].angle); + m_moduleDirections[i].setAngle(state.ModuleStates[i].angle); + m_moduleSpeeds[i].setLength(state.ModuleStates[i].speedMetersPerSecond / (2 * MaxSpeed)); + + SmartDashboard.putData("Module " + i, m_moduleMechanisms[i]); } - /* What to publish over networktables for telemetry */ - NetworkTableInstance inst = NetworkTableInstance.getDefault(); - - /* Robot pose for field positioning */ - NetworkTable table = inst.getTable("Pose"); - DoubleArrayPublisher fieldPub = table.getDoubleArrayTopic("robotPose").publish(); - StringPublisher fieldTypePub = table.getStringTopic(".type").publish(); - - /* Robot speeds for general checking */ - NetworkTable driveStats = inst.getTable("Drive"); - DoublePublisher velocityX = driveStats.getDoubleTopic("Velocity X").publish(); - DoublePublisher velocityY = driveStats.getDoubleTopic("Velocity Y").publish(); - DoublePublisher speed = driveStats.getDoubleTopic("Speed").publish(); - DoublePublisher odomPeriod = driveStats.getDoubleTopic("Odometry Period").publish(); - - /* Keep a reference of the last pose to calculate the speeds */ - Pose2d m_lastPose = new Pose2d(); - double lastTime = Utils.getCurrentTimeSeconds(); - - /* Mechanisms to represent the swerve module states */ - Mechanism2d[] m_moduleMechanisms = new Mechanism2d[] { - new Mechanism2d(1, 1), - new Mechanism2d(1, 1), - new Mechanism2d(1, 1), - new Mechanism2d(1, 1), - }; - /* A direction and length changing ligament for speed representation */ - MechanismLigament2d[] m_moduleSpeeds = new MechanismLigament2d[] { - m_moduleMechanisms[0].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), - m_moduleMechanisms[1].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), - m_moduleMechanisms[2].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), - m_moduleMechanisms[3].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), - }; - /* A direction changing and length constant ligament for module direction */ - MechanismLigament2d[] m_moduleDirections = new MechanismLigament2d[] { - m_moduleMechanisms[0].getRoot("RootDirection", 0.5, 0.5) - .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - m_moduleMechanisms[1].getRoot("RootDirection", 0.5, 0.5) - .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - m_moduleMechanisms[2].getRoot("RootDirection", 0.5, 0.5) - .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - m_moduleMechanisms[3].getRoot("RootDirection", 0.5, 0.5) - .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - }; - - /* Accept the swerve drive state and telemeterize it to smartdashboard */ - public void telemeterize(SwerveDriveState state) { - /* Telemeterize the pose */ - Pose2d pose = state.Pose; - fieldTypePub.set("Field2d"); - fieldPub.set(new double[] { - pose.getX(), - pose.getY(), - pose.getRotation().getDegrees() - }); - - /* Telemeterize the robot's general speeds */ - double currentTime = Utils.getCurrentTimeSeconds(); - double diffTime = currentTime - lastTime; - lastTime = currentTime; - Translation2d distanceDiff = pose.minus(m_lastPose).getTranslation(); - m_lastPose = pose; - - Translation2d velocities = distanceDiff.div(diffTime); - - speed.set(velocities.getNorm()); - velocityX.set(velocities.getX()); - velocityY.set(velocities.getY()); - odomPeriod.set(1.0 / state.OdometryPeriod); - - /* Telemeterize the module's states */ - for (int i = 0; i < 4; ++i) { - m_moduleSpeeds[i].setAngle(state.ModuleStates[i].angle); - m_moduleDirections[i].setAngle(state.ModuleStates[i].angle); - m_moduleSpeeds[i].setLength(state.ModuleStates[i].speedMetersPerSecond / (2 * MaxSpeed)); - - SmartDashboard.putData("Module " + i, m_moduleMechanisms[i]); - } - - DataLogManager.getLog().appendDoubleArray(logEntry, new double[] {pose.getX(), pose.getY(), pose.getRotation().getDegrees()}, (long)(Timer.getFPGATimestamp() * 1000000)); - DataLogManager.getLog().appendDouble(odomEntry, state.OdometryPeriod, (long)(Timer.getFPGATimestamp() * 1000000)); - } + DataLogManager.getLog() + .appendDoubleArray( + logEntry, + new double[] {pose.getX(), pose.getY(), pose.getRotation().getDegrees()}, + (long) (Timer.getFPGATimestamp() * 1000000)); + DataLogManager.getLog() + .appendDouble(odomEntry, state.OdometryPeriod, (long) (Timer.getFPGATimestamp() * 1000000)); + } } diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index 28ce7ee..a66c7d8 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -10,92 +10,124 @@ import frc.robot.CommandSwerveDrivetrain; public class TunerConstants { - static class CustomSlotGains extends Slot0Configs { - public CustomSlotGains(double kP, double kI, double kD, double kV, double kS) { - this.kP = kP; - this.kI = kI; - this.kD = kD; - this.kV = kV; - this.kS = kS; - } + static class CustomSlotGains extends Slot0Configs { + public CustomSlotGains(double kP, double kI, double kD, double kV, double kS) { + this.kP = kP; + this.kI = kI; + this.kD = kD; + this.kV = kV; + this.kS = kS; } - - private static final CustomSlotGains steerGains = new CustomSlotGains(100, 0, 0.05, 0, 0); - private static final CustomSlotGains driveGains = new CustomSlotGains(3, 0, 0, 0, 0); - - private static final double kCoupleRatio = 3.5; - - private static final double kDriveGearRatio = 7.363636364; - private static final double kSteerGearRatio = 15.42857143; - private static final double kWheelRadiusInches = 2.167; // Estimated at first, then fudge-factored to make odom match record - private static final int kPigeonId = 1; - private static final boolean kSteerMotorReversed = true; - private static final String kCANbusName = "rio"; - private static final boolean kInvertLeftSide = false; - private static final boolean kInvertRightSide = true; - - - private static double kSteerInertia = 0.00001; - private static double kDriveInertia = 0.001; - - private static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() - .withPigeon2Id(kPigeonId) - .withCANbusName(kCANbusName) - .withSupportsPro(true); - - private static final SwerveModuleConstantsFactory ConstantCreator = new SwerveModuleConstantsFactory() - .withDriveMotorGearRatio(kDriveGearRatio) - .withSteerMotorGearRatio(kSteerGearRatio) - .withWheelRadius(kWheelRadiusInches) - .withSlipCurrent(30) - .withSteerMotorGains(steerGains) - .withDriveMotorGains(driveGains) - .withSpeedAt12VoltsMps(6) // Theoretical free speed is 10 meters per second at 12v applied output - .withSteerInertia(kSteerInertia) - .withDriveInertia(kDriveInertia) - .withFeedbackSource(SwerveModuleSteerFeedbackType.FusedCANcoder) - .withCouplingGearRatio(kCoupleRatio) // Every 1 rotation of the azimuth results in couple ratio drive turns - .withSteerMotorInverted(kSteerMotorReversed); - - private static final int kFrontLeftDriveMotorId = 5; - private static final int kFrontLeftSteerMotorId = 4; - private static final int kFrontLeftEncoderId = 2; - private static final double kFrontLeftEncoderOffset = -0.83544921875; - - private static final double kFrontLeftXPosInches = 10.5; - private static final double kFrontLeftYPosInches = 10.5; - private static final int kFrontRightDriveMotorId = 7; - private static final int kFrontRightSteerMotorId = 6; - private static final int kFrontRightEncoderId = 3; - private static final double kFrontRightEncoderOffset = -0.15234375; - - private static final double kFrontRightXPosInches = 10.5; - private static final double kFrontRightYPosInches = -10.5; - private static final int kBackLeftDriveMotorId = 1; - private static final int kBackLeftSteerMotorId = 0; - private static final int kBackLeftEncoderId = 0; - private static final double kBackLeftEncoderOffset = -0.4794921875; - - private static final double kBackLeftXPosInches = -10.5; - private static final double kBackLeftYPosInches = 10.5; - private static final int kBackRightDriveMotorId = 3; - private static final int kBackRightSteerMotorId = 2; - private static final int kBackRightEncoderId = 1; - private static final double kBackRightEncoderOffset = -0.84130859375; - - private static final double kBackRightXPosInches = -10.5; - private static final double kBackRightYPosInches = -10.5; - - - private static final SwerveModuleConstants FrontLeft = ConstantCreator.createModuleConstants( - kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset, Units.inchesToMeters(kFrontLeftXPosInches), Units.inchesToMeters(kFrontLeftYPosInches), kInvertLeftSide); - private static final SwerveModuleConstants FrontRight = ConstantCreator.createModuleConstants( - kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset, Units.inchesToMeters(kFrontRightXPosInches), Units.inchesToMeters(kFrontRightYPosInches), kInvertRightSide); - private static final SwerveModuleConstants BackLeft = ConstantCreator.createModuleConstants( - kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset, Units.inchesToMeters(kBackLeftXPosInches), Units.inchesToMeters(kBackLeftYPosInches), kInvertLeftSide); - private static final SwerveModuleConstants BackRight = ConstantCreator.createModuleConstants( - kBackRightSteerMotorId, kBackRightDriveMotorId, kBackRightEncoderId, kBackRightEncoderOffset, Units.inchesToMeters(kBackRightXPosInches), Units.inchesToMeters(kBackRightYPosInches), kInvertRightSide); - - public static final CommandSwerveDrivetrain DriveTrain = new CommandSwerveDrivetrain(DrivetrainConstants, 250, FrontLeft, - FrontRight, BackLeft, BackRight); + } + + private static final CustomSlotGains steerGains = new CustomSlotGains(100, 0, 0.05, 0, 0); + private static final CustomSlotGains driveGains = new CustomSlotGains(3, 0, 0, 0, 0); + + private static final double kCoupleRatio = 3.5; + + private static final double kDriveGearRatio = 7.363636364; + private static final double kSteerGearRatio = 15.42857143; + private static final double kWheelRadiusInches = + 2.167; // Estimated at first, then fudge-factored to make odom match record + private static final int kPigeonId = 1; + private static final boolean kSteerMotorReversed = true; + private static final String kCANbusName = "rio"; + private static final boolean kInvertLeftSide = false; + private static final boolean kInvertRightSide = true; + + private static double kSteerInertia = 0.00001; + private static double kDriveInertia = 0.001; + + private static final SwerveDrivetrainConstants DrivetrainConstants = + new SwerveDrivetrainConstants() + .withPigeon2Id(kPigeonId) + .withCANbusName(kCANbusName) + .withSupportsPro(true); + + private static final SwerveModuleConstantsFactory ConstantCreator = + new SwerveModuleConstantsFactory() + .withDriveMotorGearRatio(kDriveGearRatio) + .withSteerMotorGearRatio(kSteerGearRatio) + .withWheelRadius(kWheelRadiusInches) + .withSlipCurrent(30) + .withSteerMotorGains(steerGains) + .withDriveMotorGains(driveGains) + .withSpeedAt12VoltsMps( + 6) // Theoretical free speed is 10 meters per second at 12v applied output + .withSteerInertia(kSteerInertia) + .withDriveInertia(kDriveInertia) + .withFeedbackSource(SwerveModuleSteerFeedbackType.FusedCANcoder) + .withCouplingGearRatio( + kCoupleRatio) // Every 1 rotation of the azimuth results in couple ratio drive turns + .withSteerMotorInverted(kSteerMotorReversed); + + private static final int kFrontLeftDriveMotorId = 5; + private static final int kFrontLeftSteerMotorId = 4; + private static final int kFrontLeftEncoderId = 2; + private static final double kFrontLeftEncoderOffset = -0.83544921875; + + private static final double kFrontLeftXPosInches = 10.5; + private static final double kFrontLeftYPosInches = 10.5; + private static final int kFrontRightDriveMotorId = 7; + private static final int kFrontRightSteerMotorId = 6; + private static final int kFrontRightEncoderId = 3; + private static final double kFrontRightEncoderOffset = -0.15234375; + + private static final double kFrontRightXPosInches = 10.5; + private static final double kFrontRightYPosInches = -10.5; + private static final int kBackLeftDriveMotorId = 1; + private static final int kBackLeftSteerMotorId = 0; + private static final int kBackLeftEncoderId = 0; + private static final double kBackLeftEncoderOffset = -0.4794921875; + + private static final double kBackLeftXPosInches = -10.5; + private static final double kBackLeftYPosInches = 10.5; + private static final int kBackRightDriveMotorId = 3; + private static final int kBackRightSteerMotorId = 2; + private static final int kBackRightEncoderId = 1; + private static final double kBackRightEncoderOffset = -0.84130859375; + + private static final double kBackRightXPosInches = -10.5; + private static final double kBackRightYPosInches = -10.5; + + private static final SwerveModuleConstants FrontLeft = + ConstantCreator.createModuleConstants( + kFrontLeftSteerMotorId, + kFrontLeftDriveMotorId, + kFrontLeftEncoderId, + kFrontLeftEncoderOffset, + Units.inchesToMeters(kFrontLeftXPosInches), + Units.inchesToMeters(kFrontLeftYPosInches), + kInvertLeftSide); + private static final SwerveModuleConstants FrontRight = + ConstantCreator.createModuleConstants( + kFrontRightSteerMotorId, + kFrontRightDriveMotorId, + kFrontRightEncoderId, + kFrontRightEncoderOffset, + Units.inchesToMeters(kFrontRightXPosInches), + Units.inchesToMeters(kFrontRightYPosInches), + kInvertRightSide); + private static final SwerveModuleConstants BackLeft = + ConstantCreator.createModuleConstants( + kBackLeftSteerMotorId, + kBackLeftDriveMotorId, + kBackLeftEncoderId, + kBackLeftEncoderOffset, + Units.inchesToMeters(kBackLeftXPosInches), + Units.inchesToMeters(kBackLeftYPosInches), + kInvertLeftSide); + private static final SwerveModuleConstants BackRight = + ConstantCreator.createModuleConstants( + kBackRightSteerMotorId, + kBackRightDriveMotorId, + kBackRightEncoderId, + kBackRightEncoderOffset, + Units.inchesToMeters(kBackRightXPosInches), + Units.inchesToMeters(kBackRightYPosInches), + kInvertRightSide); + + public static final CommandSwerveDrivetrain DriveTrain = + new CommandSwerveDrivetrain( + DrivetrainConstants, 250, FrontLeft, FrontRight, BackLeft, BackRight); } From 12bc04e656c9e6a46e61006e253ac69442bfcdb6 Mon Sep 17 00:00:00 2001 From: IanTapply22 Date: Sun, 12 Nov 2023 13:44:36 -0500 Subject: [PATCH 12/14] Remove linting check --- .../workflows/{build-lint.yml => build.yml} | 20 +------------------ 1 file changed, 1 insertion(+), 19 deletions(-) rename .github/workflows/{build-lint.yml => build.yml} (61%) diff --git a/.github/workflows/build-lint.yml b/.github/workflows/build.yml similarity index 61% rename from .github/workflows/build-lint.yml rename to .github/workflows/build.yml index c668b40..39bb4bb 100644 --- a/.github/workflows/build-lint.yml +++ b/.github/workflows/build.yml @@ -1,4 +1,4 @@ -name: Build and Lint +name: Build # Controls when the action will run. Triggers the workflow on push to all branches. on: [ push ] @@ -28,21 +28,3 @@ jobs: # Runs a single command using the runners shell - name: Compile and run tests on robot code run: ./gradlew build - - # Lints our code to find any format violations - lint: - # The type of runner that the job will run on - runs-on: ubuntu-latest - - steps: - # Checks-out your repository under $GITHUB_WORKSPACE, so your job can access it - - uses: actions/checkout@v3 - with: - fetch-depth: 0 - - uses: actions/setup-java@v3 - with: - distribution: 'zulu' - java-version: 17 - # Executes linting - # If this fails, run ./gradlew spotlessApply locally to fix formatting - - run: ./gradlew spotlessCheck From 8486afaf9628269f282d0a6bb49427e199819d4e Mon Sep 17 00:00:00 2001 From: IanTapply22 Date: Sun, 12 Nov 2023 14:24:33 -0500 Subject: [PATCH 13/14] Made spotless only google java format --- build.gradle | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/build.gradle b/build.gradle index f6de22f..f88b430 100644 --- a/build.gradle +++ b/build.gradle @@ -15,11 +15,11 @@ spotless { include '**/*.java' exclude '**/build/**', '**/build-*/**' } - toggleOffOn() + // toggleOffOn() googleJavaFormat() - removeUnusedImports() - trimTrailingWhitespace() - endWithNewline() + // removeUnusedImports() + // trimTrailingWhitespace() + // endWithNewline() } } From 609ee9066a723e28961a05d9d3bf3b2ea523087e Mon Sep 17 00:00:00 2001 From: IanTapply22 Date: Sun, 12 Nov 2023 14:26:31 -0500 Subject: [PATCH 14/14] Removed spotless --- build.gradle | 15 --------------- 1 file changed, 15 deletions(-) diff --git a/build.gradle b/build.gradle index f88b430..b233ef1 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,5 @@ plugins { id "java" - id 'com.diffplug.spotless' version "6.12.0" id "edu.wpi.first.GradleRIO" version "2023.4.3" } @@ -9,20 +8,6 @@ targetCompatibility = JavaVersion.VERSION_11 def ROBOT_MAIN_CLASS = "frc.robot.Main" -spotless { - java { - target fileTree('.') { - include '**/*.java' - exclude '**/build/**', '**/build-*/**' - } - // toggleOffOn() - googleJavaFormat() - // removeUnusedImports() - // trimTrailingWhitespace() - // endWithNewline() - } -} - // Define my targets (RoboRIO) and artifacts (deployable files) // This is added by GradleRIO's backing project DeployUtils. deploy {