diff --git a/.gitignore b/.gitignore index 8f73c0cf..8af68c68 100644 --- a/.gitignore +++ b/.gitignore @@ -169,6 +169,8 @@ out/ .fleet # Simulation GUI and other tools window save file +networktables.json +simgui.json *-window.json # Simulation data log directory @@ -177,12 +179,15 @@ logs/ # Folder that has CTRE Phoenix Sim device config storage ctre_sim/ -BuildConstants.java -networktables.json -simgui-ds.json -simgui.json +# clangd +/.cache +compile_commands.json -!src/main/deploy/photon-configs/*.zip +# Eclipse generated file for annotation processors +.factorypath +BuildConstants.java +simgui-ds.json /*.wpilog /*-profile.json +!src/main/deploy/photon-configs/*.zip diff --git a/.vscode/settings.json b/.vscode/settings.json index 41509c47..6b3ebfb5 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -18,14 +18,48 @@ { "name": "WPIlibUnitTests", "workingDirectory": "${workspaceFolder}/build/jni/release", - "vmargs": [ "-Djava.library.path=${workspaceFolder}/build/jni/release" ], + "vmargs": [ + "-Djava.library.path=${workspaceFolder}/build/jni/release" + ], "env": { - "LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" , + "LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release", "DYLD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" } }, ], "java.test.defaultConfig": "WPIlibUnitTests", + "java.import.gradle.annotationProcessing.enabled": false, + "java.completion.favoriteStaticMembers": [ + "org.junit.Assert.*", + "org.junit.Assume.*", + "org.junit.jupiter.api.Assertions.*", + "org.junit.jupiter.api.Assumptions.*", + "org.junit.jupiter.api.DynamicContainer.*", + "org.junit.jupiter.api.DynamicTest.*", + "org.mockito.Mockito.*", + "org.mockito.ArgumentMatchers.*", + "org.mockito.Answers.*", + "edu.wpi.first.units.Units.*" + ], + "java.completion.filteredTypes": [ + "java.awt.*", + "com.sun.*", + "sun.*", + "jdk.*", + "org.graalvm.*", + "io.micrometer.shaded.*", + "java.beans.*", + "java.util.Base64.*", + "java.util.Timer", + "java.sql.*", + "javax.swing.*", + "javax.management.*", + "javax.smartcardio.*", + "edu.wpi.first.math.proto.*", + "edu.wpi.first.math.**.proto.*", + "edu.wpi.first.math.**.struct.*", + ], + "java.test.defaultConfig": "WPIlibUnitTests", "java.format.settings.url": "formatter.xml", "java.format.settings.profile": "GoogleStyle", "[java]": { @@ -37,4 +71,4 @@ "editor.formatOnSave": true, "editor.formatOnPaste": true, "terminal.integrated.defaultProfile.windows": "Git Bash" -} +} \ No newline at end of file diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index 73526a5d..81736c3f 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -1,6 +1,6 @@ { "enableCppIntellisense": false, "currentLanguage": "java", - "projectYear": "2024", + "projectYear": "2025", "teamNumber": 5572 } \ No newline at end of file diff --git a/WPILib-License.md b/WPILib-License.md index 43b62ec2..645e5425 100644 --- a/WPILib-License.md +++ b/WPILib-License.md @@ -1,4 +1,4 @@ -Copyright (c) 2009-2023 FIRST and other WPILib contributors +Copyright (c) 2009-2024 FIRST and other WPILib contributors All rights reserved. Redistribution and use in source and binary forms, with or without diff --git a/build.gradle b/build.gradle index efa08734..8d174e91 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.3.2" + id "edu.wpi.first.GradleRIO" version "2025.2.1" id "com.peterabeles.gversion" version "1.10" } @@ -28,19 +28,14 @@ deploy { // getTargetTypeClass is a shortcut to get the class type using a string frcJava(getArtifactTypeClass('FRCJavaArtifact')) { - // jvmArgs.add("-XX:StartFlightRecording=duration=30s,filename=/home/lvuser/flight.jfr") - // jvmArgs.add("-Dcom.sun.management.jmxremote=true") - // jvmArgs.add("-Dcom.sun.management.jmxremote.port=1198") - // jvmArgs.add("-Dcom.sun.management.jmxremote.local.only=false") - // jvmArgs.add("-Dcom.sun.management.jmxremote.ssl=false") - // jvmArgs.add("-Dcom.sun.management.jmxremote.authenticate=false") - // jvmArgs.add("-Djava.rmi.server.hostname=10.55.72.2") // Replace XX.XX with team number } // Static files artifact frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { files = project.fileTree('src/main/deploy') directory = '/home/lvuser/deploy' + deleteOldFiles = false // Change to true to delete files on roboRIO that no + // longer exist in deploy directory of this project } } } @@ -57,32 +52,20 @@ def includeDesktopSupport = false // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. // Also defines JUnit 5. - repositories { - 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" - } - } mavenLocal() maven { url 'https://jitpack.io' } } -configurations.all { - exclude group: "edu.wpi.first.wpilibj" -} - -task(checkAkitInstall, dependsOn: "classes", type: JavaExec) { - mainClass = "org.littletonrobotics.junction.CheckInstall" +task(replayWatch, type: JavaExec) { + mainClass = "org.littletonrobotics.junction.ReplayWatch" classpath = sourceSets.main.runtimeClasspath } -compileJava.finalizedBy checkAkitInstall dependencies { def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) - annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitJson.version" + annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version" + annotationProcessor wpi.java.deps.wpilibAnnotations() implementation wpi.java.deps.wpilib() implementation wpi.java.vendor.java() @@ -100,12 +83,20 @@ dependencies { nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop) simulationRelease wpi.sim.enableRelease() + testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' + testRuntimeOnly 'org.junit.platform:junit-platform-launcher' + annotationProcessor 'com.github.Frc5572:RobotTools:v1.1.1' implementation 'org.apache.httpcomponents:httpclient:4.5.14' implementation 'org.apache.httpcomponents:httpmime:4.5.14' } +test { + useJUnitPlatform() + systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true' +} + // Simulation configuration (e.g. environment variables). wpi.sim.addGui().defaultEnabled = true wpi.sim.addDriverstation() diff --git a/gradle/wrapper/gradle-wrapper.jar b/gradle/wrapper/gradle-wrapper.jar index d64cd491..a4b76b95 100644 Binary files a/gradle/wrapper/gradle-wrapper.jar and b/gradle/wrapper/gradle-wrapper.jar differ diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties index 5e82d67b..34bd9ce9 100644 --- a/gradle/wrapper/gradle-wrapper.properties +++ b/gradle/wrapper/gradle-wrapper.properties @@ -1,6 +1,6 @@ distributionBase=GRADLE_USER_HOME distributionPath=permwrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-8.5-bin.zip +distributionUrl=https\://services.gradle.org/distributions/gradle-8.11-bin.zip networkTimeout=10000 validateDistributionUrl=true zipStoreBase=GRADLE_USER_HOME diff --git a/gradlew b/gradlew index 1aa94a42..f5feea6d 100755 --- a/gradlew +++ b/gradlew @@ -15,6 +15,8 @@ # See the License for the specific language governing permissions and # limitations under the License. # +# SPDX-License-Identifier: Apache-2.0 +# ############################################################################## # @@ -55,7 +57,7 @@ # Darwin, MinGW, and NonStop. # # (3) This script is generated from the Groovy template -# https://github.com/gradle/gradle/blob/HEAD/subprojects/plugins/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt +# https://github.com/gradle/gradle/blob/HEAD/platforms/jvm/plugins-application/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt # within the Gradle project. # # You can find Gradle at https://github.com/gradle/gradle/. @@ -84,7 +86,8 @@ done # shellcheck disable=SC2034 APP_BASE_NAME=${0##*/} # Discard cd standard output in case $CDPATH is set (https://github.com/gradle/gradle/issues/25036) -APP_HOME=$( cd "${APP_HOME:-./}" > /dev/null && pwd -P ) || exit +APP_HOME=$( cd -P "${APP_HOME:-./}" > /dev/null && printf '%s +' "$PWD" ) || exit # Use the maximum available, or set MAX_FD != -1 to use that value. MAX_FD=maximum diff --git a/gradlew.bat b/gradlew.bat index 93e3f59f..9d21a218 100644 --- a/gradlew.bat +++ b/gradlew.bat @@ -13,6 +13,8 @@ @rem See the License for the specific language governing permissions and @rem limitations under the License. @rem +@rem SPDX-License-Identifier: Apache-2.0 +@rem @if "%DEBUG%"=="" @echo off @rem ########################################################################## @@ -43,11 +45,11 @@ set JAVA_EXE=java.exe %JAVA_EXE% -version >NUL 2>&1 if %ERRORLEVEL% equ 0 goto execute -echo. -echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. -echo. -echo Please set the JAVA_HOME variable in your environment to match the -echo location of your Java installation. +echo. 1>&2 +echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. 1>&2 +echo. 1>&2 +echo Please set the JAVA_HOME variable in your environment to match the 1>&2 +echo location of your Java installation. 1>&2 goto fail @@ -57,11 +59,11 @@ set JAVA_EXE=%JAVA_HOME%/bin/java.exe if exist "%JAVA_EXE%" goto execute -echo. -echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% -echo. -echo Please set the JAVA_HOME variable in your environment to match the -echo location of your Java installation. +echo. 1>&2 +echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% 1>&2 +echo. 1>&2 +echo Please set the JAVA_HOME variable in your environment to match the 1>&2 +echo location of your Java installation. 1>&2 goto fail diff --git a/settings.gradle b/settings.gradle index d94f73c6..969c7b09 100644 --- a/settings.gradle +++ b/settings.gradle @@ -4,7 +4,7 @@ pluginManagement { repositories { mavenLocal() gradlePluginPortal() - String frcYear = '2024' + String frcYear = '2025' File frcHome if (OperatingSystem.current().isWindows()) { String publicFolder = System.getenv('PUBLIC') diff --git a/src/main/java/frc/lib/profiling/LoggingProfiler.java b/src/main/java/frc/lib/profiling/LoggingProfiler.java index 3ae4559d..7aa1fd31 100644 --- a/src/main/java/frc/lib/profiling/LoggingProfiler.java +++ b/src/main/java/frc/lib/profiling/LoggingProfiler.java @@ -109,6 +109,7 @@ public void endTick() { if (!this.tickStarted) { throw new RuntimeException("Profiler tick already ended. Missing startTick()?"); } else { + this.push(fullPath); this.pop(); this.tickStarted = false; if (!this.fullPath.isEmpty()) { diff --git a/src/main/java/frc/lib/util/photon/PhotonCameraWrapper.java b/src/main/java/frc/lib/util/photon/PhotonCameraWrapper.java deleted file mode 100644 index 7a7d723f..00000000 --- a/src/main/java/frc/lib/util/photon/PhotonCameraWrapper.java +++ /dev/null @@ -1,168 +0,0 @@ -package frc.lib.util.photon; - -import java.util.Optional; -import org.littletonrobotics.junction.Logger; -import org.photonvision.EstimatedRobotPose; -import org.photonvision.PhotonPoseEstimator.PoseStrategy; -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc.lib.util.photon.PhotonIO.PhotonInputs; -import frc.robot.Constants; - -/** - * PhotonCamera-based Pose Estimator. - */ -public class PhotonCameraWrapper { - public PhotonIO io; - public PhotonInputs inputs = new PhotonInputs(); - public PhotonIOPoseEstimator photonPoseEstimator; - private double resetTimer = 0; - - /** - * PhotonCamera-based Pose Estimator. - * - * @param io Camera IO. - * @param robotToCam transform from robot body coordinates to camera coordinates. - */ - public PhotonCameraWrapper(PhotonIO io, Transform3d robotToCam) { - this.io = io; - - // Attempt to load the AprilTagFieldLayout that will tell us where the tags are on the - // field. - AprilTagFieldLayout fieldLayout = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); - // Create pose estimator - photonPoseEstimator = new PhotonIOPoseEstimator(fieldLayout, - PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, this.inputs, robotToCam); - photonPoseEstimator.setMultiTagFallbackStrategy(PoseStrategy.CLOSEST_TO_REFERENCE_POSE); - - // SmartDashboard.putNumber("rx", 0); - // SmartDashboard.putNumber("ry", 0); - // SmartDashboard.putNumber("rz", 0); - // SmartDashboard.putNumber("tx", 0); - // SmartDashboard.putNumber("ty", 0); - // SmartDashboard.putNumber("tz", 0); - - } - - /** - * Update inputs and such. - */ - public void periodic() { - this.io.updateInputs(this.inputs); - Logger.processInputs("PhotonVision/" + inputs.name, inputs); - // if (this.inputs.distCoeffs.length == 0 && Timer.getFPGATimestamp() - resetTimer > 5) { - // resetTimer = Timer.getFPGATimestamp(); - // try { - // this.io.uploadSettings(this.io.ip + ":5800", - // new File(Filesystem.getDeployDirectory().getAbsoluteFile(), - // "photon-configs/" + inputs.name + ".zip")); - // } catch (IOException e) { - // e.printStackTrace(); - // } - // } - } - - /** - * Gets if photonvision can see a target. - */ - public boolean seesTarget() { - return inputs.result != null && inputs.result.hasTargets(); - } - - /** A PhotonVision tag solve. */ - public static class VisionObservation { - public int fudicialId; - public Pose2d robotPose; - public Matrix stdDev; - - /** All fields constructor. */ - public VisionObservation(int fudicialId, Pose2d robotPose, Matrix stdDev) { - this.fudicialId = fudicialId; - this.robotPose = robotPose; - this.stdDev = stdDev; - } - } - - /** - * Get estimated pose without a prior. - * - * @return an estimated Pose2d based solely on apriltags - */ - public Optional getInitialPose() { - var res = inputs.result; - if (res == null || inputs.timeSinceLastHeartbeat > 0.5) { - SmartDashboard.putString("reason", "res is null or heartbeat too long"); - return Optional.empty(); - } - if (res.hasTargets()) { - var target = res.getBestTarget(); - var camToTargetTrans = target.getBestCameraToTarget(); - var aprilTagPose = - photonPoseEstimator.getFieldTags().getTagPose(target.getFiducialId()); - if (aprilTagPose.isPresent()) { - var camPose = aprilTagPose.get().transformBy(camToTargetTrans.inverse()); - var robotPose = - camPose.transformBy(photonPoseEstimator.getRobotToCameraTransform()).toPose2d(); - - Translation2d toTarget = - new Pose3d().plus(camToTargetTrans).toPose2d().getTranslation(); - double stdDev = - toTarget.getX() * toTarget.getX() + toTarget.getY() * toTarget.getY(); - return Optional.of(new VisionObservation(target.getFiducialId(), robotPose, - VecBuilder.fill(stdDev * Constants.CameraConstants.XY_STD_DEV_COEFF, - stdDev * Constants.CameraConstants.XY_STD_DEV_COEFF, - stdDev * Constants.CameraConstants.THETA_STD_DEV_COEFF))); - } - } - SmartDashboard.putString("reason", "no targets"); - return Optional.empty(); - } - - public double latency() { - var res = inputs.result; - return Timer.getFPGATimestamp() - res.getTimestampSeconds(); - } - - /** - * @param prevEstimatedRobotPose The current best guess at robot pose - * - * @return an EstimatedRobotPose with an estimated pose, the timestamp, and targets used to - * create the estimate - */ - public Optional getEstimatedGlobalPose(Pose2d prevEstimatedRobotPose) { - var res = inputs.result; - - // photonPoseEstimator.setRobotToCameraTransform(new Transform3d( - // new Translation3d(Units.inchesToMeters(SmartDashboard.getNumber("tx", 0)), - // Units.inchesToMeters(SmartDashboard.getNumber("ty", 0)), - // Units.inchesToMeters(SmartDashboard.getNumber("tz", 0))), - // new Rotation3d(Math.toRadians(SmartDashboard.getNumber("rx", 0)), - // Math.toRadians(SmartDashboard.getNumber("ry", 0)), - // Math.toRadians(SmartDashboard.getNumber("rz", 0))))); - - // SmartDashboard.putNumber("photonLatency", - // Timer.getFPGATimestamp() - res.getTimestampSeconds()); - if (Timer.getFPGATimestamp() - res.getTimestampSeconds() > 0.4) { - SmartDashboard.putString("reason", "too long"); - return Optional.empty(); - } - if (photonPoseEstimator == null) { - SmartDashboard.putString("reason", "no estimator"); - // The field layout failed to load, so we cannot estimate poses. - return Optional.empty(); - } - photonPoseEstimator.setReferencePose(prevEstimatedRobotPose); - SmartDashboard.putString("reason", "good"); - return photonPoseEstimator.update(); - } -} diff --git a/src/main/java/frc/lib/util/photon/PhotonIO.java b/src/main/java/frc/lib/util/photon/PhotonIO.java deleted file mode 100755 index a5d7ca90..00000000 --- a/src/main/java/frc/lib/util/photon/PhotonIO.java +++ /dev/null @@ -1,115 +0,0 @@ -package frc.lib.util.photon; - -import java.io.File; -import java.io.IOException; -import java.util.Optional; -import org.littletonrobotics.junction.LogTable; -import org.littletonrobotics.junction.inputs.LoggableInputs; -import org.photonvision.common.dataflow.structures.Packet; -import org.photonvision.targeting.PhotonPipelineResult; -import edu.wpi.first.math.MatBuilder; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.Nat; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.math.numbers.N5; - -/** PhotonVision Camera IO. */ -public abstract class PhotonIO { - - /** - * Inputs from PhotonVision. - */ - public static class PhotonInputs implements LoggableInputs { - - public byte[] rawBytes; - public PhotonPipelineResult result; - public String versionString; - public double timeSinceLastHeartbeat; - public double[] cameraMatrix; - public double[] distCoeffs; - public String name; - - /** - * Default Constructor. - */ - public PhotonInputs() { - rawBytes = new byte[0]; - result = null; - versionString = ""; - timeSinceLastHeartbeat = -1; - } - - @Override - public void toLog(LogTable table) { - table.put("rawBytes", rawBytes); - table.put("versionString", versionString); - table.put("timeSinceLastHeartbeat", timeSinceLastHeartbeat); - table.put("cameraMatrix", cameraMatrix); - table.put("distCoeffs", distCoeffs); - } - - @Override - public void fromLog(LogTable table) { - this.rawBytes = table.get("rawBytes", rawBytes); - if (rawBytes.length > 0) { - Packet p = new Packet(rawBytes); - this.result = PhotonPipelineResult.serde.unpack(p); - } else { - this.result = null; - } - - this.versionString = table.get("versionString", versionString); - this.timeSinceLastHeartbeat = - table.get("timeSinceLastHeartbeat", timeSinceLastHeartbeat); - this.cameraMatrix = table.get("cameraMatrix", cameraMatrix); - this.distCoeffs = table.get("distCoeffs", distCoeffs); - } - - /** - * Camera Matrix from PhotonVision - */ - public Optional> getCameraMatrix() { - if (cameraMatrix != null && cameraMatrix.length == 9) { - return Optional.of(MatBuilder.fill(Nat.N3(), Nat.N3(), cameraMatrix)); - } else { - return Optional.empty(); - } - } - - /** - * Distortion Coefficients from PhotonVision - */ - public Optional> getDistCoeffs() { - if (distCoeffs != null && distCoeffs.length == 5) { - return Optional.of(MatBuilder.fill(Nat.N5(), Nat.N1(), distCoeffs)); - } else { - return Optional.empty(); - } - } - - } - - protected final String name; - protected final String ip; - - /** - * Photon IO - * - * @param name Camera Name - * @param ip Camera IP - */ - public PhotonIO(String name, String ip) { - this.name = name; - this.ip = ip; - - } - - public void updateInputs(PhotonInputs inputs) { - inputs.name = name; - } - - public boolean uploadSettings(String ip, File file) throws IOException { - return false; - } -} diff --git a/src/main/java/frc/lib/util/photon/PhotonIOPoseEstimator.java b/src/main/java/frc/lib/util/photon/PhotonIOPoseEstimator.java deleted file mode 100755 index c47fd7e3..00000000 --- a/src/main/java/frc/lib/util/photon/PhotonIOPoseEstimator.java +++ /dev/null @@ -1,675 +0,0 @@ -/* - * MIT License - * - * Copyright (c) PhotonVision - * - * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and - * associated documentation files (the "Software"), to deal in the Software without restriction, - * including without limitation the rights to use, copy, modify, merge, publish, distribute, - * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all copies or - * substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT - * NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND - * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, - * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - * - * Later modified by FRC Team 5572. - */ - -package frc.lib.util.photon; - -import java.util.ArrayList; -import java.util.HashSet; -import java.util.List; -import java.util.Optional; -import java.util.Set; -import org.photonvision.EstimatedRobotPose; -import org.photonvision.PhotonPoseEstimator.PoseStrategy; -import org.photonvision.estimation.TargetModel; -import org.photonvision.estimation.VisionEstimation; -import org.photonvision.targeting.PhotonPipelineResult; -import org.photonvision.targeting.PhotonTrackedTarget; -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.hal.FRCNetComm.tResourceType; -import edu.wpi.first.hal.HAL; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.Pair; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.math.numbers.N5; -import edu.wpi.first.wpilibj.DriverStation; -import frc.lib.util.photon.PhotonIO.PhotonInputs; - -/** - * The PhotonPoseEstimator class filters or combines readings from all the AprilTags visible at a - * given timestamp on the field to produce a single robot in field pose, using the strategy set - * below. Example usage can be found in our apriltagExample example project. - */ -public class PhotonIOPoseEstimator { - private static int InstanceCount = 0; - - private AprilTagFieldLayout fieldTags; - private TargetModel tagModel = TargetModel.kAprilTag16h5; - private PoseStrategy primaryStrategy; - private PoseStrategy multiTagFallbackStrategy = PoseStrategy.LOWEST_AMBIGUITY; - private final PhotonInputs camera; - private Transform3d robotToCamera; - - private Pose3d lastPose; - private Pose3d referencePose; - protected double poseCacheTimestampSeconds = -1; - private final Set reportedErrors = new HashSet<>(); - - /** - * Create a new PhotonPoseEstimator. - * - * @param fieldTags A WPILib {@link AprilTagFieldLayout} linking AprilTag IDs to Pose3d objects - * with respect to the FIRST field using the Field - * Coordinate System. Note that setting the origin of this layout object will affect - * the results from this class. - * @param strategy The strategy it should use to determine the best pose. - * @param camera PhotonCamera - * @param robotToCamera Transform3d from the center of the robot to the camera mount position - * (ie, robot -> camera) in the Robot - * Coordinate System. - */ - public PhotonIOPoseEstimator(AprilTagFieldLayout fieldTags, PoseStrategy strategy, - PhotonInputs camera, Transform3d robotToCamera) { - this.fieldTags = fieldTags; - this.primaryStrategy = strategy; - this.camera = camera; - this.robotToCamera = robotToCamera; - - HAL.report(tResourceType.kResourceType_PhotonPoseEstimator, InstanceCount); - InstanceCount++; - } - - public PhotonIOPoseEstimator(AprilTagFieldLayout fieldTags, PoseStrategy strategy, - Transform3d robotToCamera) { - this(fieldTags, strategy, null, robotToCamera); - } - - /** Invalidates the pose cache. */ - private void invalidatePoseCache() { - poseCacheTimestampSeconds = -1; - } - - private void checkUpdate(Object oldObj, Object newObj) { - if (oldObj != newObj && oldObj != null && !oldObj.equals(newObj)) { - invalidatePoseCache(); - } - } - - /** - * Get the AprilTagFieldLayout being used by the PositionEstimator. - * - *

- * Note: Setting the origin of this layout will affect the results from this class. - * - * @return the AprilTagFieldLayout - */ - public AprilTagFieldLayout getFieldTags() { - return fieldTags; - } - - /** - * Set the AprilTagFieldLayout being used by the PositionEstimator. - * - *

- * Note: Setting the origin of this layout will affect the results from this class. - * - * @param fieldTags the AprilTagFieldLayout - */ - public void setFieldTags(AprilTagFieldLayout fieldTags) { - checkUpdate(this.fieldTags, fieldTags); - this.fieldTags = fieldTags; - } - - /** - * Get the TargetModel representing the tags being detected. This is used for on-rio multitag. - * - *

- * By default, this is {@link TargetModel#kAprilTag16h5}. - */ - public TargetModel getTagModel() { - return tagModel; - } - - /** - * Set the TargetModel representing the tags being detected. This is used for on-rio multitag. - * - * @param tagModel E.g. {@link TargetModel#kAprilTag16h5}. - */ - public void setTagModel(TargetModel tagModel) { - this.tagModel = tagModel; - } - - /** - * Get the Position Estimation Strategy being used by the Position Estimator. - * - * @return the strategy - */ - public PoseStrategy getPrimaryStrategy() { - return primaryStrategy; - } - - /** - * Set the Position Estimation Strategy used by the Position Estimator. - * - * @param strategy the strategy to set - */ - public void setPrimaryStrategy(PoseStrategy strategy) { - checkUpdate(this.primaryStrategy, strategy); - this.primaryStrategy = strategy; - } - - /** - * Set the Position Estimation Strategy used in multi-tag mode when only one tag can be seen. - * Must NOT be MULTI_TAG_PNP - * - * @param strategy the strategy to set - */ - public void setMultiTagFallbackStrategy(PoseStrategy strategy) { - checkUpdate(this.multiTagFallbackStrategy, strategy); - if (strategy == PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR - || strategy == PoseStrategy.MULTI_TAG_PNP_ON_RIO) { - DriverStation.reportWarning( - "Fallback cannot be set to MULTI_TAG_PNP! Setting to lowest ambiguity", false); - strategy = PoseStrategy.LOWEST_AMBIGUITY; - } - this.multiTagFallbackStrategy = strategy; - } - - /** - * Return the reference position that is being used by the estimator. - * - * @return the referencePose - */ - public Pose3d getReferencePose() { - return referencePose; - } - - /** - * Update the stored reference pose for use when using the CLOSEST_TO_REFERENCE_POSE - * strategy. - * - * @param referencePose the referencePose to set - */ - public void setReferencePose(Pose3d referencePose) { - checkUpdate(this.referencePose, referencePose); - this.referencePose = referencePose; - } - - /** - * Update the stored reference pose for use when using the CLOSEST_TO_REFERENCE_POSE - * strategy. - * - * @param referencePose the referencePose to set - */ - public void setReferencePose(Pose2d referencePose) { - setReferencePose(new Pose3d(referencePose)); - } - - /** - * Update the stored last pose. Useful for setting the initial estimate when using the - * CLOSEST_TO_LAST_POSE strategy. - * - * @param lastPose the lastPose to set - */ - public void setLastPose(Pose3d lastPose) { - this.lastPose = lastPose; - } - - /** - * Update the stored last pose. Useful for setting the initial estimate when using the - * CLOSEST_TO_LAST_POSE strategy. - * - * @param lastPose the lastPose to set - */ - public void setLastPose(Pose2d lastPose) { - setLastPose(new Pose3d(lastPose)); - } - - /** - * @return The current transform from the center of the robot to the camera mount position - */ - public Transform3d getRobotToCameraTransform() { - return robotToCamera; - } - - /** - * Useful for pan and tilt mechanisms and such. - * - * @param robotToCamera The current transform from the center of the robot to the camera mount - * position - */ - public void setRobotToCameraTransform(Transform3d robotToCamera) { - this.robotToCamera = robotToCamera; - } - - /** - * Poll data from the configured cameras and update the estimated position of the robot. Returns - * empty if: - * - *

    - *
  • New data has not been received since the last call to {@code update()}. - *
  • No targets were found from the camera - *
  • There is no camera set - *
- * - * @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to - * create the estimate. - */ - public Optional update() { - if (camera == null) { - DriverStation.reportError("[PhotonPoseEstimator] Missing camera!", false); - return Optional.empty(); - } - - PhotonPipelineResult cameraResult = camera.result; - - return update(cameraResult, camera.getCameraMatrix(), camera.getDistCoeffs()); - } - - /** - * Updates the estimated position of the robot. Returns empty if: - * - *
    - *
  • The timestamp of the provided pipeline result is the same as in the previous call to - * {@code update()}. - *
  • No targets were found in the pipeline results. - *
- * - * @param cameraResult The latest pipeline result from the camera - * @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to - * create the estimate. - */ - public Optional update(PhotonPipelineResult cameraResult) { - if (camera == null) { - return update(cameraResult, Optional.empty(), Optional.empty()); - } - return update(cameraResult, camera.getCameraMatrix(), camera.getDistCoeffs()); - } - - /** - * Updates the estimated position of the robot. Returns empty if: - * - *
    - *
  • The timestamp of the provided pipeline result is the same as in the previous call to - * {@code update()}. - *
  • No targets were found in the pipeline results. - *
- * - * @param cameraMatrix Camera calibration data that can be used in the case of no assigned - * PhotonCamera. - * @param distCoeffs Camera calibration data that can be used in the case of no assigned - * PhotonCamera - * @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to - * create the estimate. - */ - public Optional update(PhotonPipelineResult cameraResult, - Optional> cameraMatrix, Optional> distCoeffs) { - // Time in the past -- give up, since the following if expects times > 0 - if (cameraResult.getTimestampSeconds() < 0) { - return Optional.empty(); - } - - // If the pose cache timestamp was set, and the result is from the same - // timestamp, return an - // empty result - if (poseCacheTimestampSeconds > 0 - && Math.abs(poseCacheTimestampSeconds - cameraResult.getTimestampSeconds()) < 1e-6) { - return Optional.empty(); - } - - // Remember the timestamp of the current result used - poseCacheTimestampSeconds = cameraResult.getTimestampSeconds(); - - // If no targets seen, trivial case -- return empty result - if (!cameraResult.hasTargets()) { - return Optional.empty(); - } - - return update(cameraResult, cameraMatrix, distCoeffs, this.primaryStrategy); - } - - private Optional update(PhotonPipelineResult cameraResult, - Optional> cameraMatrix, Optional> distCoeffs, - PoseStrategy strategy) { - Optional estimatedPose; - switch (strategy) { - case LOWEST_AMBIGUITY: - estimatedPose = lowestAmbiguityStrategy(cameraResult); - break; - case CLOSEST_TO_CAMERA_HEIGHT: - estimatedPose = closestToCameraHeightStrategy(cameraResult); - break; - case CLOSEST_TO_REFERENCE_POSE: - estimatedPose = closestToReferencePoseStrategy(cameraResult, referencePose); - break; - case CLOSEST_TO_LAST_POSE: - setReferencePose(lastPose); - estimatedPose = closestToReferencePoseStrategy(cameraResult, referencePose); - break; - case AVERAGE_BEST_TARGETS: - estimatedPose = averageBestTargetsStrategy(cameraResult); - break; - case MULTI_TAG_PNP_ON_RIO: - estimatedPose = multiTagOnRioStrategy(cameraResult, cameraMatrix, distCoeffs); - break; - case MULTI_TAG_PNP_ON_COPROCESSOR: - estimatedPose = multiTagOnCoprocStrategy(cameraResult, cameraMatrix, distCoeffs); - break; - default: - DriverStation.reportError( - "[PhotonPoseEstimator] Unknown Position Estimation Strategy!", false); - return Optional.empty(); - } - - if (estimatedPose.isPresent()) { - lastPose = estimatedPose.get().estimatedPose; - } - - return estimatedPose; - } - - private Optional multiTagOnCoprocStrategy(PhotonPipelineResult result, - Optional> cameraMatrixOpt, Optional> distCoeffsOpt) { - if (result.getMultiTagResult().estimatedPose.isPresent) { - var best_tf = result.getMultiTagResult().estimatedPose.best; - var best = new Pose3d().plus(best_tf) // field-to-camera - .relativeTo(fieldTags.getOrigin()).plus(robotToCamera.inverse()); // field-to-robot - return Optional.of(new EstimatedRobotPose(best, result.getTimestampSeconds(), - result.getTargets(), PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR)); - } else { - return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy); - } - } - - private Optional multiTagOnRioStrategy(PhotonPipelineResult result, - Optional> cameraMatrixOpt, Optional> distCoeffsOpt) { - boolean hasCalibData = cameraMatrixOpt.isPresent() && distCoeffsOpt.isPresent(); - // cannot run multitagPNP, use fallback strategy - if (!hasCalibData || result.getTargets().size() < 2) { - return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy); - } - - var pnpResult = VisionEstimation.estimateCamPosePNP(cameraMatrixOpt.get(), - distCoeffsOpt.get(), result.getTargets(), fieldTags, tagModel); - // try fallback strategy if solvePNP fails for some reason - if (!pnpResult.isPresent) { - return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy); - } - var best = new Pose3d().plus(pnpResult.best) // field-to-camera - .plus(robotToCamera.inverse()); // field-to-robot - - return Optional.of(new EstimatedRobotPose(best, result.getTimestampSeconds(), - result.getTargets(), PoseStrategy.MULTI_TAG_PNP_ON_RIO)); - } - - /** - * Return the estimated position of the robot with the lowest position ambiguity from a List of - * pipeline results. - * - * @param result pipeline result - * @return the estimated position of the robot in the FCS and the estimated timestamp of this - * estimation. - */ - private Optional lowestAmbiguityStrategy(PhotonPipelineResult result) { - PhotonTrackedTarget lowestAmbiguityTarget = null; - - double lowestAmbiguityScore = 10; - - for (PhotonTrackedTarget target : result.targets) { - double targetPoseAmbiguity = target.getPoseAmbiguity(); - // Make sure the target is a Fiducial target. - if (targetPoseAmbiguity != -1 && targetPoseAmbiguity < lowestAmbiguityScore) { - lowestAmbiguityScore = targetPoseAmbiguity; - lowestAmbiguityTarget = target; - } - } - - // Although there are confirmed to be targets, none of them may be fiducial - // targets. - if (lowestAmbiguityTarget == null) { - return Optional.empty(); - } - - int targetFiducialId = lowestAmbiguityTarget.getFiducialId(); - - Optional targetPosition = fieldTags.getTagPose(targetFiducialId); - - if (targetPosition.isEmpty()) { - reportFiducialPoseError(targetFiducialId); - return Optional.empty(); - } - - return Optional.of(new EstimatedRobotPose( - targetPosition.get() - .transformBy(lowestAmbiguityTarget.getBestCameraToTarget().inverse()) - .transformBy(robotToCamera.inverse()), - result.getTimestampSeconds(), result.getTargets(), PoseStrategy.LOWEST_AMBIGUITY)); - } - - /** - * Return the estimated position of the robot using the target with the lowest delta height - * difference between the estimated and actual height of the camera. - * - * @param result pipeline result - * @return the estimated position of the robot in the FCS and the estimated timestamp of this - * estimation. - */ - private Optional closestToCameraHeightStrategy( - PhotonPipelineResult result) { - double smallestHeightDifference = 10e9; - EstimatedRobotPose closestHeightTarget = null; - - for (PhotonTrackedTarget target : result.targets) { - int targetFiducialId = target.getFiducialId(); - - // Don't report errors for non-fiducial targets. This could also be resolved by - // adding -1 to - // the initial HashSet. - if (targetFiducialId == -1) { - continue; - } - - Optional targetPosition = fieldTags.getTagPose(target.getFiducialId()); - - if (targetPosition.isEmpty()) { - reportFiducialPoseError(target.getFiducialId()); - continue; - } - - double alternateTransformDelta = Math.abs(robotToCamera.getZ() - targetPosition.get() - .transformBy(target.getAlternateCameraToTarget().inverse()).getZ()); - double bestTransformDelta = Math.abs(robotToCamera.getZ() - targetPosition.get() - .transformBy(target.getBestCameraToTarget().inverse()).getZ()); - - if (alternateTransformDelta < smallestHeightDifference) { - smallestHeightDifference = alternateTransformDelta; - closestHeightTarget = new EstimatedRobotPose( - targetPosition.get().transformBy(target.getAlternateCameraToTarget().inverse()) - .transformBy(robotToCamera.inverse()), - result.getTimestampSeconds(), result.getTargets(), - PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT); - } - - if (bestTransformDelta < smallestHeightDifference) { - smallestHeightDifference = bestTransformDelta; - closestHeightTarget = new EstimatedRobotPose( - targetPosition.get().transformBy(target.getBestCameraToTarget().inverse()) - .transformBy(robotToCamera.inverse()), - result.getTimestampSeconds(), result.getTargets(), - PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT); - } - } - - // Need to null check here in case none of the provided targets are fiducial. - return Optional.ofNullable(closestHeightTarget); - } - - /** - * Return the estimated position of the robot using the target with the lowest delta in the - * vector magnitude between it and the reference pose. - * - * @param result pipeline result - * @param referencePose reference pose to check vector magnitude difference against. - * @return the estimated position of the robot in the FCS and the estimated timestamp of this - * estimation. - */ - private Optional closestToReferencePoseStrategy(PhotonPipelineResult result, - Pose3d referencePose) { - if (referencePose == null) { - DriverStation - .reportError("[PhotonPoseEstimator] Tried to use reference pose strategy without " - + "setting the reference!", false); - return Optional.empty(); - } - - double smallestPoseDelta = 10e9; - EstimatedRobotPose lowestDeltaPose = null; - - for (PhotonTrackedTarget target : result.targets) { - int targetFiducialId = target.getFiducialId(); - - // Don't report errors for non-fiducial targets. This could also be resolved by - // adding -1 to - // the initial HashSet. - if (targetFiducialId == -1) { - continue; - } - - Optional targetPosition = fieldTags.getTagPose(target.getFiducialId()); - - if (targetPosition.isEmpty()) { - reportFiducialPoseError(targetFiducialId); - continue; - } - - Pose3d altTransformPosition = - targetPosition.get().transformBy(target.getAlternateCameraToTarget().inverse()) - .transformBy(robotToCamera.inverse()); - Pose3d bestTransformPosition = - targetPosition.get().transformBy(target.getBestCameraToTarget().inverse()) - .transformBy(robotToCamera.inverse()); - - double altDifference = - Math.abs(calculateDifference(referencePose, altTransformPosition)); - double bestDifference = - Math.abs(calculateDifference(referencePose, bestTransformPosition)); - - if (altDifference < smallestPoseDelta) { - smallestPoseDelta = altDifference; - lowestDeltaPose = - new EstimatedRobotPose(altTransformPosition, result.getTimestampSeconds(), - result.getTargets(), PoseStrategy.CLOSEST_TO_REFERENCE_POSE); - } - if (bestDifference < smallestPoseDelta) { - smallestPoseDelta = bestDifference; - lowestDeltaPose = - new EstimatedRobotPose(bestTransformPosition, result.getTimestampSeconds(), - result.getTargets(), PoseStrategy.CLOSEST_TO_REFERENCE_POSE); - } - } - return Optional.ofNullable(lowestDeltaPose); - } - - /** - * Return the average of the best target poses using ambiguity as weight. - * - * @param result pipeline result - * @return the estimated position of the robot in the FCS and the estimated timestamp of this - * estimation. - */ - private Optional averageBestTargetsStrategy(PhotonPipelineResult result) { - List> estimatedRobotPoses = new ArrayList<>(); - double totalAmbiguity = 0; - - for (PhotonTrackedTarget target : result.targets) { - int targetFiducialId = target.getFiducialId(); - - // Don't report errors for non-fiducial targets. This could also be resolved by - // adding -1 to - // the initial HashSet. - if (targetFiducialId == -1) { - continue; - } - - Optional targetPosition = fieldTags.getTagPose(target.getFiducialId()); - - if (targetPosition.isEmpty()) { - reportFiducialPoseError(targetFiducialId); - continue; - } - - double targetPoseAmbiguity = target.getPoseAmbiguity(); - - // Pose ambiguity is 0, use that pose - if (targetPoseAmbiguity == 0) { - return Optional.of(new EstimatedRobotPose( - targetPosition.get().transformBy(target.getBestCameraToTarget().inverse()) - .transformBy(robotToCamera.inverse()), - result.getTimestampSeconds(), result.getTargets(), - PoseStrategy.AVERAGE_BEST_TARGETS)); - } - - totalAmbiguity += 1.0 / target.getPoseAmbiguity(); - - estimatedRobotPoses.add(new Pair<>(target, - targetPosition.get().transformBy(target.getBestCameraToTarget().inverse()) - .transformBy(robotToCamera.inverse()))); - } - - // Take the average - - Translation3d transform = new Translation3d(); - Rotation3d rotation = new Rotation3d(); - - if (estimatedRobotPoses.isEmpty()) { - return Optional.empty(); - } - - for (Pair pair : estimatedRobotPoses) { - // Total ambiguity is non-zero confirmed because if it was zero, that pose was - // returned. - double weight = (1.0 / pair.getFirst().getPoseAmbiguity()) / totalAmbiguity; - Pose3d estimatedPose = pair.getSecond(); - transform = transform.plus(estimatedPose.getTranslation().times(weight)); - rotation = rotation.plus(estimatedPose.getRotation().times(weight)); - } - - return Optional.of(new EstimatedRobotPose(new Pose3d(transform, rotation), - result.getTimestampSeconds(), result.getTargets(), PoseStrategy.AVERAGE_BEST_TARGETS)); - } - - /** - * Difference is defined as the vector magnitude between the two poses - * - * @return The absolute "difference" (>=0) between two Pose3ds. - */ - private double calculateDifference(Pose3d x, Pose3d y) { - return x.getTranslation().getDistance(y.getTranslation()); - } - - private void reportFiducialPoseError(int fiducialId) { - if (!reportedErrors.contains(fiducialId)) { - DriverStation.reportError( - "[PhotonPoseEstimator] Tried to get pose of unknown AprilTag: " + fiducialId, - false); - reportedErrors.add(fiducialId); - } - } -} diff --git a/src/main/java/frc/lib/util/photon/PhotonReal.java b/src/main/java/frc/lib/util/photon/PhotonReal.java deleted file mode 100755 index 994351f4..00000000 --- a/src/main/java/frc/lib/util/photon/PhotonReal.java +++ /dev/null @@ -1,204 +0,0 @@ -package frc.lib.util.photon; - -import java.io.File; -import java.io.IOException; -import java.io.InputStream; -import java.nio.charset.StandardCharsets; -import java.util.concurrent.atomic.AtomicBoolean; -import org.apache.http.HttpEntity; -import org.apache.http.client.methods.CloseableHttpResponse; -import org.apache.http.client.methods.HttpPost; -import org.apache.http.entity.mime.MultipartEntityBuilder; -import org.apache.http.entity.mime.content.FileBody; -import org.apache.http.impl.client.CloseableHttpClient; -import org.apache.http.impl.client.HttpClients; -import org.photonvision.PhotonCamera; -import org.photonvision.common.dataflow.structures.Packet; -import org.photonvision.common.dataflow.structures.PacketSerde; -import org.photonvision.common.networktables.PacketSubscriber; -import org.photonvision.targeting.PhotonPipelineResult; -import edu.wpi.first.hal.HAL; -import edu.wpi.first.networktables.DoubleArraySubscriber; -import edu.wpi.first.networktables.IntegerSubscriber; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.networktables.PubSubOption; -import edu.wpi.first.networktables.StringSubscriber; -import edu.wpi.first.wpilibj.Filesystem; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -/** Represents an actual camera that is connected to PhotonVision. Based on {@link PhotonCamera}. */ -public class PhotonReal extends PhotonIO implements AutoCloseable { - public static final String kTableName = "photonvision"; - private static int InstanceCount = 0; - - private final NetworkTable cameraTable; - private PacketSubscriber resultSubscriber; - private IntegerSubscriber heartbeatEntry; - private StringSubscriber versionEntry; - private DoubleArraySubscriber cameraIntrinsicsSubscriber; - private DoubleArraySubscriber cameraDistortionSubscriber; - - private long prevHeartbeatValue = -1; - private double prevHeartbeatChangeTime = 0; - - private static class PhotonPipelineResultIntermediate { - public PhotonPipelineResult result; - public byte[] rawBytes; - - public PhotonPipelineResultIntermediate() { - result = new PhotonPipelineResult(); - rawBytes = new byte[0]; - } - - public PhotonPipelineResultIntermediate(PhotonPipelineResult result, byte[] rawBytes) { - this.result = result; - this.rawBytes = rawBytes; - } - } - - private static class PhotonResultIntermediateBuilder - implements PacketSerde { - - @Override - public int getMaxByteSize() { - // This uses dynamic packets so it doesn't matter - return -1; - } - - @Override - public void pack(Packet packet, PhotonPipelineResultIntermediate value) { - PhotonPipelineResult.serde.pack(packet, value.result); - } - - @Override - public PhotonPipelineResultIntermediate unpack(Packet packet) { - byte[] data = packet.getData(); - var result = PhotonPipelineResult.serde.unpack(packet); - return new PhotonPipelineResultIntermediate(result, data); - } - - } - - /** - * Upload Settings - * - * @param ip Camera IP - * @param file Camera settings file - */ - public boolean uploadSettings(String ip, File file) throws IOException { - try (final CloseableHttpClient httpClient = HttpClients.createDefault()) { - HttpPost postReq = new HttpPost("http://" + ip + "/api/settings"); - HttpEntity entity = - MultipartEntityBuilder.create().addPart("data", new FileBody(file)).build(); - postReq.setEntity(entity); - try (CloseableHttpResponse response = httpClient.execute(postReq)) { - SmartDashboard.putString("uploadSettings/" + this.name + "/status", - response.getStatusLine().getStatusCode() + ": " - + response.getStatusLine().getReasonPhrase()); - var ent = response.getEntity(); - if (ent != null) { - try (InputStream stream = ent.getContent()) { - String text = new String(stream.readAllBytes(), StandardCharsets.UTF_8); - SmartDashboard.putString("uploadSettings/" + this.name + "/content", text); - } - } else { - SmartDashboard.putString("uploadSettings/" + this.name + "/content", "null"); - } - return response.getStatusLine().getStatusCode() == 200; - } - } - } - - private AtomicBoolean isPhotonOk = new AtomicBoolean(false); - - /** - * Constructs a PhotonReal from a root table. - * - * @param instance The NetworkTableInstance to pull data from. - * @param cameraName The name of the camera, as seen in the UI. - */ - public PhotonReal(NetworkTableInstance instance, String cameraName, String cameraIP) { - super(cameraName, cameraIP); - new Thread(() -> { - Timer timer = new Timer(); - while (true) { - if (timer.advanceIfElapsed(5.0) && !isPhotonOk.get()) { - try { - uploadSettings(cameraIP + ":5800", - new File(Filesystem.getDeployDirectory().getAbsoluteFile(), - "photon-configs/" + cameraName + ".zip")); - } catch (IOException e) { - e.printStackTrace(); - } - } - Thread.yield(); - } - }).start(); - var photonvision_root_table = instance.getTable(kTableName); - this.cameraTable = photonvision_root_table.getSubTable(cameraName); - var rawBytesEntry = cameraTable.getRawTopic("rawBytes").subscribe("rawBytes", new byte[] {}, - PubSubOption.periodic(0.01), PubSubOption.sendAll(true)); - resultSubscriber = new PacketSubscriber(rawBytesEntry, - new PhotonResultIntermediateBuilder(), new PhotonPipelineResultIntermediate()); - heartbeatEntry = cameraTable.getIntegerTopic("heartbeat").subscribe(-1); - cameraIntrinsicsSubscriber = - cameraTable.getDoubleArrayTopic("cameraIntrinsics").subscribe(null); - cameraDistortionSubscriber = - cameraTable.getDoubleArrayTopic("cameraDistortion").subscribe(null); - versionEntry = photonvision_root_table.getStringTopic("version").subscribe(""); - - HAL.report(edu.wpi.first.hal.FRCNetComm.tResourceType.kResourceType_PhotonCamera, - InstanceCount); - InstanceCount++; - } - - /** - * Constructs a PhotonReal from the name of the camera. - * - * @param cameraName The nickname of the camera (found in the PhotonVision UI). - */ - public PhotonReal(String cameraName, String cameraIP) { - this(NetworkTableInstance.getDefault(), cameraName, cameraIP); - } - - private static final double[] EMPTY = new double[0]; - - @Override - public void updateInputs(PhotonInputs inputs) { - super.updateInputs(inputs); - var result = resultSubscriber.get(); - result.result.setTimestampSeconds((resultSubscriber.subscriber.getLastChange() / 1e6) - - result.result.getLatencyMillis() / 1e3); - inputs.rawBytes = result.rawBytes; - inputs.result = result.result; - - String versionString = versionEntry.get(""); - inputs.versionString = versionString; - - var curHeartbeat = heartbeatEntry.get(); - var now = Timer.getFPGATimestamp(); - - if (curHeartbeat != prevHeartbeatValue) { - prevHeartbeatChangeTime = now; - prevHeartbeatValue = curHeartbeat; - } - - inputs.timeSinceLastHeartbeat = (now - prevHeartbeatChangeTime); - - inputs.cameraMatrix = cameraIntrinsicsSubscriber.get(EMPTY); - inputs.distCoeffs = cameraDistortionSubscriber.get(EMPTY); - - isPhotonOk.set(inputs.distCoeffs.length != 0); - } - - @Override - public void close() throws Exception { - resultSubscriber.close(); - heartbeatEntry.close(); - cameraIntrinsicsSubscriber.close(); - cameraDistortionSubscriber.close(); - } - -} diff --git a/src/main/java/frc/lib/util/swerve/SwerveModule.java b/src/main/java/frc/lib/util/swerve/SwerveModule.java index b3a23d7b..957d7814 100644 --- a/src/main/java/frc/lib/util/swerve/SwerveModule.java +++ b/src/main/java/frc/lib/util/swerve/SwerveModule.java @@ -1,5 +1,8 @@ package frc.lib.util.swerve; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; @@ -7,7 +10,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.lib.math.Conversions; import frc.robot.Constants; -import frc.robot.Robot; /** * Swerve Module Subsystem @@ -42,11 +44,11 @@ public SwerveModule(int moduleNumber, Rotation2d angleOffset, SwerveModuleIO io) * Update inputs for a Swerve Module. */ public void periodic() { - Robot.profiler.push("updateInputs"); + // Robot.profiler.push("updateInputs"); io.updateInputs(inputs); - Robot.profiler.swap("processInputs"); + // Robot.profiler.swap("processInputs"); Logger.processInputs("SwerveModule" + moduleNumber, inputs); - Robot.profiler.pop(); + // Robot.profiler.pop(); } /** @@ -56,7 +58,7 @@ public void periodic() { * @param isOpenLoop Whether the state should be open or closed loop controlled */ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop) { - desiredState = SwerveModuleState.optimize(desiredState, getState().angle); + desiredState.optimize(getState().angle); io.setAngleMotor(desiredState.angle.getRotations()); setSpeed(desiredState, isOpenLoop); SmartDashboard.putNumber("desired state speed/" + moduleNumber, @@ -86,7 +88,7 @@ private void setSpeed(SwerveModuleState desiredState, boolean isOpenLoop) { * @return The rotation of the CANCoder in {@link Rotation2d} */ public Rotation2d getCANcoder() { - return Rotation2d.fromRotations(inputs.absolutePositionAngleEncoder); + return Rotation2d.fromRotations(inputs.absolutePositionAngleEncoder.in(Rotations)); } /** @@ -96,9 +98,10 @@ public Rotation2d getCANcoder() { */ public SwerveModuleState getState() { return new SwerveModuleState( - Conversions.rotationPerSecondToMetersPerSecond(inputs.driveMotorSelectedSensorVelocity, - Constants.Swerve.wheelCircumference), - Rotation2d.fromRotations(inputs.angleMotorSelectedPosition)); + Conversions.rotationPerSecondToMetersPerSecond( + inputs.driveMotorSelectedSensorVelocity.in(RotationsPerSecond), + Constants.Swerve.wheelCircumference.in(Meters)), + Rotation2d.fromRotations(inputs.angleMotorSelectedPosition.in(Rotations))); } /** @@ -108,9 +111,9 @@ public SwerveModuleState getState() { */ public SwerveModulePosition getPosition() { return new SwerveModulePosition( - Conversions.rotationsToMeters(inputs.driveMotorSelectedPosition, - Constants.Swerve.wheelCircumference), - Rotation2d.fromRotations(inputs.angleMotorSelectedPosition)); - + Conversions.rotationsToMeters( + inputs.driveMotorSelectedSensorVelocity.in(RotationsPerSecond), + Constants.Swerve.wheelCircumference.in(Meters)), + Rotation2d.fromRotations(inputs.angleMotorSelectedPosition.in(Rotations))); } } diff --git a/src/main/java/frc/lib/util/swerve/SwerveModuleIO.java b/src/main/java/frc/lib/util/swerve/SwerveModuleIO.java index 011a051c..54f9d075 100644 --- a/src/main/java/frc/lib/util/swerve/SwerveModuleIO.java +++ b/src/main/java/frc/lib/util/swerve/SwerveModuleIO.java @@ -2,15 +2,18 @@ import org.littletonrobotics.junction.AutoLog; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; + /** IO Class for SwerveModule */ public interface SwerveModuleIO { /** Inputs Class for SwerveModule */ @AutoLog public static class SwerveModuleInputs { - public double driveMotorSelectedPosition; - public double driveMotorSelectedSensorVelocity; - public double angleMotorSelectedPosition; - public double absolutePositionAngleEncoder; + public Angle driveMotorSelectedPosition; + public AngularVelocity driveMotorSelectedSensorVelocity; + public Angle angleMotorSelectedPosition; + public Angle absolutePositionAngleEncoder; public double[] odometryTimestamps; // public double driveMotorTemp; // public double angleMotorTemp; diff --git a/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java b/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java index 05a5dc29..ce0092ca 100644 --- a/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java +++ b/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java @@ -1,5 +1,6 @@ package frc.lib.util.swerve; +import static edu.wpi.first.units.Units.Meter; import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CANcoderConfiguration; @@ -9,9 +10,11 @@ import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue; +// import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue; import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; import frc.lib.math.Conversions; import frc.robot.Constants; @@ -27,10 +30,10 @@ public class SwerveModuleReal implements SwerveModuleIO { private TalonFXConfiguration swerveDriveFXConfig = new TalonFXConfiguration(); private CANcoderConfiguration swerveCANcoderConfig = new CANcoderConfiguration(); - private StatusSignal driveMotorSelectedPosition; - private StatusSignal driveMotorSelectedSensorVelocity; - private StatusSignal angleMotorSelectedPosition; - private StatusSignal absolutePositionAngleEncoder; + private StatusSignal driveMotorSelectedPosition; + private StatusSignal driveMotorSelectedSensorVelocity; + private StatusSignal angleMotorSelectedPosition; + private StatusSignal absolutePositionAngleEncoder; /* drive motor control requests */ private final DutyCycleOut driveDutyCycle = new DutyCycleOut(0); @@ -77,9 +80,9 @@ private void configAngleMotor() { swerveAngleFXConfig.CurrentLimits.SupplyCurrentLimitEnable = Constants.Swerve.angleEnableCurrentLimit; swerveAngleFXConfig.CurrentLimits.SupplyCurrentLimit = Constants.Swerve.angleCurrentLimit; - swerveAngleFXConfig.CurrentLimits.SupplyCurrentThreshold = + swerveAngleFXConfig.CurrentLimits.SupplyCurrentLowerLimit = Constants.Swerve.angleCurrentThreshold; - swerveAngleFXConfig.CurrentLimits.SupplyTimeThreshold = + swerveAngleFXConfig.CurrentLimits.SupplyCurrentLowerTime = Constants.Swerve.angleCurrentThresholdTime; /* PID Config */ @@ -103,9 +106,9 @@ private void configDriveMotor() { swerveDriveFXConfig.CurrentLimits.SupplyCurrentLimitEnable = Constants.Swerve.driveEnableCurrentLimit; swerveDriveFXConfig.CurrentLimits.SupplyCurrentLimit = Constants.Swerve.driveCurrentLimit; - swerveDriveFXConfig.CurrentLimits.SupplyCurrentThreshold = + swerveDriveFXConfig.CurrentLimits.SupplyCurrentLowerLimit = Constants.Swerve.driveCurrentThreshold; - swerveDriveFXConfig.CurrentLimits.SupplyTimeThreshold = + swerveDriveFXConfig.CurrentLimits.SupplyCurrentLowerTime = Constants.Swerve.driveCurrentThresholdTime; /* PID Config */ @@ -133,8 +136,7 @@ private void configDriveMotor() { private void configAngleEncoder() { /* Angle Encoder Config */ swerveCANcoderConfig.MagnetSensor.SensorDirection = Constants.Swerve.cancoderInvert; - swerveCANcoderConfig.MagnetSensor.AbsoluteSensorRange = - AbsoluteSensorRangeValue.Signed_PlusMinusHalf; + swerveCANcoderConfig.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 0.5; swerveCANcoderConfig.MagnetSensor.MagnetOffset = -angleOffset.getRotations(); angleEncoder.getConfigurator().apply(swerveCANcoderConfig); @@ -149,7 +151,7 @@ public void setAngleMotor(double angle) { public void setDriveMotor(double mps) { // driveVelocity.FeedForward = feedforward; double driveRPS = Conversions.metersPerSecondToRotationPerSecond(mps, - Constants.Swerve.wheelCircumference); + Constants.Swerve.wheelCircumference.in(Meter)); driveVelocity.Velocity = driveRPS; mDriveMotor.setControl(driveVelocity); } diff --git a/src/main/java/frc/lib/util/swerve/SwerveModuleSim.java b/src/main/java/frc/lib/util/swerve/SwerveModuleSim.java index 7cc8166b..25e45fd8 100644 --- a/src/main/java/frc/lib/util/swerve/SwerveModuleSim.java +++ b/src/main/java/frc/lib/util/swerve/SwerveModuleSim.java @@ -1,11 +1,19 @@ package frc.lib.util.swerve; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.Seconds; import org.littletonrobotics.junction.LoggedRobot; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.simulation.FlywheelSim; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -19,10 +27,11 @@ public class SwerveModuleSim implements SwerveModuleIO { public int moduleNumber; private FlywheelSim driveSim = - new FlywheelSim(DCMotor.getFalcon500(1), Constants.Swerve.driveGearRatio, 0.025); + new FlywheelSim(LinearSystemId.createFlywheelSystem(DCMotor.getFalcon500(1), + Constants.Swerve.driveGearRatio, 0.025), DCMotor.getFalcon500(1)); - private double angle; - private double distance; + private Angle angle = Rotations.of(0); + private Angle distance = Rotations.of(0); private double driveAppliedVolts = 0.0; private SimpleMotorFeedforward driveFeedforward = new SimpleMotorFeedforward(0.0, 0.13); @@ -41,11 +50,12 @@ public void setModNumber(int number) { @Override public void updateInputs(SwerveModuleInputs inputs) { driveSim.update(LoggedRobot.defaultPeriodSecs); - double driveSpeed = Units.radiansToRotations(driveSim.getAngularVelocityRadPerSec()); - this.distance += driveSpeed * LoggedRobot.defaultPeriodSecs; + AngularVelocity driveSpeed = + RadiansPerSecond.of(driveSim.getAngularAccelerationRadPerSecSq()); + RotationsPerSecond.of(Units.radiansToRotations(driveSim.getAngularVelocityRadPerSec())); + this.distance = driveSpeed.times(Seconds.of(LoggedRobot.defaultPeriodSecs)).plus(distance); inputs.driveMotorSelectedPosition = this.distance; inputs.driveMotorSelectedSensorVelocity = driveSpeed; - inputs.angleMotorSelectedPosition = angle; inputs.absolutePositionAngleEncoder = angle; @@ -59,7 +69,7 @@ public void updateInputs(SwerveModuleInputs inputs) { */ public void setDriveMotor(double mps) { double rpm = Conversions.metersPerSecondToRotationPerSecond(mps, - Constants.Swerve.wheelCircumference); + Constants.Swerve.wheelCircumference.in(Meters)); driveFeedback.setSetpoint(rpm); double driveFF = driveFeedforward.calculate(mps); SmartDashboard.putNumber("ff/" + moduleNumber, driveFF); @@ -76,7 +86,7 @@ public void setDriveMotor(double mps) { * * @param angle Angle to set */ - public void setAngleMotor(double angle) { + public void setAngleMotor(Angle angle) { this.angle = angle; } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b00d16d6..f6069d04 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -1,11 +1,10 @@ package frc.robot; +import static edu.wpi.first.units.Units.Inches; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.SensorDirectionValue; -import com.pathplanner.lib.util.HolonomicPathFollowerConfig; -import com.pathplanner.lib.util.PIDConstants; -import com.pathplanner.lib.util.ReplanningConfig; +import com.studica.frc.AHRS.NavXComType; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; @@ -14,6 +13,7 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Distance; import edu.wpi.first.wpilibj.util.Color; import frc.lib.util.FieldConstants; @@ -186,8 +186,7 @@ public static final class Swerve { public static final double AUTO_ROTATION_KI = 0.0; public static final double AUTO_ROTATION_KD = 0.0; - public static final edu.wpi.first.wpilibj.SPI.Port navXID = - edu.wpi.first.wpilibj.SPI.Port.kMXP; + public static final NavXComType navXID = NavXComType.kMXP_SPI; public static final boolean invertGyro = true; public static final boolean isFieldRelative = true; public static final boolean isOpenLoop = false; @@ -195,8 +194,8 @@ public static final class Swerve { /* Drivetrain Constants */ public static final double trackWidth = Units.inchesToMeters(23.75); public static final double wheelBase = Units.inchesToMeters(17.75); - public static final double wheelDiameter = Units.inchesToMeters(3.8); - public static final double wheelCircumference = wheelDiameter * Math.PI; + public static final Distance wheelDiameter = Inches.of(3.8); + public static final Distance wheelCircumference = wheelDiameter.times(Math.PI); public static final Translation2d MOD0_MODOFFSET = new Translation2d(wheelBase / 2.0, trackWidth / 2.0); @@ -259,10 +258,10 @@ public static final class Swerve { /* Swerve Profiling Values */ /** Meters per Second */ - public static final double maxSpeed = 10.0; + public static final double maxSpeed = 3.0; public static final double AUTO_MAX_SPEED = 3.0; /** Radians per Second */ - public static final double maxAngularVelocity = 15.0; + public static final double maxAngularVelocity = 4.0; /* Neutral Modes */ public static final NeutralModeValue angleNeutralMode = NeutralModeValue.Coast; @@ -317,11 +316,12 @@ public static final class Mod3 { public static final Rotation2d angleOffset = Rotation2d.fromRotations(0.317627 + 0.5); } - public static final HolonomicPathFollowerConfig pathFollowerConfig = - new HolonomicPathFollowerConfig(new PIDConstants(5.0, 0, 0), - new PIDConstants(AUTO_ROTATION_KP, AUTO_ROTATION_KI, AUTO_ROTATION_KD), - // Drive base radius (distance from center to furthest module) - AUTO_MAX_SPEED, MOD0_MODOFFSET.getNorm(), new ReplanningConfig()); + + // public static final HolonomicPathFollowerConfig pathFollowerConfig = + // new HolonomicPathFollowerConfig(new PIDConstants(5.0, 0, 0), + // new PIDConstants(AUTO_ROTATION_KP, AUTO_ROTATION_KI, AUTO_ROTATION_KD), + // // Drive base radius (distance from center to furthest module) + // AUTO_MAX_SPEED, MOD0_MODOFFSET.getNorm(), new ReplanningConfig()); } /** @@ -485,7 +485,7 @@ public static final class ShooterConstants { public static final double KS = 0; public static final double TOP_KV = 6.18e-4; public static final double BOTTOM_KV = 6.18e-4; - public static final double GEAR_RATIO = 3; + public static final double GEAR_RATIO = 3; // gear ratio public static final double HEIGHT_FROM_LOWEST_POS = Units.inchesToMeters(32.0); public static final double HEIGHT_FROM_SPEAKER = FieldConstants.centerSpeaker - HEIGHT_FROM_LOWEST_POS; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 3ec1211f..b0da4f63 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -20,9 +20,6 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import frc.lib.profiling.EmptyProfiler; -import frc.lib.profiling.LoggingProfiler; -import frc.lib.profiling.Profiler; /** * Runs tasks on Roborio in this file. @@ -31,7 +28,7 @@ public class Robot extends LoggedRobot { private RobotContainer robotContainer; private Command autoChooser; - public static Profiler profiler; + // public static Profiler profiler; /** * Robot Run type @@ -101,15 +98,15 @@ public Robot() { } } Logger.start(); // Start logging! No more data receivers, replay sources, or metadata values - switch (robotRunType) { - case kReal -> profiler = - new LoggingProfiler(() -> Logger.getRealTimestamp(), 1000000.0); - case kReplay -> profiler = EmptyProfiler.INSTANCE; - case kSimulation -> profiler = - new LoggingProfiler(() -> Logger.getRealTimestamp(), 1000000.0); - default -> { - } - } + // switch (robotRunType) { + // // case kReal -> profiler = + // new LoggingProfiler(() -> Logger.getRealTimestamp(), 1000000.0); + // case kReplay -> profiler = EmptyProfiler.INSTANCE; + // case kSimulation -> profiler = + // new LoggingProfiler(() -> Logger.getRealTimestamp(), 1000000.0); + // default -> { + // } + // } // Logger.disableDeterministicTimestamps() // See "Deterministic Timestamps" in the // "Understanding Data Flow" page @@ -130,19 +127,19 @@ public Robot() { @Override public void robotPeriodic() { if (hasStarted) { - profiler.endTick(); + // profiler.endTick(); if (profileTimer.advanceIfElapsed(1)) { if (hasDoneSomething) { - profiler.save(); - profiler.reset(); + // profiler.save(); + // profiler.reset(); } } } else { hasStarted = true; } - profiler.startTick(); - profiler.push("robotPeriodic()"); - profiler.push("draw_state_to_shuffleboard"); + // profiler.startTick(); + // profiler.push("robotPeriodic()"); + // profiler.push("draw_state_to_shuffleboard"); robotContainer.operatorState.setString(OperatorState.getCurrentState().displayName); robotContainer.operatorManualMode.setBoolean(OperatorState.manualModeEnabled()); robotContainer.matchTime.setDouble(Timer.getMatchTime()); @@ -154,16 +151,16 @@ public void robotPeriodic() { // subsystem periodic() methods. This must be called from the robot's periodic block in // order for // anything in the Command-based framework to work. - profiler.swap("command_scheduler"); + // profiler.swap("command_scheduler"); CommandScheduler.getInstance().run(); - profiler.swap("manual-gc"); + // profiler.swap("manual-gc"); if (gcTimer.advanceIfElapsed(5)) { System.gc(); } - profiler.swap("viz"); + // profiler.swap("viz"); robotContainer.updateViz(); - profiler.pop(); - profiler.pop(); + // profiler.pop(); + // profiler.pop(); } @Override @@ -178,7 +175,7 @@ public void disabledPeriodic() {} @Override public void autonomousInit() { hasDoneSomething = true; - profiler.push("autonomousInit()"); + // profiler.push("autonomousInit()"); inAuto = true; OperatorState.disableManualMode(); @@ -189,7 +186,7 @@ public void autonomousInit() { if (autoChooser != null) { autoChooser.schedule(); } - profiler.pop(); + // profiler.pop(); } /** This function is called periodically during autonomous. */ @@ -199,12 +196,12 @@ public void autonomousPeriodic() {} @Override public void teleopInit() { hasDoneSomething = true; - profiler.push("teleopInit()"); + // profiler.push("teleopInit()"); inAuto = false; if (autoChooser != null) { autoChooser.cancel(); } - profiler.pop(); + // profiler.pop(); } /** This function is called periodically during operator control. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ce70e993..603f7502 100755 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -28,14 +28,14 @@ import frc.lib.sim.SimulatedArena; import frc.lib.sim.SimulatedPumbaa; import frc.lib.util.FieldConstants; -import frc.lib.util.photon.PhotonCameraWrapper; -import frc.lib.util.photon.PhotonReal; +// import frc.lib.util.photon.PhotonCameraWrapper; +// import frc.lib.util.photon.PhotonReal; import frc.lib.viz.PumbaaViz; import frc.robot.Robot.RobotRunType; -import frc.robot.autos.JustShoot1; -import frc.robot.autos.P123; -import frc.robot.autos.P321; -import frc.robot.autos.P8765; +// import frc.robot.autos.JustShoot1; +// import frc.robot.autos.P123; +// import frc.robot.autos.P321; +// import frc.robot.autos.P8765; import frc.robot.commands.CommandFactory; import frc.robot.commands.FlashingLEDColor; import frc.robot.commands.MovingColorLEDs; @@ -130,7 +130,7 @@ public class RobotContainer { private Swerve s_Swerve; private Shooter shooter; private Intake intake; - private PhotonCameraWrapper[] cameras; + // private PhotonCameraWrapper[] cameras; private ElevatorWrist elevatorWrist; private LEDs leds = new LEDs(Constants.LEDConstants.LED_COUNT, Constants.LEDConstants.PWM_PORT); // private PhotonCamera backLeftCamera = new PhotonCamera("back-left"); @@ -160,44 +160,44 @@ public RobotContainer(RobotRunType runtimeType) { numNoteChooser.addOption(String.valueOf(i), i); } /* - * Camera Order: 0 - Front Left 1 - Front RIght 2 - Back Left 3 - Back Right + * Camera Order: 0 - Front Left 1 - Front RIght 2 - Back Left 3 - Back Right // */ - cameras = new PhotonCameraWrapper[] {new PhotonCameraWrapper( - new PhotonReal(Constants.CameraConstants.FrontRightFacingCamera.CAMERA_NAME, - Constants.CameraConstants.FrontRightFacingCamera.CAMERA_IP), - Constants.CameraConstants.FrontRightFacingCamera.KCAMERA_TO_ROBOT)}; + // cameras = new PhotonCameraWrapper[] {new PhotonCameraWrapper( + // new PhotonReal(Constants.CameraConstants.FrontRightFacingCamera.CAMERA_NAME, + // Constants.CameraConstants.FrontRightFacingCamera.CAMERA_IP), + // Constants.CameraConstants.FrontRightFacingCamera.KCAMERA_TO_ROBOT)}; switch (runtimeType) { case kReal: viz = new PumbaaViz("Viz", null); shooter = new Shooter(new ShooterVortex()); intake = new Intake(new IntakeIOFalcon(), viz); - s_Swerve = new Swerve(new SwerveReal(), cameras, viz); + s_Swerve = new Swerve(new SwerveReal(), viz); elevatorWrist = new ElevatorWrist(new ElevatorWristReal(), operator, viz); break; case kSimulation: SimulatedPumbaa pumbaa = arena.newPumbaa(); viz = new PumbaaViz("Viz", pumbaa); - s_Swerve = new Swerve(new SwerveSim(pumbaa), cameras, viz); + s_Swerve = new Swerve(new SwerveSim(pumbaa), viz); shooter = new Shooter(new ShooterSim(pumbaa)); intake = new Intake(new IntakeIOSim(pumbaa), viz); elevatorWrist = new ElevatorWrist(new ElevatorWristIOSim(pumbaa), operator, viz); break; default: viz = new PumbaaViz("Viz", null); - s_Swerve = new Swerve(new SwerveIO() {}, cameras, viz); + s_Swerve = new Swerve(new SwerveIO() {}, viz); shooter = new Shooter(new ShooterIO() {}); intake = new Intake(new IntakeIO() {}, viz); elevatorWrist = new ElevatorWrist(new ElevatorWristIO() {}, operator, viz); } - autoChooser.setDefaultOption("Nothing", Commands.none()); - autoChooser.addOption("P123", new P123(s_Swerve, elevatorWrist, intake, shooter)); - autoChooser.addOption("P321", new P321(s_Swerve, elevatorWrist, intake, shooter)); - autoChooser.addOption("P8765", new P8765(s_Swerve, elevatorWrist, intake, shooter)); - autoChooser.addOption("Just Shoot 1", - new JustShoot1(s_Swerve, elevatorWrist, intake, shooter)); - // autoChooser.addOption("P32", new P32(s_Swerve, elevatorWrist, intake, shooter)); + // ?autoChooser.setDefaultOption("Nothing", Commands.none()); + // autoChooser.addOption("P123", new P123(s_Swerve, elevatorWrist, intake, shooter)); + // autoChooser.addOption("P321", new P321(s_Swerve, elevatorWrist, intake, shooter)); + // autoChooser.addOption("P8765", new P8765(s_Swerve, elevatorWrist, intake, shooter)); + // autoChooser.addOption("Just Shoot 1", + // new JustShoot1(s_Swerve, elevatorWrist, intake, shooter)); + // // autoChooser.addOption("P32", new P32(s_Swerve, elevatorWrist, intake, shooter)); // autoChooser.addOption("P675", new P675(s_Swerve, elevatorWrist, intake, shooter)); // autoChooser.addOption("P3675", new P3675(s_Swerve, elevatorWrist, intake, shooter)); @@ -225,19 +225,22 @@ private void configureButtonBindings() { /* Driver Buttons */ driver.y().onTrue(new InstantCommand(() -> s_Swerve.resetFieldRelativeOffset())); - driver.start().onTrue( - new InstantCommand(() -> s_Swerve.resetPvInitialization()).ignoringDisable(true)); + // driver.start().onTrue( + // new InstantCommand(() -> s_Swerve.resetPvInitialization()).ignoringDisable(true)); // intake forward driver.rightTrigger().whileTrue(CommandFactory.newIntakeCommand(intake, elevatorWrist)); // intake backward driver.leftTrigger().and(() -> elevatorWrist.getWristAngle().getDegrees() <= 24.0) .whileTrue(intake.runIntakeMotorNonStop(-1, -.20)); + driver.x().whileTrue(CommandFactory.spit(shooter, intake)); + + /* Operator Buttons */ // spit note currently in robot through shooter operator.x().whileTrue(CommandFactory.spit(shooter, intake)); // reset apriltag vision - operator.b().onTrue(new InstantCommand(() -> s_Swerve.resetPvInitialization())); + // operator.b().onTrue(new InstantCommand(() -> s_Swerve.resetPvInitialization())); // spin up shooter operator.leftTrigger().whileTrue(shooter.shootSpeaker()); // operator.leftTrigger() diff --git a/src/main/java/frc/robot/autos/JustShoot1.java b/src/main/java/frc/robot/autos/JustShoot1.txt similarity index 100% rename from src/main/java/frc/robot/autos/JustShoot1.java rename to src/main/java/frc/robot/autos/JustShoot1.txt diff --git a/src/main/java/frc/robot/autos/P123.java b/src/main/java/frc/robot/autos/P123.txt similarity index 100% rename from src/main/java/frc/robot/autos/P123.java rename to src/main/java/frc/robot/autos/P123.txt diff --git a/src/main/java/frc/robot/autos/P32.java b/src/main/java/frc/robot/autos/P32.txt similarity index 100% rename from src/main/java/frc/robot/autos/P32.java rename to src/main/java/frc/robot/autos/P32.txt diff --git a/src/main/java/frc/robot/autos/P321.java b/src/main/java/frc/robot/autos/P321.txt similarity index 100% rename from src/main/java/frc/robot/autos/P321.java rename to src/main/java/frc/robot/autos/P321.txt diff --git a/src/main/java/frc/robot/autos/P3675.java b/src/main/java/frc/robot/autos/P3675.txt similarity index 100% rename from src/main/java/frc/robot/autos/P3675.java rename to src/main/java/frc/robot/autos/P3675.txt diff --git a/src/main/java/frc/robot/autos/P675.java b/src/main/java/frc/robot/autos/P675.txt similarity index 100% rename from src/main/java/frc/robot/autos/P675.java rename to src/main/java/frc/robot/autos/P675.txt diff --git a/src/main/java/frc/robot/autos/P8765.java b/src/main/java/frc/robot/autos/P8765.txt similarity index 100% rename from src/main/java/frc/robot/autos/P8765.java rename to src/main/java/frc/robot/autos/P8765.txt diff --git a/src/main/java/frc/robot/autos/Resnick3.java b/src/main/java/frc/robot/autos/Resnick3.txt similarity index 100% rename from src/main/java/frc/robot/autos/Resnick3.java rename to src/main/java/frc/robot/autos/Resnick3.txt diff --git a/src/main/java/frc/robot/autos/Resnick4.java b/src/main/java/frc/robot/autos/Resnick4.txt similarity index 100% rename from src/main/java/frc/robot/autos/Resnick4.java rename to src/main/java/frc/robot/autos/Resnick4.txt diff --git a/src/main/java/frc/robot/autos/Resnick5.java b/src/main/java/frc/robot/autos/Resnick5.txt similarity index 100% rename from src/main/java/frc/robot/autos/Resnick5.java rename to src/main/java/frc/robot/autos/Resnick5.txt diff --git a/src/main/java/frc/robot/commands/TeleopSwerve.java b/src/main/java/frc/robot/commands/TeleopSwerve.java index dad2a806..24b27228 100644 --- a/src/main/java/frc/robot/commands/TeleopSwerve.java +++ b/src/main/java/frc/robot/commands/TeleopSwerve.java @@ -4,7 +4,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants; -import frc.robot.Robot; import frc.robot.subsystems.swerve.Swerve; /** @@ -50,7 +49,7 @@ public TeleopSwerve(Swerve swerveDrive, CommandXboxController controller, boolea @Override public void execute() { - Robot.profiler.push("teleop_swerve"); + // Robot.profiler.push("teleop_swerve"); double yaxis = -controller.getLeftY() * speedMultiplier; double xaxis = -controller.getLeftX() * speedMultiplier; double raxis = -controller.getRightX() * speedMultiplier; @@ -69,6 +68,6 @@ public void execute() { new Translation2d(yaxis, xaxis).times(Constants.Swerve.maxSpeed); double rotation = raxis * Constants.Swerve.maxAngularVelocity; swerveDrive.drive(translation, rotation, fieldRelative, openLoop); - Robot.profiler.pop(); + // Robot.profiler.pop(); } } diff --git a/src/main/java/frc/robot/subsystems/elevator_wrist/ElevatorWrist.java b/src/main/java/frc/robot/subsystems/elevator_wrist/ElevatorWrist.java index 3ee5e47f..83502737 100644 --- a/src/main/java/frc/robot/subsystems/elevator_wrist/ElevatorWrist.java +++ b/src/main/java/frc/robot/subsystems/elevator_wrist/ElevatorWrist.java @@ -25,7 +25,6 @@ import frc.lib.viz.PumbaaViz; import frc.robot.Constants; import frc.robot.OperatorState; -import frc.robot.Robot; import frc.robot.RobotContainer; /** @@ -109,12 +108,12 @@ public ElevatorWrist(ElevatorWristIO io, CommandXboxController operator, PumbaaV @Override public void periodic() { - Robot.profiler.push("ElevatorWrist periodic"); - Robot.profiler.push("updateInputs"); + // Robot.profiler.push("ElevatorWrist periodic"); + // Robot.profiler.push("updateInputs"); io.updateInputs(inputs); - Robot.profiler.swap("processInputs"); + // Robot.profiler.swap("processInputs"); Logger.processInputs("ElevatorWrist", inputs); - Robot.profiler.swap("PID stuff"); + // Robot.profiler.swap("PID stuff"); if (inputs.wristAbsoluteEncRawValue > 0.9) { inputs.wristAbsoluteEncRawValue -= 1.0; } @@ -193,7 +192,7 @@ && getHeight() > Constants.ElevatorWristConstants.SetPoints.HOME_HEIGHT)) { io.setWristVoltage(0); } - Robot.profiler.swap("Publish to SmartDashboard"); + // Robot.profiler.swap("Publish to SmartDashboard"); SmartDashboard.putNumber("wristError", Rotation2d.fromRotations(wristPIDController.getPositionError()).getDegrees()); @@ -225,10 +224,10 @@ && getHeight() > Constants.ElevatorWristConstants.SetPoints.HOME_HEIGHT)) { // Logger.recordOutput("/ElevatorWrist/Wrist/Combined Voltage", // wristFeedForwardValue + wristPIDValue); Logger.recordOutput("/ElevatorWrist/Wrist/Combined Voltage", wristPIDValue); - Robot.profiler.swap("viz"); + // Robot.profiler.swap("viz"); viz.setElevatorWrist(calculatedHeight, calculatedWristAngle); - Robot.profiler.pop(); - Robot.profiler.pop(); + // Robot.profiler.pop(); + // Robot.profiler.pop(); } /** diff --git a/src/main/java/frc/robot/subsystems/elevator_wrist/ElevatorWristReal.java b/src/main/java/frc/robot/subsystems/elevator_wrist/ElevatorWristReal.java index 059099ce..658c6999 100644 --- a/src/main/java/frc/robot/subsystems/elevator_wrist/ElevatorWristReal.java +++ b/src/main/java/frc/robot/subsystems/elevator_wrist/ElevatorWristReal.java @@ -2,50 +2,56 @@ import com.revrobotics.AbsoluteEncoder; -import com.revrobotics.CANSparkBase.IdleMode; -import com.revrobotics.CANSparkLowLevel.MotorType; -import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; -import com.revrobotics.SparkAbsoluteEncoder.Type; -import com.revrobotics.SparkRelativeEncoder; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkMaxConfig; import frc.robot.Constants; /** * Elevator and wrist real robot class */ public class ElevatorWristReal implements ElevatorWristIO { - public final CANSparkMax leftElevatorMotor = - new CANSparkMax(Constants.Motors.ElevatorWrist.ELEVATOR_LEFT_NEO_ID, MotorType.kBrushless); - public final CANSparkMax rightElevatorMotor = - new CANSparkMax(Constants.Motors.ElevatorWrist.ELEVATOR_RIGHT_NEO_ID, MotorType.kBrushless); - public final CANSparkMax wristMotor = - new CANSparkMax(Constants.Motors.ElevatorWrist.WRIST_NEO_ID, MotorType.kBrushless); + public final SparkMax leftElevatorMotor = + new SparkMax(Constants.Motors.ElevatorWrist.ELEVATOR_LEFT_NEO_ID, MotorType.kBrushless); + public final SparkMax rightElevatorMotor = + new SparkMax(Constants.Motors.ElevatorWrist.ELEVATOR_RIGHT_NEO_ID, MotorType.kBrushless); + public final SparkMax wristMotor = + new SparkMax(Constants.Motors.ElevatorWrist.WRIST_NEO_ID, MotorType.kBrushless); + + public final AbsoluteEncoder wristAbsoluteEnc = wristMotor.getAbsoluteEncoder(); + public final RelativeEncoder leftElevatorRelativeEnc = leftElevatorMotor.getEncoder(); + public final RelativeEncoder rightElevatorRelativeEnc = rightElevatorMotor.getEncoder(); + public final SparkMaxConfig leftELIConfig = new SparkMaxConfig(); + public final SparkMaxConfig rightELIConfig = new SparkMaxConfig(); + public final SparkMaxConfig wristConfig = new SparkMaxConfig(); - public final AbsoluteEncoder wristAbsoluteEnc = wristMotor.getAbsoluteEncoder(Type.kDutyCycle); - public final RelativeEncoder leftElevatorRelativeEnc = - leftElevatorMotor.getEncoder(SparkRelativeEncoder.Type.kHallSensor, 42); - public final RelativeEncoder rightElevatorRelativeEnc = - rightElevatorMotor.getEncoder(SparkRelativeEncoder.Type.kHallSensor, 42); /** * Constructor for elevator wrist real class */ public ElevatorWristReal() { - rightElevatorMotor.restoreFactoryDefaults(); - rightElevatorRelativeEnc.setPositionConversionFactor(60); - rightElevatorMotor.setIdleMode(IdleMode.kBrake); - rightElevatorMotor.setInverted(true); - leftElevatorMotor.restoreFactoryDefaults(); - leftElevatorRelativeEnc.setPositionConversionFactor(60); - leftElevatorMotor.setIdleMode(IdleMode.kBrake); - leftElevatorMotor.setInverted(false); - wristAbsoluteEnc.setPositionConversionFactor(1); - wristMotor.setIdleMode(IdleMode.kBrake); - wristMotor.setInverted(false); + // left elevator motor config + leftELIConfig.inverted(true).idleMode(IdleMode.kBrake); + leftELIConfig.encoder.positionConversionFactor(60); + leftElevatorMotor.configure(leftELIConfig, ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters); + + // right elevator motor config + rightELIConfig.inverted(false).idleMode(IdleMode.kBrake); + rightELIConfig.encoder.positionConversionFactor(60); + rightElevatorMotor.configure(rightELIConfig, ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters); - leftElevatorMotor.burnFlash(); - rightElevatorMotor.burnFlash(); + // wrist motor config + wristConfig.inverted(false).idleMode(IdleMode.kBrake); + wristConfig.encoder.positionConversionFactor(1); + wristMotor.configure(wristConfig, ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters); } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOFalcon.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOFalcon.java index 9bb4f827..db9b7462 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOFalcon.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOFalcon.java @@ -5,11 +5,13 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; -import com.revrobotics.CANSparkBase.IdleMode; -import com.revrobotics.CANSparkLowLevel.MotorType; -import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; -import com.revrobotics.SparkRelativeEncoder; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkMaxConfig; import edu.wpi.first.wpilibj.DigitalInput; import frc.robot.Constants; @@ -18,12 +20,11 @@ */ public class IntakeIOFalcon implements IntakeIO { - private final CANSparkMax intakeMotorLeft = - new CANSparkMax(Constants.Motors.Intake.INTAKE_MOTOR_ID_LEFT, MotorType.kBrushless); - private final CANSparkMax intakeMotorRight = - new CANSparkMax(Constants.Motors.Intake.INTAKE_MOTOR_ID_RIGHT, MotorType.kBrushless); - public final RelativeEncoder intakeRelativeEnc = - intakeMotorLeft.getEncoder(SparkRelativeEncoder.Type.kHallSensor, 42); + private final SparkMax intakeMotorLeft = + new SparkMax(Constants.Motors.Intake.INTAKE_MOTOR_ID_LEFT, MotorType.kBrushless); + private final SparkMax intakeMotorRight = + new SparkMax(Constants.Motors.Intake.INTAKE_MOTOR_ID_RIGHT, MotorType.kBrushless); + public final RelativeEncoder intakeRelativeEnc = intakeMotorLeft.getEncoder(); private final TalonFX indexerMotor = new TalonFX(Constants.Motors.Intake.INDEXER_MOTOR_ID); private final DutyCycleOut indexerDutyCycleOut = new DutyCycleOut(0); @@ -32,22 +33,26 @@ public class IntakeIOFalcon implements IntakeIO { new DigitalInput(Constants.IntakeConstants.INDEXER_BEAM_BRAKE_DIO_PORT); private final DigitalInput intakeBeamBrake = new DigitalInput(Constants.IntakeConstants.INTAKE_BEAM_BRAKE_DIO_PORT); + SparkMaxConfig leftConfig = new SparkMaxConfig(); + SparkMaxConfig rightConfig = new SparkMaxConfig(); /** * Intake IO Layer with real motors and sensors */ public IntakeIOFalcon() { - intakeMotorLeft.restoreFactoryDefaults(); - intakeMotorRight.restoreFactoryDefaults(); - intakeMotorLeft.setInverted(Constants.IntakeConstants.INTAKE_MOTOR_INVERTED); - intakeMotorRight.setInverted(false); - intakeMotorLeft.setIdleMode(IdleMode.kCoast); - intakeMotorRight.setIdleMode(IdleMode.kCoast); - intakeMotorLeft.setSmartCurrentLimit(40); - intakeMotorRight.setSmartCurrentLimit(40); + // SparkMaxConfig config = new SparkMaxConfig(); + // config.signals.primaryEncoderPositionPeriodMs(5); + leftConfig.inverted(Constants.IntakeConstants.INTAKE_MOTOR_INVERTED) + .idleMode(IdleMode.kCoast).smartCurrentLimit(40).voltageCompensation(12); + rightConfig.inverted(false).idleMode(IdleMode.kCoast).smartCurrentLimit(40) + .voltageCompensation(12); indexerConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; indexerConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; indexerMotor.getConfigurator().apply(indexerConfig); + intakeMotorLeft.configure(leftConfig, ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters); + intakeMotorRight.configure(rightConfig, ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters); } @Override @@ -55,8 +60,10 @@ public void updateInputs(IntakeInputs inputs) { // inputs.intakeSupplyVoltage = intakeMotorLeft.getBusVoltage(); // inputs.intakeAmps = intakeMotorLeft.getOutputCurrent(); // inputs.intakeRPM = intakeRelativeEnc.getVelocity(); - // inputs.indexerSupplyVoltage = indexerMotor.getSupplyVoltage().getValueAsDouble(); - // inputs.indexerMotorVoltage = indexerMotor.getMotorVoltage().getValueAsDouble(); + // inputs.indexerSupplyVoltage = + // indexerMotor.getSupplyVoltage().getValueAsDouble(); + // inputs.indexerMotorVoltage = + // indexerMotor.getMotorVoltage().getValueAsDouble(); // inputs.indexerAmps = indexerMotor.getSupplyCurrent().getValueAsDouble(); // inputs.indexerRPM = indexerMotor.getVelocity().getValueAsDouble(); inputs.indexerBeamBrake = !indexerBeamBrake.get(); // true == game piece diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java index 36035001..0b69c715 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java @@ -3,6 +3,7 @@ import org.littletonrobotics.junction.LoggedRobot; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.wpilibj.simulation.FlywheelSim; import frc.lib.sim.SimulatedPumbaa; @@ -11,8 +12,11 @@ */ public class IntakeIOSim implements IntakeIO { - private FlywheelSim intakeSim = new FlywheelSim(DCMotor.getNEO(2), 1, 0.025); - private FlywheelSim indexerSim = new FlywheelSim(DCMotor.getFalcon500(1), 1, 0.025); + private FlywheelSim intakeSim = new FlywheelSim( + LinearSystemId.createFlywheelSystem(DCMotor.getNEO(2), 1, 0.025), DCMotor.getNEO(2), 0.0); + private FlywheelSim indexerSim = + new FlywheelSim(LinearSystemId.createFlywheelSystem(DCMotor.getFalcon500(1), 1, 0.025), + DCMotor.getFalcon500(1), 0.0); private double intakeAppliedVolts = 0.0; private double indexerAppliedVolts = 0.0; diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSim.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSim.java index da82fa28..7a5a6160 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSim.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSim.java @@ -3,6 +3,7 @@ import org.littletonrobotics.junction.LoggedRobot; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.wpilibj.simulation.FlywheelSim; import frc.lib.sim.SimulatedPumbaa; import frc.robot.Constants; @@ -13,9 +14,14 @@ public class ShooterSim implements ShooterIO { private FlywheelSim topShooterMotor = - new FlywheelSim(DCMotor.getNeoVortex(1), Constants.ShooterConstants.GEAR_RATIO, 0.01); + new FlywheelSim(LinearSystemId.createFlywheelSystem(DCMotor.getNeoVortex(2), 1, + Constants.ShooterConstants.GEAR_RATIO), DCMotor.getNeoVortex(2), 0.0); + // new FlywheelSim(DCMotor.getNeoVortex(1), Constants.ShooterConstants.GEAR_RATIO, 0.01); private FlywheelSim bottomShooterMotor = - new FlywheelSim(DCMotor.getNeoVortex(1), Constants.ShooterConstants.GEAR_RATIO, 0.01); + new FlywheelSim(LinearSystemId.createFlywheelSystem(DCMotor.getNeoVortex(1), 1, + Constants.ShooterConstants.GEAR_RATIO), DCMotor.getNeoVortex(1), 0.0); + + // new FlywheelSim(DCMotor.getNeoVortex(1), Constants.ShooterConstants.GEAR_RATIO, 0.01); private double topAppliedVolts = 0.0; private double bottomAppliedVolts = 0.0; diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterVortex.java b/src/main/java/frc/robot/subsystems/shooter/ShooterVortex.java index 95f2ffc5..11f2ae02 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterVortex.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterVortex.java @@ -1,45 +1,45 @@ package frc.robot.subsystems.shooter; -import com.revrobotics.CANSparkBase.IdleMode; -import com.revrobotics.CANSparkFlex; -import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.SparkBaseConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkFlexConfig; import frc.robot.Constants; /** * Class for ShooterVortex */ public class ShooterVortex implements ShooterIO { - public final CANSparkFlex topShooterMotor = - new CANSparkFlex(Constants.Motors.Shooter.SHOOTER_TOP_ID, MotorType.kBrushless); - public final CANSparkFlex bottomShooterMotor = - new CANSparkFlex(Constants.Motors.Shooter.SHOOTER_BOTTOM_ID, MotorType.kBrushless); + public final SparkFlex topShooterMotor = + new SparkFlex(Constants.Motors.Shooter.SHOOTER_TOP_ID, MotorType.kBrushless); + public final SparkFlex bottomShooterMotor = + new SparkFlex(Constants.Motors.Shooter.SHOOTER_BOTTOM_ID, MotorType.kBrushless); private RelativeEncoder topEncoder = topShooterMotor.getEncoder(); private RelativeEncoder bottomEncoder = bottomShooterMotor.getEncoder(); + SparkBaseConfig topConfig = new SparkFlexConfig(); + SparkBaseConfig bottomConfig = new SparkFlexConfig(); /** * Constructor Shooter Subsystem - sets motor and encoder preferences */ public ShooterVortex() { - topShooterMotor.restoreFactoryDefaults(); - bottomShooterMotor.restoreFactoryDefaults(); - topShooterMotor.setIdleMode(IdleMode.kCoast); - bottomShooterMotor.setIdleMode(IdleMode.kCoast); - topShooterMotor.setInverted(false); - bottomShooterMotor.setInverted(true); - // topShooterMotor.setSmartCurrentLimit(20); - // bottomShooterMotor.setSmartCurrentLimit(20); - // topShooterMotor.enableVoltageCompensation(12); - // bottomShooterMotor.enableVoltageCompensation(12); - // gear ratio 31:16 - topEncoder.setPositionConversionFactor(Constants.ShooterConstants.GEAR_RATIO); - topEncoder.setVelocityConversionFactor(Constants.ShooterConstants.GEAR_RATIO); - bottomEncoder.setPositionConversionFactor(Constants.ShooterConstants.GEAR_RATIO); - bottomEncoder.setVelocityConversionFactor(Constants.ShooterConstants.GEAR_RATIO); - bottomShooterMotor.burnFlash(); - topShooterMotor.burnFlash(); + topConfig.inverted(false).idleMode(IdleMode.kCoast).voltageCompensation(12) + .voltageCompensation(12); + bottomConfig.inverted(true).idleMode(IdleMode.kCoast).voltageCompensation(12) + .voltageCompensation(12); + topConfig.encoder.positionConversionFactor(Constants.ShooterConstants.GEAR_RATIO); + topConfig.encoder.velocityConversionFactor(Constants.ShooterConstants.GEAR_RATIO); + bottomConfig.encoder.positionConversionFactor(Constants.ShooterConstants.GEAR_RATIO); + bottomConfig.encoder.velocityConversionFactor(Constants.ShooterConstants.GEAR_RATIO); - // thread.start(); + topShooterMotor.configure(topConfig, ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters); + bottomShooterMotor.configure(bottomConfig, ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters); } public void setTopMotor(double power) { diff --git a/src/main/java/frc/robot/subsystems/swerve/Swerve.java b/src/main/java/frc/robot/subsystems/swerve/Swerve.java index 63e815c8..42e75ac1 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Swerve.java +++ b/src/main/java/frc/robot/subsystems/swerve/Swerve.java @@ -1,11 +1,8 @@ package frc.robot.subsystems.swerve; -import java.util.Arrays; -import java.util.Map; import java.util.Optional; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; -import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.util.PathPlannerLogging; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; @@ -15,20 +12,15 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.lib.util.FieldConstants; -import frc.lib.util.photon.PhotonCameraWrapper; import frc.lib.util.swerve.SwerveModule; import frc.lib.viz.PumbaaViz; import frc.robot.Constants; -import frc.robot.Robot; import frc.robot.RobotContainer; /** @@ -41,23 +33,22 @@ public class Swerve extends SubsystemBase { private double fieldOffset; private SwerveInputsAutoLogged inputs = new SwerveInputsAutoLogged(); private SwerveIO swerveIO; - private boolean hasInitialized = false; - private PhotonCameraWrapper[] cameras; - private Boolean[] cameraSeesTarget = {false, false, false, false}; + // private boolean hasInitialized = false; + // private Boolean[] cameraSeesTarget = {false, false, false, false}; - private GenericEntry aprilTagTarget = RobotContainer.mainDriverTab.add("See April Tag", false) - .withWidget(BuiltInWidgets.kBooleanBox) - .withProperties(Map.of("Color when true", "green", "Color when false", "red")) - .withPosition(11, 0).withSize(2, 2).getEntry(); + // private GenericEntry aprilTagTarget = RobotContainer.mainDriverTab.add("See April Tag", + // false) + // .withWidget(BuiltInWidgets.kBooleanBox) + // .withProperties(Map.of("Color when true", "green", "Color when false", "red")) + // .withPosition(11, 0).withSize(2, 2).getEntry(); private final PumbaaViz viz; /** * Swerve Subsystem */ - public Swerve(SwerveIO swerveIO, PhotonCameraWrapper[] cameras, PumbaaViz viz) { + public Swerve(SwerveIO swerveIO, PumbaaViz viz) { this.swerveIO = swerveIO; - this.cameras = cameras; this.viz = viz; swerveMods = swerveIO.createModules(); fieldOffset = getGyroYaw().getDegrees(); @@ -67,9 +58,10 @@ public Swerve(SwerveIO swerveIO, PhotonCameraWrapper[] cameras, PumbaaViz viz) { swerveIO.updateInputs(inputs); - AutoBuilder.configureHolonomic(this::getPose, this::resetOdometry, this::getChassisSpeeds, - this::setModuleStates, Constants.Swerve.pathFollowerConfig, () -> shouldFlipPath(), - this); + // AutoBuilder.configureHolonomic(this::getPose, this::resetOdometry, + // this::getChassisSpeeds, + // this::setModuleStates, Constants.Swerve.pathFollowerConfig, () -> shouldFlipPath(), + // this); RobotContainer.mainDriverTab.add("Field Pos", field).withWidget(BuiltInWidgets.kField) .withSize(8, 4) // make the widget 2x1 @@ -98,14 +90,14 @@ public Swerve(SwerveIO swerveIO, PhotonCameraWrapper[] cameras, PumbaaViz viz) { */ public void drive(Translation2d translation, double rotation, boolean fieldRelative, boolean isOpenLoop) { - Robot.profiler.push("swerve.drive()"); + // Robot.profiler.push("swerve.drive()"); ChassisSpeeds chassisSpeeds = fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(translation.getX(), translation.getY(), rotation, getFieldRelativeHeading()) : new ChassisSpeeds(translation.getX(), translation.getY(), rotation); setModuleStates(chassisSpeeds); - Robot.profiler.pop(); + // Robot.profiler.pop(); } /** @@ -225,88 +217,88 @@ public void resetFieldRelativeOffset() { fieldOffset = getGyroYaw().getDegrees() + 180; } - public void resetPvInitialization() { - hasInitialized = false; - } + // public void resetPvInitialization() { + // // hasInitialized = false; + // } @Override public void periodic() { - Robot.profiler.push("swerve_periodic"); - Robot.profiler.push("update_inputs"); + // Robot.profiler.push("swerve_periodic"); + // Robot.profiler.push("update_inputs"); swerveIO.updateInputs(inputs); - Robot.profiler.swap("update_swerve_mods"); + // Robot.profiler.swap("update_swerve_mods"); for (var mod : swerveMods) { mod.periodic(); } - Robot.profiler.swap("update_swerve_odometry"); + // Robot.profiler.swap("update_swerve_odometry"); swerveOdometry.update(getGyroYaw(), getModulePositions()); - Robot.profiler.swap("process_inputs"); + // Robot.profiler.swap("process_inputs"); Logger.processInputs("Swerve", inputs); - Robot.profiler.swap("process_cameras"); - for (int i = 0; i < cameras.length; i++) { - cameras[i].periodic(); - cameraSeesTarget[i] = cameras[i].seesTarget(); - } - Robot.profiler.swap("do_camera_stuff"); - Logger.recordOutput("/Swerve/hasInitialized", hasInitialized); - if (!hasInitialized && !DriverStation.isAutonomous()) { - Robot.profiler.push("init"); - for (int i = 0; i < cameras.length; i++) { - Robot.profiler.push(cameras[i].inputs.name); - var robotPose = cameras[i].getInitialPose(); - Logger.recordOutput("/Swerve/hasInitialPose[" + i + "]", robotPose.isPresent()); - - if (robotPose.isPresent()) { - swerveOdometry.resetPosition(getGyroYaw(), getModulePositions(), - robotPose.get().robotPose); - hasInitialized = true; - Robot.profiler.pop(); - break; - } - Robot.profiler.pop(); - } - Robot.profiler.pop(); - } else { - Robot.profiler.push("update"); - for (int i = 0; i < cameras.length; i++) { - Robot.profiler.push(cameras[i].inputs.name); - var result = cameras[i].getEstimatedGlobalPose(getPose()); - if (result.isPresent()) { - if (DriverStation.isAutonomous() && result.get().targetsUsed.size() < 2) { - Robot.profiler.pop(); - continue; - } else if (result.get().targetsUsed.size() == 1 - && result.get().targetsUsed.get(0).getPoseAmbiguity() > 0.1) { - Robot.profiler.pop(); - continue; - } - swerveOdometry.addVisionMeasurement(result.get().estimatedPose.toPose2d(), - Timer.getFPGATimestamp() - cameras[i].latency()); - } - Robot.profiler.pop(); - } - Robot.profiler.pop(); - } - Robot.profiler.swap("update_shuffleboard"); - Robot.profiler.push("field"); - field.setRobotPose(getPose()); - Robot.profiler.swap("apriltag"); - aprilTagTarget - .setBoolean(Arrays.asList(cameraSeesTarget).stream().anyMatch(val -> val == true)); - - Robot.profiler.swap("dist-to-speaker"); - SmartDashboard.putNumber("Distance to Speaker", - FieldConstants.allianceFlip(FieldConstants.Speaker.centerSpeakerOpening) - .getTranslation().minus(getPose().getTranslation()).getNorm()); - Robot.profiler.swap("simple"); - SmartDashboard.putBoolean("Has Initialized", hasInitialized); - SmartDashboard.putNumber("Gyro Yaw", getGyroYaw().getDegrees()); - Logger.recordOutput("/Swerve/ActualStates", getModuleStates()); - Robot.profiler.pop(); - Robot.profiler.pop(); - Robot.profiler.swap("viz"); - viz.setPose(getPose()); - Robot.profiler.pop(); + // Robot.profiler.swap("process_cameras"); + // for (int i = 0; i < cameras.length; i++) { + // cameras[i].periodic(); + // cameraSeesTarget[i] = cameras[i].seesTarget(); + // } + // Robot.profiler.swap("do_camera_stuff"); + // Logger.recordOutput("/Swerve/hasInitialized", hasInitialized); + // if (!hasInitialized && !DriverStation.isAutonomous()) { + // Robot.profiler.push("init"); + // for (int i = 0; i < cameras.length; i++) { + // Robot.profiler.push(cameras[i].inputs.name); + // var robotPose = cameras[i].getInitialPose(); + // Logger.recordOutput("/Swerve/hasInitialPose[" + i + "]", robotPose.isPresent()); + + // if (robotPose.isPresent()) { + // swerveOdometry.resetPosition(getGyroYaw(), getModulePositions(), + // robotPose.get().robotPose); + // hasInitialized = true; + // Robot.profiler.pop(); + // break; + // } + // Robot.profiler.pop(); + // } + // Robot.profiler.pop(); + // // } else { + // Robot.profiler.push("update"); + // for (int i = 0; i < cameras.length; i++) { + // Robot.profiler.push(cameras[i].inputs.name); + // // var result = cameras[i].getEstimatedGlobalPose(getPose()); + // if (result.isPresent()) { + // if (DriverStation.isAutonomous() && result.get().targetsUsed.size() < 2) { + // Robot.profiler.pop(); + // continue; + // } else if (result.get().targetsUsed.size() == 1 + // && result.get().targetsUsed.get(0).getPoseAmbiguity() > 0.1) { + // Robot.profiler.pop(); + // continue; + // } + // swerveOdometry.addVisionMeasurement(result.get().estimatedPose.toPose2d(), + // Timer.getFPGATimestamp() - cameras[i].latency()); + // // } + // Robot.profiler.pop(); + // } + // Robot.profiler.pop(); + // } + // Robot.profiler.swap("update_shuffleboard"); + // Robot.profiler.push("field"); + // // field.setRobotPose(getPose()); + // Robot.profiler.swap("apriltag"); + // aprilTagTarget + // .setBoolean(Arrays.asList(cameraSeesTarget).stream().anyMatch(val -> val == true)); + + // Robot.profiler.swap("dist-to-speaker"); + // SmartDashboard.putNumber("Distance to Speaker", + // FieldConstants.allianceFlip(FieldConstants.Speaker.centerSpeakerOpening) + // .getTranslation().minus(getPose().getTranslation()).getNorm()); + // Robot.profiler.swap("simple"); + // SmartDashboard.putBoolean("Has Initialized", hasInitialized); + // SmartDashboard.putNumber("Gyro Yaw", getGyroYaw().getDegrees()); + // Logger.recordOutput("/Swerve/ActualStates", getModuleStates()); + // Robot.profiler.pop(); + // Robot.profiler.pop(); + // Robot.profiler.swap("viz"); + // viz.setPose(getPose()); + // Robot.profiler.pop(); } /** diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveReal.java b/src/main/java/frc/robot/subsystems/swerve/SwerveReal.java index 7e3dfca2..fd48abf0 100755 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveReal.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveReal.java @@ -1,6 +1,7 @@ package frc.robot.subsystems.swerve; -import com.kauailabs.navx.frc.AHRS; + +import com.studica.frc.AHRS; import edu.wpi.first.math.geometry.Rotation2d; import frc.lib.util.swerve.SwerveModule; import frc.lib.util.swerve.SwerveModuleReal; diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json index 63dacbb5..1fa7c033 100644 --- a/vendordeps/AdvantageKit.json +++ b/vendordeps/AdvantageKit.json @@ -1,33 +1,25 @@ { "fileName": "AdvantageKit.json", "name": "AdvantageKit", - "version": "3.2.1", + "version": "4.0.0", "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", - "frcYear": "2024", - "mavenUrls": [], + "frcYear": "2025", + "mavenUrls": [ + "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/" + ], "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", "javaDependencies": [ { - "groupId": "org.littletonrobotics.akit.junction", - "artifactId": "wpilib-shim", - "version": "3.2.1" - }, - { - "groupId": "org.littletonrobotics.akit.junction", - "artifactId": "junction-core", - "version": "3.2.1" - }, - { - "groupId": "org.littletonrobotics.akit.conduit", - "artifactId": "conduit-api", - "version": "3.2.1" + "groupId": "org.littletonrobotics.akit", + "artifactId": "akit-java", + "version": "4.0.0" } ], "jniDependencies": [ { - "groupId": "org.littletonrobotics.akit.conduit", - "artifactId": "conduit-wpilibio", - "version": "3.2.1", + "groupId": "org.littletonrobotics.akit", + "artifactId": "akit-wpilibio", + "version": "4.0.0", "skipInvalidPlatforms": false, "isJar": false, "validPlatforms": [ diff --git a/vendordeps/NavX.json b/vendordeps/NavX.json deleted file mode 100644 index e978a5f7..00000000 --- a/vendordeps/NavX.json +++ /dev/null @@ -1,40 +0,0 @@ -{ - "fileName": "NavX.json", - "name": "NavX", - "version": "2024.1.0", - "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", - "frcYear": "2024", - "mavenUrls": [ - "https://dev.studica.com/maven/release/2024/" - ], - "jsonUrl": "https://dev.studica.com/releases/2024/NavX.json", - "javaDependencies": [ - { - "groupId": "com.kauailabs.navx.frc", - "artifactId": "navx-frc-java", - "version": "2024.1.0" - } - ], - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "com.kauailabs.navx.frc", - "artifactId": "navx-frc-cpp", - "version": "2024.1.0", - "headerClassifier": "headers", - "sourcesClassifier": "sources", - "sharedLibrary": false, - "libName": "navx_frc", - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "linuxraspbian", - "linuxarm32", - "linuxarm64", - "linuxx86-64", - "osxuniversal", - "windowsx86-64" - ] - } - ] -} \ No newline at end of file diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib-2025.2.1.json similarity index 85% rename from vendordeps/PathplannerLib.json rename to vendordeps/PathplannerLib-2025.2.1.json index 6dc648db..71e25f3d 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib-2025.2.1.json @@ -1,9 +1,9 @@ { - "fileName": "PathplannerLib.json", + "fileName": "PathplannerLib-2025.2.1.json", "name": "PathplannerLib", - "version": "2024.2.8", + "version": "2025.2.1", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", - "frcYear": "2024", + "frcYear": "2025", "mavenUrls": [ "https://3015rangerrobotics.github.io/pathplannerlib/repo" ], @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2024.2.8" + "version": "2025.2.1" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2024.2.8", + "version": "2025.2.1", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6-25.1.0.json similarity index 77% rename from vendordeps/Phoenix6.json rename to vendordeps/Phoenix6-25.1.0.json index 03223850..473f6a89 100644 --- a/vendordeps/Phoenix6.json +++ b/vendordeps/Phoenix6-25.1.0.json @@ -1,76 +1,94 @@ { - "fileName": "Phoenix6.json", + "fileName": "Phoenix6-25.1.0.json", "name": "CTRE-Phoenix (v6)", - "version": "24.3.0", - "frcYear": 2024, + "version": "25.1.0", + "frcYear": "2025", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ "https://maven.ctr-electronics.com/release/" ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json", + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-latest.json", "conflictsWith": [ { - "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a", - "errorMessage": "The combined Phoenix-6-And-5 vendordep is no longer supported. Please remove the vendordep and instead add both the latest Phoenix 6 vendordep and Phoenix 5 vendordep.", - "offlineFileName": "Phoenix6And5.json" + "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", + "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.", + "offlineFileName": "Phoenix6-replay-frc2025-latest.json" } ], "javaDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "24.3.0" + "version": "25.1.0" } ], "jniDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "api-cpp", + "version": "25.1.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.3.0", + "version": "25.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "linuxathena" ], "simMode": "hwsim" }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "tools-sim", - "version": "24.3.0", + "artifactId": "api-cpp-sim", + "version": "25.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonSRX", - "version": "24.3.0", + "artifactId": "tools-sim", + "version": "25.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonFX", - "version": "24.3.0", + "artifactId": "simTalonSRX", + "version": "25.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -78,12 +96,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.3.0", + "version": "25.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -91,12 +110,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.3.0", + "version": "25.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -104,12 +124,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.3.0", + "version": "25.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -117,12 +138,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.3.0", + "version": "25.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -130,12 +152,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.3.0", + "version": "25.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -143,12 +166,27 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.3.0", + "version": "25.1.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "25.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -158,7 +196,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "24.3.0", + "version": "25.1.0", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -166,6 +204,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "linuxathena" ], "simMode": "hwsim" @@ -173,7 +212,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.3.0", + "version": "25.1.0", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -181,6 +220,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "linuxathena" ], "simMode": "hwsim" @@ -188,7 +228,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "24.3.0", + "version": "25.1.0", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -196,6 +236,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -203,7 +244,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "24.3.0", + "version": "25.1.0", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -211,6 +252,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -218,7 +260,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "24.3.0", + "version": "25.1.0", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -226,21 +268,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonFX", - "version": "24.3.0", - "libName": "CTRE_SimTalonFX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -248,7 +276,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.3.0", + "version": "25.1.0", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -256,6 +284,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -263,7 +292,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.3.0", + "version": "25.1.0", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -271,6 +300,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -278,7 +308,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.3.0", + "version": "25.1.0", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -286,6 +316,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -293,7 +324,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.3.0", + "version": "25.1.0", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -301,6 +332,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -308,7 +340,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.3.0", + "version": "25.1.0", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -316,6 +348,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -323,7 +356,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.3.0", + "version": "25.1.0", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -331,6 +364,23 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "25.1.0", + "libName": "CTRE_SimProCANrange", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib-2025.0.0.json similarity index 88% rename from vendordeps/REVLib.json rename to vendordeps/REVLib-2025.0.0.json index f85acd40..cde60117 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib-2025.0.0.json @@ -1,25 +1,25 @@ { - "fileName": "REVLib.json", + "fileName": "REVLib-2025.0.0.json", "name": "REVLib", - "version": "2024.2.4", - "frcYear": "2024", + "version": "2025.0.0", + "frcYear": "2025", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ "https://maven.revrobotics.com/" ], - "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2024.json", + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2025.json", "javaDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2024.2.4" + "version": "2025.0.0" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.4", + "version": "2025.0.0", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -37,7 +37,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2024.2.4", + "version": "2025.0.0", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -55,7 +55,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.4", + "version": "2025.0.0", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/Studica-2025.0.0.json b/vendordeps/Studica-2025.0.0.json new file mode 100644 index 00000000..ddb0e44b --- /dev/null +++ b/vendordeps/Studica-2025.0.0.json @@ -0,0 +1,71 @@ +{ + "fileName": "Studica-2025.0.0.json", + "name": "Studica", + "version": "2025.0.0", + "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", + "frcYear": "2025", + "mavenUrls": [ + "https://dev.studica.com/maven/release/2025/" + ], + "jsonUrl": "https://dev.studica.com/releases/2025/Studica-2025.0.0.json", + "cppDependencies": [ + { + "artifactId": "Studica-cpp", + "binaryPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "linuxx86-64", + "osxuniversal", + "windowsx86-64" + ], + "groupId": "com.studica.frc", + "headerClassifier": "headers", + "libName": "Studica", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "version": "2025.0.0" + }, + { + "artifactId": "Studica-driver", + "binaryPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "linuxx86-64", + "osxuniversal", + "windowsx86-64" + ], + "groupId": "com.studica.frc", + "headerClassifier": "headers", + "libName": "StudicaDriver", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "version": "2025.0.0" + } + ], + "javaDependencies": [ + { + "artifactId": "Studica-java", + "groupId": "com.studica.frc", + "version": "2025.0.0" + } + ], + "jniDependencies": [ + { + "artifactId": "Studica-driver", + "groupId": "com.studica.frc", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "linuxx86-64", + "osxuniversal", + "windowsx86-64" + ], + "version": "2025.0.0" + } + ] +} \ No newline at end of file diff --git a/vendordeps/WPILibNewCommands.json b/vendordeps/WPILibNewCommands.json index 67bf3898..3718e0ac 100644 --- a/vendordeps/WPILibNewCommands.json +++ b/vendordeps/WPILibNewCommands.json @@ -3,7 +3,7 @@ "name": "WPILib-New-Commands", "version": "1.0.0", "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", - "frcYear": "2024", + "frcYear": "2025", "mavenUrls": [], "jsonUrl": "", "javaDependencies": [ diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json deleted file mode 100644 index 0e80a16c..00000000 --- a/vendordeps/photonlib.json +++ /dev/null @@ -1,57 +0,0 @@ -{ - "fileName": "photonlib.json", - "name": "photonlib", - "version": "v2024.3.1", - "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", - "frcYear": "2024", - "mavenUrls": [ - "https://maven.photonvision.org/repository/internal", - "https://maven.photonvision.org/repository/snapshots" - ], - "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "photonlib-cpp", - "version": "v2024.3.1", - "libName": "photonlib", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - }, - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-cpp", - "version": "v2024.3.1", - "libName": "photontargeting", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - } - ], - "javaDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "photonlib-java", - "version": "v2024.3.1" - }, - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-java", - "version": "v2024.3.1" - } - ] -} \ No newline at end of file