From 1d64c627c56d71acae2deb15d69208471874acc4 Mon Sep 17 00:00:00 2001 From: MartijnKM Date: Sat, 25 Jan 2025 13:28:12 -0600 Subject: [PATCH] Start Swerve --- .gitignore | 6 +- .vscode/extensions.json | 5 + .vscode/launch.json | 8 +- .vscode/settings.json | 20 +- .wpilib/wpilib_preferences.json | 10 +- AdvantageScope Swerve Calibration.json | 463 +++++ LICENSE.md | 2 +- WPILib-License.md | 24 - build.gradle | 124 +- gradlew | 7 +- gradlew.bat | 22 +- settings.gradle | 4 +- src/main/deploy/example.txt | 3 - .../pathplanner/autos/Example Auto.auto | 19 + src/main/deploy/pathplanner/navgrid.json | 1656 +++++++++++++++++ .../pathplanner/paths/Example Path.path | 70 + src/main/deploy/pathplanner/settings.json | 21 + src/main/java/frc/robot/Constants.java | 40 +- src/main/java/frc/robot/Main.java | 15 +- src/main/java/frc/robot/Robot.java | 132 +- src/main/java/frc/robot/RobotContainer.java | 161 +- src/main/java/frc/robot/commands/Autos.java | 20 - .../frc/robot/commands/DriveCommands.java | 298 +++ .../frc/robot/commands/ExampleCommand.java | 43 - .../robot/subsystems/ExampleSubsystem.java | 47 - .../frc/robot/subsystems/drive/Drive.java | 323 ++++ .../subsystems/drive/DriveConstants.java | 114 ++ .../frc/robot/subsystems/drive/GyroIO.java | 30 + .../subsystems/drive/GyroIOCanandgyro.java | 52 + .../robot/subsystems/drive/GyroIONavX.java | 50 + .../robot/subsystems/drive/GyroIOPigeon2.java | 62 + .../frc/robot/subsystems/drive/Module.java | 131 ++ .../frc/robot/subsystems/drive/ModuleIO.java | 53 + .../robot/subsystems/drive/ModuleIOSim.java | 118 ++ .../robot/subsystems/drive/ModuleIOSpark.java | 248 +++ .../subsystems/drive/SparkOdometryThread.java | 132 ++ .../java/frc/robot/util/LocalADStarAK.java | 153 ++ src/main/java/frc/robot/util/SparkUtil.java | 62 + vendordeps/AdvantageKit.json | 35 + vendordeps/PathplannerLib.json | 38 + vendordeps/Phoenix6.json | 419 +++++ vendordeps/REVLib.json | 71 + vendordeps/ReduxLib-2025.0.1.json | 72 + vendordeps/Studica.json | 71 + vendordeps/URCL.json | 65 + 45 files changed, 5257 insertions(+), 262 deletions(-) create mode 100644 .vscode/extensions.json create mode 100644 AdvantageScope Swerve Calibration.json delete mode 100644 WPILib-License.md delete mode 100644 src/main/deploy/example.txt create mode 100644 src/main/deploy/pathplanner/autos/Example Auto.auto create mode 100644 src/main/deploy/pathplanner/navgrid.json create mode 100644 src/main/deploy/pathplanner/paths/Example Path.path create mode 100644 src/main/deploy/pathplanner/settings.json delete mode 100644 src/main/java/frc/robot/commands/Autos.java create mode 100644 src/main/java/frc/robot/commands/DriveCommands.java delete mode 100644 src/main/java/frc/robot/commands/ExampleCommand.java delete mode 100644 src/main/java/frc/robot/subsystems/ExampleSubsystem.java create mode 100644 src/main/java/frc/robot/subsystems/drive/Drive.java create mode 100644 src/main/java/frc/robot/subsystems/drive/DriveConstants.java create mode 100644 src/main/java/frc/robot/subsystems/drive/GyroIO.java create mode 100644 src/main/java/frc/robot/subsystems/drive/GyroIOCanandgyro.java create mode 100644 src/main/java/frc/robot/subsystems/drive/GyroIONavX.java create mode 100644 src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java create mode 100644 src/main/java/frc/robot/subsystems/drive/Module.java create mode 100644 src/main/java/frc/robot/subsystems/drive/ModuleIO.java create mode 100644 src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java create mode 100644 src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java create mode 100644 src/main/java/frc/robot/subsystems/drive/SparkOdometryThread.java create mode 100644 src/main/java/frc/robot/util/LocalADStarAK.java create mode 100644 src/main/java/frc/robot/util/SparkUtil.java create mode 100644 vendordeps/AdvantageKit.json create mode 100644 vendordeps/PathplannerLib.json create mode 100644 vendordeps/Phoenix6.json create mode 100644 vendordeps/REVLib.json create mode 100644 vendordeps/ReduxLib-2025.0.1.json create mode 100644 vendordeps/Studica.json create mode 100644 vendordeps/URCL.json diff --git a/.gitignore b/.gitignore index 34cbaac..d012c1d 100644 --- a/.gitignore +++ b/.gitignore @@ -170,7 +170,7 @@ out/ # Simulation GUI and other tools window save file networktables.json -simgui.json +simgui*.json *-window.json # Simulation data log directory @@ -183,5 +183,5 @@ ctre_sim/ /.cache compile_commands.json -# Eclipse generated file for annotation processors -.factorypath +# Version file +src/main/java/frc/robot/BuildConstants.java \ No newline at end of file diff --git a/.vscode/extensions.json b/.vscode/extensions.json new file mode 100644 index 0000000..567056a --- /dev/null +++ b/.vscode/extensions.json @@ -0,0 +1,5 @@ +{ + "recommendations": [ + "richardwillis.vscode-spotless-gradle" + ] +} diff --git a/.vscode/launch.json b/.vscode/launch.json index c9c9713..b8c1920 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -1,21 +1,17 @@ { - // Use IntelliSense to learn about possible attributes. - // Hover to view descriptions of existing attributes. - // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 "version": "0.2.0", "configurations": [ - { "type": "wpilib", "name": "WPILib Desktop Debug", "request": "launch", - "desktop": true, + "desktop": true }, { "type": "wpilib", "name": "WPILib roboRIO Debug", "request": "launch", - "desktop": false, + "desktop": false } ] } diff --git a/.vscode/settings.json b/.vscode/settings.json index 612cdd0..868594c 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -18,14 +18,19 @@ { "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" } }, + null ], "java.test.defaultConfig": "WPIlibUnitTests", + "spotlessGradle.format.enable": true, + "spotlessGradle.diagnostics.enable": false, "java.import.gradle.annotationProcessing.enabled": false, "java.completion.favoriteStaticMembers": [ "org.junit.Assert.*", @@ -55,6 +60,13 @@ "javax.smartcardio.*", "edu.wpi.first.math.proto.*", "edu.wpi.first.math.**.proto.*", - "edu.wpi.first.math.**.struct.*", - ] + "edu.wpi.first.math.**.struct.*" + ], + "editor.defaultFormatter": "richardwillis.vscode-spotless-gradle", + "[json]": { + "editor.defaultFormatter": "richardwillis.vscode-spotless-gradle" + }, + "[java]": { + "editor.defaultFormatter": "richardwillis.vscode-spotless-gradle" + } } diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index c08f56d..2171dac 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -1,6 +1,6 @@ { - "enableCppIntellisense": false, - "currentLanguage": "java", - "projectYear": "2025", - "teamNumber": 2846 -} \ No newline at end of file + "enableCppIntellisense": false, + "currentLanguage": "java", + "projectYear": "2025", + "teamNumber": 6328 +} diff --git a/AdvantageScope Swerve Calibration.json b/AdvantageScope Swerve Calibration.json new file mode 100644 index 0000000..f412ed2 --- /dev/null +++ b/AdvantageScope Swerve Calibration.json @@ -0,0 +1,463 @@ +{ + "hubs": [ + { + "x": 139, + "y": 95, + "width": 1100, + "height": 650, + "state": { + "sidebar": { + "width": 300, + "expanded": [ + "/Drive", + "/RealOutputs", + "/RealOutputs/SwerveStates", + "/RealOutputs/SwerveChassisSpeeds", + "/RealOutputs/Odometry" + ] + }, + "tabs": { + "selected": 1, + "tabs": [ + { + "type": 0, + "title": "", + "controller": null, + "controllerUUID": "grimvlgu9aluj8btvwotco2wpj0pto2h", + "renderer": "#/", + "controlsHeight": 0 + }, + { + "type": 9, + "title": "Drive Overview", + "controller": { + "sources": [ + { + "type": "states", + "logKey": "/RealOutputs/SwerveStates/Measured", + "logType": "SwerveModuleState[]", + "visible": true, + "options": { + "color": "#ff0000", + "arrangement": "0,1,2,3" + } + }, + { + "type": "states", + "logKey": "/RealOutputs/SwerveStates/SetpointsOptimized", + "logType": "SwerveModuleState[]", + "visible": true, + "options": { + "color": "#00ffff", + "arrangement": "0,1,2,3" + } + }, + { + "type": "chassisSpeeds", + "logKey": "/RealOutputs/SwerveChassisSpeeds/Measured", + "logType": "ChassisSpeeds", + "visible": true, + "options": { + "color": "#ff0000" + } + }, + { + "type": "chassisSpeeds", + "logKey": "/RealOutputs/SwerveChassisSpeeds/Setpoints", + "logType": "ChassisSpeeds", + "visible": true, + "options": { + "color": "#00ffff" + } + }, + { + "type": "rotation", + "logKey": "/RealOutputs/Odometry/Robot/rotation", + "logType": "Rotation2d", + "visible": true, + "options": {} + } + ], + "maxSpeed": 5, + "sizeX": 0.65, + "sizeY": 0.65, + "orientation": 1 + }, + "controllerUUID": "prdx7t2eedan6n46dxrjfu7eisf16f9o", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 3, + "title": "Field", + "controller": { + "sources": [ + { + "type": "robot", + "logKey": "/RealOutputs/Odometry/Robot", + "logType": "Pose2d", + "visible": true, + "options": { + "model": "Crab Bot" + } + }, + { + "type": "trajectory", + "logKey": "/RealOutputs/Odometry/Trajectory", + "logType": "Pose2d[]", + "visible": true, + "options": { + "color": "#ff8c00", + "size": "normal" + } + }, + { + "type": "ghost", + "logKey": "/RealOutputs/Odometry/TrajectorySetpoint", + "logType": "Pose2d", + "visible": true, + "options": { + "model": "Crab Bot", + "color": "#ff8c00" + } + } + ], + "game": "2025 Field", + "origin": "blue" + }, + "controllerUUID": "psf0y633oclnjyocus23hcnq1d4tpyte", + "renderer": { + "cameraIndex": -1, + "orbitFov": 50, + "cameraPosition": [ + 1.4695761589768238e-15, + 5.999999999999999, + -12 + ], + "cameraTarget": [ + 0, + 0.5, + 0 + ] + }, + "controlsHeight": 200 + }, + { + "type": 1, + "title": "Turn Calibration", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "/Drive/Module0/TurnPosition/value", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/Drive/Module1/TurnPosition/value", + "logType": "Number", + "visible": true, + "options": { + "color": "#af2437", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/Drive/Module2/TurnPosition/value", + "logType": "Number", + "visible": true, + "options": { + "color": "#80588e", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/Drive/Module3/TurnPosition/value", + "logType": "Number", + "visible": true, + "options": { + "color": "#e48b32", + "size": "normal" + } + } + ], + "rightSources": [], + "discreteSources": [], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "type": null, + "factor": 1 + }, + "rightUnitConversion": { + "type": null, + "factor": 1 + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "ol8lk80m83ma3aegae849d6k1d29sp7d", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 1, + "title": "Drive Calibration", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "/Drive/Module0/DrivePositionRad", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/Drive/Module1/DrivePositionRad", + "logType": "Number", + "visible": true, + "options": { + "color": "#af2437", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/Drive/Module2/DrivePositionRad", + "logType": "Number", + "visible": true, + "options": { + "color": "#80588e", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/Drive/Module3/DrivePositionRad", + "logType": "Number", + "visible": true, + "options": { + "color": "#e48b32", + "size": "normal" + } + } + ], + "rightSources": [], + "discreteSources": [], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "type": null, + "factor": 1 + }, + "rightUnitConversion": { + "type": null, + "factor": 1 + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "7jj1f83nsv59dd2ou11ls3ccz5tq1293", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 1, + "title": "Turn PID Tuning", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "/RealOutputs/SwerveStates/Measured/0/angle/value", + "logType": "Number", + "visible": true, + "options": { + "color": "#2b66a2", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/RealOutputs/SwerveStates/SetpointsOptimized/0/angle/value", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + } + ], + "rightSources": [], + "discreteSources": [], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "type": null, + "factor": 1 + }, + "rightUnitConversion": { + "type": null, + "factor": 1 + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "kj6tf6sxm5g04wnjrhed6mx2wpccmqtn", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 1, + "title": "Drive PID Tuning", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "/RealOutputs/SwerveStates/Measured/0/speed", + "logType": "Number", + "visible": true, + "options": { + "color": "#2b66a2", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/RealOutputs/SwerveStates/SetpointsOptimized/0/speed", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + } + ], + "rightSources": [], + "discreteSources": [], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "type": null, + "factor": 1 + }, + "rightUnitConversion": { + "type": null, + "factor": 1 + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "bctxg9pmwpo31m9bv9ofr92oqqtbpqzp", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 1, + "title": "Max Speed Measurement", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "/RealOutputs/SwerveChassisSpeeds/Measured/vx", + "logType": "Number", + "visible": true, + "options": { + "color": "#2b66a2", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/RealOutputs/SwerveChassisSpeeds/Measured/vy", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + } + ], + "rightSources": [], + "discreteSources": [], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "type": null, + "factor": 1 + }, + "rightUnitConversion": { + "type": null, + "factor": 1 + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "oy3vmjuhr7g36evikwnmjhfacl30mzn5", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 1, + "title": "Slip Current Measurement", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "/Drive/Module0/DriveCurrentAmps", + "logType": "Number", + "visible": true, + "options": { + "color": "#2b66a2", + "size": "normal" + } + } + ], + "rightSources": [ + { + "type": "stepped", + "logKey": "/Drive/Module0/DriveVelocityRadPerSec", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + } + ], + "discreteSources": [], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "type": null, + "factor": 1 + }, + "rightUnitConversion": { + "type": null, + "factor": 1 + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "efudylbvebbv1ga2kosknov0uegmtj7h", + "renderer": null, + "controlsHeight": 200 + } + ] + } + } + } + ], + "satellites": [], + "version": "4.0.0-beta-3" +} diff --git a/LICENSE.md b/LICENSE.md index e72bfdd..f288702 100644 --- a/LICENSE.md +++ b/LICENSE.md @@ -671,4 +671,4 @@ into proprietary programs. If your program is a subroutine library, you may consider it more useful to permit linking proprietary applications with the library. If this is what you want to do, use the GNU Lesser General Public License instead of this License. But first, please read -. \ No newline at end of file +. diff --git a/WPILib-License.md b/WPILib-License.md deleted file mode 100644 index 645e542..0000000 --- a/WPILib-License.md +++ /dev/null @@ -1,24 +0,0 @@ -Copyright (c) 2009-2024 FIRST and other WPILib contributors -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - * Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - * Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in the - documentation and/or other materials provided with the distribution. - * Neither the name of FIRST, WPILib, nor the names of other WPILib - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND -ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED -WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR -PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR -ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND -ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS -SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/build.gradle b/build.gradle index 1945af5..8c0f12a 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,8 @@ plugins { id "java" id "edu.wpi.first.GradleRIO" version "2025.2.1" + id "com.peterabeles.gversion" version "1.10" + id "com.diffplug.spotless" version "6.12.0" } java { @@ -27,14 +29,26 @@ deploy { // getTargetTypeClass is a shortcut to get the class type using a string frcJava(getArtifactTypeClass('FRCJavaArtifact')) { + jvmArgs.add("-XX:+UnlockExperimentalVMOptions") + jvmArgs.add("-XX:GCTimeRatio=5") + jvmArgs.add("-XX:+UseSerialGC") + jvmArgs.add("-XX:MaxGCPauseMillis=50") + + // The options below may improve performance, but should only be enabled on the RIO 2 + // + // final MAX_JAVA_HEAP_SIZE_MB = 100; + // jvmArgs.add("-Xmx" + MAX_JAVA_HEAP_SIZE_MB + "M") + // jvmArgs.add("-Xms" + MAX_JAVA_HEAP_SIZE_MB + "M") + // jvmArgs.add("-XX:+AlwaysPreTouch") } // 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 + // Change to true to delete files on roboRIO that no + // longer exist in deploy directory on roboRIO + deleteOldFiles = false } } } @@ -47,10 +61,16 @@ def deployArtifact = deploy.targets.roborio.artifacts.frcJava wpi.java.debugJni = false // Set this to true to enable desktop support. -def includeDesktopSupport = false +def includeDesktopSupport = true + +// Configuration for AdvantageKit +task(replayWatch, type: JavaExec) { + mainClass = "org.littletonrobotics.junction.ReplayWatch" + classpath = sourceSets.main.runtimeClasspath +} // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. -// Also defines JUnit 5. +// Also defines JUnit 4. dependencies { annotationProcessor wpi.java.deps.wpilibAnnotations() implementation wpi.java.deps.wpilib() @@ -72,6 +92,9 @@ dependencies { testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' testRuntimeOnly 'org.junit.platform:junit-platform-launcher' + + def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) + annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version" } test { @@ -80,7 +103,12 @@ test { } // Simulation configuration (e.g. environment variables). -wpi.sim.addGui().defaultEnabled = true +// +// The sim GUI is *disabled* by default to support running +// AdvantageKit log replay from the command line. Set the +// value to "true" to enable the sim GUI by default (this +// is the standard WPILib behavior). +wpi.sim.addGui().defaultEnabled = false wpi.sim.addDriverstation() // Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar') @@ -102,3 +130,89 @@ wpi.java.configureTestTasks(test) tasks.withType(JavaCompile) { options.compilerArgs.add '-XDstringConcat=inline' } + +// Create version file +project.compileJava.dependsOn(createVersionFile) +gversion { + srcDir = "src/main/java/" + classPackage = "frc.robot" + className = "BuildConstants" + dateFormat = "yyyy-MM-dd HH:mm:ss z" + timeZone = "America/New_York" + indent = " " +} + +// Create commit with working changes on event branches +task(eventDeploy) { + doLast { + if (project.gradle.startParameter.taskNames.any({ it.toLowerCase().contains("deploy") })) { + def branchPrefix = "event" + def branch = 'git branch --show-current'.execute().text.trim() + def commitMessage = "Update at '${new Date().toString()}'" + + if (branch.startsWith(branchPrefix)) { + exec { + workingDir(projectDir) + executable 'git' + args 'add', '-A' + } + exec { + workingDir(projectDir) + executable 'git' + args 'commit', '-m', commitMessage + ignoreExitValue = true + } + + println "Committed to branch: '$branch'" + println "Commit message: '$commitMessage'" + } else { + println "Not on an event branch, skipping commit" + } + } else { + println "Not running deploy task, skipping commit" + } + } +} +createVersionFile.dependsOn(eventDeploy) + +// Spotless formatting +project.compileJava.dependsOn(spotlessApply) +spotless { + java { + target fileTree(".") { + include "**/*.java" + exclude "**/build/**", "**/build-*/**" + } + toggleOffOn() + googleJavaFormat() + removeUnusedImports() + trimTrailingWhitespace() + endWithNewline() + } + groovyGradle { + target fileTree(".") { + include "**/*.gradle" + exclude "**/build/**", "**/build-*/**" + } + greclipse() + indentWithSpaces(4) + trimTrailingWhitespace() + endWithNewline() + } + json { + target fileTree(".") { + include "**/*.json" + exclude "**/build/**", "**/build-*/**" + } + gson().indentWithSpaces(2) + } + format "misc", { + target fileTree(".") { + include "**/*.md", "**/.gitignore" + exclude "**/build/**", "**/build-*/**" + } + trimTrailingWhitespace() + indentWithSpaces(2) + endWithNewline() + } +} diff --git a/gradlew b/gradlew index f5feea6..1aa94a4 100644 --- a/gradlew +++ b/gradlew @@ -15,8 +15,6 @@ # See the License for the specific language governing permissions and # limitations under the License. # -# SPDX-License-Identifier: Apache-2.0 -# ############################################################################## # @@ -57,7 +55,7 @@ # Darwin, MinGW, and NonStop. # # (3) This script is generated from the Groovy template -# https://github.com/gradle/gradle/blob/HEAD/platforms/jvm/plugins-application/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt +# https://github.com/gradle/gradle/blob/HEAD/subprojects/plugins/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt # within the Gradle project. # # You can find Gradle at https://github.com/gradle/gradle/. @@ -86,8 +84,7 @@ 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 -P "${APP_HOME:-./}" > /dev/null && printf '%s -' "$PWD" ) || exit +APP_HOME=$( cd "${APP_HOME:-./}" > /dev/null && pwd -P ) || 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 9d21a21..93e3f59 100644 --- a/gradlew.bat +++ b/gradlew.bat @@ -13,8 +13,6 @@ @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 ########################################################################## @@ -45,11 +43,11 @@ set JAVA_EXE=java.exe %JAVA_EXE% -version >NUL 2>&1 if %ERRORLEVEL% equ 0 goto execute -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 +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. goto fail @@ -59,11 +57,11 @@ set JAVA_EXE=%JAVA_HOME%/bin/java.exe if exist "%JAVA_EXE%" goto execute -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 +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. goto fail diff --git a/settings.gradle b/settings.gradle index c493958..969c7b0 100644 --- a/settings.gradle +++ b/settings.gradle @@ -20,8 +20,8 @@ pluginManagement { } def frcHomeMaven = new File(frcHome, 'maven') maven { - name = 'frcHome' - url = frcHomeMaven + name 'frcHome' + url frcHomeMaven } } } diff --git a/src/main/deploy/example.txt b/src/main/deploy/example.txt deleted file mode 100644 index bb82515..0000000 --- a/src/main/deploy/example.txt +++ /dev/null @@ -1,3 +0,0 @@ -Files placed in this directory will be deployed to the RoboRIO into the -'deploy' directory in the home folder. Use the 'Filesystem.getDeployDirectory' wpilib function -to get a proper path relative to the deploy directory. \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Example Auto.auto b/src/main/deploy/pathplanner/autos/Example Auto.auto new file mode 100644 index 0000000..70b7ab2 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Example Auto.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Example Path" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json new file mode 100644 index 0000000..bbd9194 --- /dev/null +++ b/src/main/deploy/pathplanner/navgrid.json @@ -0,0 +1,1656 @@ +{ + "field_size": { + "x": 17.548, + "y": 8.052 + }, + "nodeSizeMeters": 0.3, + "grid": [ + [ + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true + ], + [ + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true + ], + [ + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true + ], + [ + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true + ], + [ + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true + ], + [ + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true + ], + [ + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true + ], + [ + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true + ], + [ + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true + ], + [ + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true + ], + [ + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true + ], + [ + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true + ], + [ + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true + ], + [ + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true + ], + [ + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true + ], + [ + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true + ], + [ + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true + ], + [ + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true + ], + [ + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true + ], + [ + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true + ], + [ + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true + ], + [ + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true + ], + [ + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true + ], + [ + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true + ], + [ + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true + ], + [ + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true + ], + [ + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true + ] + ] +} diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path new file mode 100644 index 0000000..303dbb2 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Example Path.path @@ -0,0 +1,70 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.0, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.013282910175676, + "y": 6.5274984191074985 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.166769560390973, + "y": 5.0964860911203305 + }, + "prevControl": { + "x": 4.166769560390973, + "y": 6.0964860911203305 + }, + "nextControl": { + "x": 6.166769560390973, + "y": 4.0964860911203305 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.0, + "y": 1.0 + }, + "prevControl": { + "x": 6.75, + "y": 2.5 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json new file mode 100644 index 0000000..76ca88a --- /dev/null +++ b/src/main/deploy/pathplanner/settings.json @@ -0,0 +1,21 @@ +{ + "robotWidth": 0.9, + "robotLength": 0.9, + "holonomicMode": true, + "pathFolders": [], + "autoFolders": [], + "defaultMaxVel": 3.0, + "defaultMaxAccel": 3.0, + "defaultMaxAngVel": 540.0, + "defaultMaxAngAccel": 720.0, + "robotMass": 74.088, + "robotMOI": 6.883, + "robotWheelbase": 0.546, + "robotTrackwidth": 0.546, + "driveWheelRadius": 0.048, + "driveGearing": 5.143, + "maxDriveSpeed": 5.45, + "driveMotorType": "vortex", + "driveCurrentLimit": 60.0, + "wheelCOF": 1.2 +} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c50ba05..e4334eb 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -1,19 +1,37 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. +// Copyright 2021-2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. package frc.robot; +import edu.wpi.first.wpilibj.RobotBase; + /** - * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean - * constants. This class should not be used for any other purpose. All constants should be declared - * globally (i.e. public static). Do not put anything functional in this class. - * - *

It is advised to statically import this class (or one of its inner classes) wherever the - * constants are needed, to reduce verbosity. + * This class defines the runtime mode used by AdvantageKit. The mode is always "real" when running + * on a roboRIO. Change the value of "simMode" to switch between "sim" (physics sim) and "replay" + * (log replay from a file). */ public final class Constants { - public static class OperatorConstants { - public static final int kDriverControllerPort = 0; + public static final Mode simMode = Mode.SIM; + public static final Mode currentMode = RobotBase.isReal() ? Mode.REAL : simMode; + + public static enum Mode { + /** Running on a real robot. */ + REAL, + + /** Running a physics simulator. */ + SIM, + + /** Replaying from a log file. */ + REPLAY } } diff --git a/src/main/java/frc/robot/Main.java b/src/main/java/frc/robot/Main.java index 8776e5d..0ae5e3c 100644 --- a/src/main/java/frc/robot/Main.java +++ b/src/main/java/frc/robot/Main.java @@ -1,6 +1,15 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. +// Copyright 2021-2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. package frc.robot; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 47d3d5a..a7b2b5b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -1,64 +1,124 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. +// Copyright 2021-2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. package frc.robot; -import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.Threads; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import org.littletonrobotics.junction.LogFileUtil; +import org.littletonrobotics.junction.LoggedRobot; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.networktables.NT4Publisher; +import org.littletonrobotics.junction.wpilog.WPILOGReader; +import org.littletonrobotics.junction.wpilog.WPILOGWriter; +import org.littletonrobotics.urcl.URCL; /** - * The methods in this class are called automatically corresponding to each mode, as described in - * the TimedRobot documentation. If you change the name of this class or the package after creating - * this project, you must also update the Main.java file in the project. + * The VM is configured to automatically run this class, and to call the functions corresponding to + * each mode, as described in the TimedRobot documentation. If you change the name of this class or + * the package after creating this project, you must also update the build.gradle file in the + * project. */ -public class Robot extends TimedRobot { - private Command m_autonomousCommand; +public class Robot extends LoggedRobot { + private Command autonomousCommand; + private RobotContainer robotContainer; - private final RobotContainer m_robotContainer; - - /** - * This function is run when the robot is first started up and should be used for any - * initialization code. - */ public Robot() { - // Instantiate our RobotContainer. This will perform all our button bindings, and put our - // autonomous chooser on the dashboard. - m_robotContainer = new RobotContainer(); + // Record metadata + Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME); + Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE); + Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA); + Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE); + Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH); + switch (BuildConstants.DIRTY) { + case 0: + Logger.recordMetadata("GitDirty", "All changes committed"); + break; + case 1: + Logger.recordMetadata("GitDirty", "Uncomitted changes"); + break; + default: + Logger.recordMetadata("GitDirty", "Unknown"); + break; + } + + // Set up data receivers & replay source + switch (Constants.currentMode) { + case REAL: + // Running on a real robot, log to a USB stick ("/U/logs") + Logger.addDataReceiver(new WPILOGWriter()); + Logger.addDataReceiver(new NT4Publisher()); + break; + + case SIM: + // Running a physics simulator, log to NT + Logger.addDataReceiver(new NT4Publisher()); + break; + + case REPLAY: + // Replaying a log, set up replay source + setUseTiming(false); // Run as fast as possible + String logPath = LogFileUtil.findReplayLog(); + Logger.setReplaySource(new WPILOGReader(logPath)); + Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"))); + break; + } + + // Initialize URCL + Logger.registerURCL(URCL.startExternal()); + + // Start AdvantageKit logger + Logger.start(); + + // Instantiate our RobotContainer. This will perform all our button bindings, + // and put our autonomous chooser on the dashboard. + robotContainer = new RobotContainer(); } - /** - * This function is called every 20 ms, no matter the mode. Use this for items like diagnostics - * that you want ran during disabled, autonomous, teleoperated and test. - * - *

This runs after the mode specific periodic functions, but before LiveWindow and - * SmartDashboard integrated updating. - */ + /** This function is called periodically during all modes. */ @Override public void robotPeriodic() { - // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled - // commands, running already-scheduled commands, removing finished or interrupted commands, - // and running subsystem periodic() methods. This must be called from the robot's periodic - // block in order for anything in the Command-based framework to work. + // Switch thread to high priority to improve loop timing + Threads.setCurrentThreadPriority(true, 99); + + // Runs the Scheduler. This is responsible for polling buttons, adding + // newly-scheduled commands, running already-scheduled commands, removing + // finished or interrupted commands, and running subsystem periodic() methods. + // This must be called from the robot's periodic block in order for anything in + // the Command-based framework to work. CommandScheduler.getInstance().run(); + + // Return to normal thread priority + Threads.setCurrentThreadPriority(false, 10); } - /** This function is called once each time the robot enters Disabled mode. */ + /** This function is called once when the robot is disabled. */ @Override public void disabledInit() {} + /** This function is called periodically when disabled. */ @Override public void disabledPeriodic() {} /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ @Override public void autonomousInit() { - m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + autonomousCommand = robotContainer.getAutonomousCommand(); // schedule the autonomous command (example) - if (m_autonomousCommand != null) { - m_autonomousCommand.schedule(); + if (autonomousCommand != null) { + autonomousCommand.schedule(); } } @@ -66,14 +126,15 @@ public void autonomousInit() { @Override public void autonomousPeriodic() {} + /** This function is called once when teleop is enabled. */ @Override public void teleopInit() { // This makes sure that the autonomous stops running when // teleop starts running. If you want the autonomous to // continue until interrupted by another command, remove // this line or comment it out. - if (m_autonomousCommand != null) { - m_autonomousCommand.cancel(); + if (autonomousCommand != null) { + autonomousCommand.cancel(); } } @@ -81,6 +142,7 @@ public void teleopInit() { @Override public void teleopPeriodic() {} + /** This function is called once when test mode is enabled. */ @Override public void testInit() { // Cancels all running commands at the start of test mode. diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a33249e..7992950 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,54 +1,150 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. +// Copyright 2021-2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. package frc.robot; -import frc.robot.Constants.OperatorConstants; -import frc.robot.commands.Autos; -import frc.robot.commands.ExampleCommand; -import frc.robot.subsystems.ExampleSubsystem; +import com.pathplanner.lib.auto.AutoBuilder; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import edu.wpi.first.wpilibj2.command.button.Trigger; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.commands.DriveCommands; +import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.drive.GyroIO; +import frc.robot.subsystems.drive.GyroIOPigeon2; +import frc.robot.subsystems.drive.ModuleIO; +import frc.robot.subsystems.drive.ModuleIOSim; +import frc.robot.subsystems.drive.ModuleIOSpark; +import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; /** * This class is where the bulk of the robot should be declared. Since Command-based is a * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including - * subsystems, commands, and trigger mappings) should be declared here. + * subsystems, commands, and button mappings) should be declared here. */ public class RobotContainer { - // The robot's subsystems and commands are defined here... - private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem(); + // Subsystems + private final Drive drive; - // Replace with CommandPS4Controller or CommandJoystick if needed - private final CommandXboxController m_driverController = - new CommandXboxController(OperatorConstants.kDriverControllerPort); + // Controller + private final CommandXboxController controller = new CommandXboxController(0); + + // Dashboard inputs + private final LoggedDashboardChooser autoChooser; /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { - // Configure the trigger bindings - configureBindings(); + switch (Constants.currentMode) { + case REAL: + // Real robot, instantiate hardware IO implementations + drive = + new Drive( + new GyroIOPigeon2(), + new ModuleIOSpark(0), + new ModuleIOSpark(1), + new ModuleIOSpark(2), + new ModuleIOSpark(3)); + break; + + case SIM: + // Sim robot, instantiate physics sim IO implementations + drive = + new Drive( + new GyroIO() {}, + new ModuleIOSim(), + new ModuleIOSim(), + new ModuleIOSim(), + new ModuleIOSim()); + break; + + default: + // Replayed robot, disable IO implementations + drive = + new Drive( + new GyroIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {}); + break; + } + + // Set up auto routines + autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); + + // Set up SysId routines + autoChooser.addOption( + "Drive Wheel Radius Characterization", DriveCommands.wheelRadiusCharacterization(drive)); + autoChooser.addOption( + "Drive Simple FF Characterization", DriveCommands.feedforwardCharacterization(drive)); + autoChooser.addOption( + "Drive SysId (Quasistatic Forward)", + drive.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); + autoChooser.addOption( + "Drive SysId (Quasistatic Reverse)", + drive.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); + autoChooser.addOption( + "Drive SysId (Dynamic Forward)", drive.sysIdDynamic(SysIdRoutine.Direction.kForward)); + autoChooser.addOption( + "Drive SysId (Dynamic Reverse)", drive.sysIdDynamic(SysIdRoutine.Direction.kReverse)); + + // Configure the button bindings + configureButtonBindings(); } /** - * Use this method to define your trigger->command mappings. Triggers can be created via the - * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with an arbitrary - * predicate, or via the named factories in {@link - * edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses for {@link - * CommandXboxController Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller - * PS4} controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick Flight - * joysticks}. + * Use this method to define your button->command mappings. Buttons can be created by + * instantiating a {@link GenericHID} or one of its subclasses ({@link + * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link + * edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ - private void configureBindings() { - // Schedule `ExampleCommand` when `exampleCondition` changes to `true` - new Trigger(m_exampleSubsystem::exampleCondition) - .onTrue(new ExampleCommand(m_exampleSubsystem)); - - // Schedule `exampleMethodCommand` when the Xbox controller's B button is pressed, - // cancelling on release. - m_driverController.b().whileTrue(m_exampleSubsystem.exampleMethodCommand()); + private void configureButtonBindings() { + // Default command, normal field-relative drive + drive.setDefaultCommand( + DriveCommands.joystickDrive( + drive, + () -> -controller.getLeftY(), + () -> -controller.getLeftX(), + () -> -controller.getRightX())); + + // Lock to 0° when A button is held + controller + .a() + .whileTrue( + DriveCommands.joystickDriveAtAngle( + drive, + () -> -controller.getLeftY(), + () -> -controller.getLeftX(), + () -> new Rotation2d())); + + // Switch to X pattern when X button is pressed + controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive)); + + // Reset gyro to 0° when B button is pressed + controller + .b() + .onTrue( + Commands.runOnce( + () -> + drive.setPose( + new Pose2d(drive.getPose().getTranslation(), new Rotation2d())), + drive) + .ignoringDisable(true)); } /** @@ -57,7 +153,6 @@ private void configureBindings() { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - // An example command will be run in autonomous - return Autos.exampleAuto(m_exampleSubsystem); + return autoChooser.get(); } } diff --git a/src/main/java/frc/robot/commands/Autos.java b/src/main/java/frc/robot/commands/Autos.java deleted file mode 100644 index 107aad7..0000000 --- a/src/main/java/frc/robot/commands/Autos.java +++ /dev/null @@ -1,20 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import frc.robot.subsystems.ExampleSubsystem; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; - -public final class Autos { - /** Example static factory for an autonomous command. */ - public static Command exampleAuto(ExampleSubsystem subsystem) { - return Commands.sequence(subsystem.exampleMethodCommand(), new ExampleCommand(subsystem)); - } - - private Autos() { - throw new UnsupportedOperationException("This is a utility class!"); - } -} diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java new file mode 100644 index 0000000..0a9079d --- /dev/null +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -0,0 +1,298 @@ +// Copyright 2021-2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.commands; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.drive.DriveConstants; +import java.text.DecimalFormat; +import java.text.NumberFormat; +import java.util.LinkedList; +import java.util.List; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; + +public class DriveCommands { + private static final double DEADBAND = 0.1; + private static final double ANGLE_KP = 5.0; + private static final double ANGLE_KD = 0.4; + private static final double ANGLE_MAX_VELOCITY = 8.0; + private static final double ANGLE_MAX_ACCELERATION = 20.0; + private static final double FF_START_DELAY = 2.0; // Secs + private static final double FF_RAMP_RATE = 0.1; // Volts/Sec + private static final double WHEEL_RADIUS_MAX_VELOCITY = 0.25; // Rad/Sec + private static final double WHEEL_RADIUS_RAMP_RATE = 0.05; // Rad/Sec^2 + + private DriveCommands() {} + + private static Translation2d getLinearVelocityFromJoysticks(double x, double y) { + // Apply deadband + double linearMagnitude = MathUtil.applyDeadband(Math.hypot(x, y), DEADBAND); + Rotation2d linearDirection = new Rotation2d(Math.atan2(y, x)); + + // Square magnitude for more precise control + linearMagnitude = linearMagnitude * linearMagnitude; + + // Return new linear velocity + return new Pose2d(new Translation2d(), linearDirection) + .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())) + .getTranslation(); + } + + /** + * Field relative drive command using two joysticks (controlling linear and angular velocities). + */ + public static Command joystickDrive( + Drive drive, + DoubleSupplier xSupplier, + DoubleSupplier ySupplier, + DoubleSupplier omegaSupplier) { + return Commands.run( + () -> { + // Get linear velocity + Translation2d linearVelocity = + getLinearVelocityFromJoysticks(xSupplier.getAsDouble(), ySupplier.getAsDouble()); + + // Apply rotation deadband + double omega = MathUtil.applyDeadband(omegaSupplier.getAsDouble(), DEADBAND); + + // Square rotation value for more precise control + omega = Math.copySign(omega * omega, omega); + + // Convert to field relative speeds & send command + ChassisSpeeds speeds = + new ChassisSpeeds( + linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(), + linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(), + omega * drive.getMaxAngularSpeedRadPerSec()); + boolean isFlipped = + DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == Alliance.Red; + drive.runVelocity( + ChassisSpeeds.fromFieldRelativeSpeeds( + speeds, + isFlipped + ? drive.getRotation().plus(new Rotation2d(Math.PI)) + : drive.getRotation())); + }, + drive); + } + + /** + * Field relative drive command using joystick for linear control and PID for angular control. + * Possible use cases include snapping to an angle, aiming at a vision target, or controlling + * absolute rotation with a joystick. + */ + public static Command joystickDriveAtAngle( + Drive drive, + DoubleSupplier xSupplier, + DoubleSupplier ySupplier, + Supplier rotationSupplier) { + + // Create PID controller + ProfiledPIDController angleController = + new ProfiledPIDController( + ANGLE_KP, + 0.0, + ANGLE_KD, + new TrapezoidProfile.Constraints(ANGLE_MAX_VELOCITY, ANGLE_MAX_ACCELERATION)); + angleController.enableContinuousInput(-Math.PI, Math.PI); + + // Construct command + return Commands.run( + () -> { + // Get linear velocity + Translation2d linearVelocity = + getLinearVelocityFromJoysticks(xSupplier.getAsDouble(), ySupplier.getAsDouble()); + + // Calculate angular speed + double omega = + angleController.calculate( + drive.getRotation().getRadians(), rotationSupplier.get().getRadians()); + + // Convert to field relative speeds & send command + ChassisSpeeds speeds = + new ChassisSpeeds( + linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(), + linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(), + omega); + boolean isFlipped = + DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == Alliance.Red; + drive.runVelocity( + ChassisSpeeds.fromFieldRelativeSpeeds( + speeds, + isFlipped + ? drive.getRotation().plus(new Rotation2d(Math.PI)) + : drive.getRotation())); + }, + drive) + + // Reset PID controller when command starts + .beforeStarting(() -> angleController.reset(drive.getRotation().getRadians())); + } + + /** + * Measures the velocity feedforward constants for the drive motors. + * + *

This command should only be used in voltage control mode. + */ + public static Command feedforwardCharacterization(Drive drive) { + List velocitySamples = new LinkedList<>(); + List voltageSamples = new LinkedList<>(); + Timer timer = new Timer(); + + return Commands.sequence( + // Reset data + Commands.runOnce( + () -> { + velocitySamples.clear(); + voltageSamples.clear(); + }), + + // Allow modules to orient + Commands.run( + () -> { + drive.runCharacterization(0.0); + }, + drive) + .withTimeout(FF_START_DELAY), + + // Start timer + Commands.runOnce(timer::restart), + + // Accelerate and gather data + Commands.run( + () -> { + double voltage = timer.get() * FF_RAMP_RATE; + drive.runCharacterization(voltage); + velocitySamples.add(drive.getFFCharacterizationVelocity()); + voltageSamples.add(voltage); + }, + drive) + + // When cancelled, calculate and print results + .finallyDo( + () -> { + int n = velocitySamples.size(); + double sumX = 0.0; + double sumY = 0.0; + double sumXY = 0.0; + double sumX2 = 0.0; + for (int i = 0; i < n; i++) { + sumX += velocitySamples.get(i); + sumY += voltageSamples.get(i); + sumXY += velocitySamples.get(i) * voltageSamples.get(i); + sumX2 += velocitySamples.get(i) * velocitySamples.get(i); + } + double kS = (sumY * sumX2 - sumX * sumXY) / (n * sumX2 - sumX * sumX); + double kV = (n * sumXY - sumX * sumY) / (n * sumX2 - sumX * sumX); + + NumberFormat formatter = new DecimalFormat("#0.00000"); + System.out.println("********** Drive FF Characterization Results **********"); + System.out.println("\tkS: " + formatter.format(kS)); + System.out.println("\tkV: " + formatter.format(kV)); + })); + } + + /** Measures the robot's wheel radius by spinning in a circle. */ + public static Command wheelRadiusCharacterization(Drive drive) { + SlewRateLimiter limiter = new SlewRateLimiter(WHEEL_RADIUS_RAMP_RATE); + WheelRadiusCharacterizationState state = new WheelRadiusCharacterizationState(); + + return Commands.parallel( + // Drive control sequence + Commands.sequence( + // Reset acceleration limiter + Commands.runOnce( + () -> { + limiter.reset(0.0); + }), + + // Turn in place, accelerating up to full speed + Commands.run( + () -> { + double speed = limiter.calculate(WHEEL_RADIUS_MAX_VELOCITY); + drive.runVelocity(new ChassisSpeeds(0.0, 0.0, speed)); + }, + drive)), + + // Measurement sequence + Commands.sequence( + // Wait for modules to fully orient before starting measurement + Commands.waitSeconds(1.0), + + // Record starting measurement + Commands.runOnce( + () -> { + state.positions = drive.getWheelRadiusCharacterizationPositions(); + state.lastAngle = drive.getRotation(); + state.gyroDelta = 0.0; + }), + + // Update gyro delta + Commands.run( + () -> { + var rotation = drive.getRotation(); + state.gyroDelta += Math.abs(rotation.minus(state.lastAngle).getRadians()); + state.lastAngle = rotation; + }) + + // When cancelled, calculate and print results + .finallyDo( + () -> { + double[] positions = drive.getWheelRadiusCharacterizationPositions(); + double wheelDelta = 0.0; + for (int i = 0; i < 4; i++) { + wheelDelta += Math.abs(positions[i] - state.positions[i]) / 4.0; + } + double wheelRadius = + (state.gyroDelta * DriveConstants.driveBaseRadius) / wheelDelta; + + NumberFormat formatter = new DecimalFormat("#0.000"); + System.out.println( + "********** Wheel Radius Characterization Results **********"); + System.out.println( + "\tWheel Delta: " + formatter.format(wheelDelta) + " radians"); + System.out.println( + "\tGyro Delta: " + formatter.format(state.gyroDelta) + " radians"); + System.out.println( + "\tWheel Radius: " + + formatter.format(wheelRadius) + + " meters, " + + formatter.format(Units.metersToInches(wheelRadius)) + + " inches"); + }))); + } + + private static class WheelRadiusCharacterizationState { + double[] positions = new double[4]; + Rotation2d lastAngle = new Rotation2d(); + double gyroDelta = 0.0; + } +} diff --git a/src/main/java/frc/robot/commands/ExampleCommand.java b/src/main/java/frc/robot/commands/ExampleCommand.java deleted file mode 100644 index 7481d3c..0000000 --- a/src/main/java/frc/robot/commands/ExampleCommand.java +++ /dev/null @@ -1,43 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import frc.robot.subsystems.ExampleSubsystem; -import edu.wpi.first.wpilibj2.command.Command; - -/** An example command that uses an example subsystem. */ -public class ExampleCommand extends Command { - @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) - private final ExampleSubsystem m_subsystem; - - /** - * Creates a new ExampleCommand. - * - * @param subsystem The subsystem used by this command. - */ - public ExampleCommand(ExampleSubsystem subsystem) { - m_subsystem = subsystem; - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(subsystem); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() {} - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() {} - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc/robot/subsystems/ExampleSubsystem.java b/src/main/java/frc/robot/subsystems/ExampleSubsystem.java deleted file mode 100644 index 6b375da..0000000 --- a/src/main/java/frc/robot/subsystems/ExampleSubsystem.java +++ /dev/null @@ -1,47 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems; - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class ExampleSubsystem extends SubsystemBase { - /** Creates a new ExampleSubsystem. */ - public ExampleSubsystem() {} - - /** - * Example command factory method. - * - * @return a command - */ - public Command exampleMethodCommand() { - // Inline construction of command goes here. - // Subsystem::RunOnce implicitly requires `this` subsystem. - return runOnce( - () -> { - /* one-time action goes here */ - }); - } - - /** - * An example method querying a boolean state of the subsystem (for example, a digital sensor). - * - * @return value of some boolean subsystem state, such as a digital sensor. - */ - public boolean exampleCondition() { - // Query some boolean state, such as a digital sensor. - return false; - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - } - - @Override - public void simulationPeriodic() { - // This method will be called once per scheduler run during simulation - } -} diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java new file mode 100644 index 0000000..60b0076 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -0,0 +1,323 @@ +// Copyright 2021-2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.subsystems.drive; + +import static edu.wpi.first.units.Units.*; +import static frc.robot.subsystems.drive.DriveConstants.*; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; +import com.pathplanner.lib.pathfinding.Pathfinding; +import com.pathplanner.lib.util.PathPlannerLogging; +import edu.wpi.first.hal.FRCNetComm.tInstances; +import edu.wpi.first.hal.FRCNetComm.tResourceType; +import edu.wpi.first.hal.HAL; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +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.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.Constants; +import frc.robot.Constants.Mode; +import frc.robot.util.LocalADStarAK; +import java.util.concurrent.locks.Lock; +import java.util.concurrent.locks.ReentrantLock; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + +public class Drive extends SubsystemBase { + static final Lock odometryLock = new ReentrantLock(); + private final GyroIO gyroIO; + private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); + private final Module[] modules = new Module[4]; // FL, FR, BL, BR + private final SysIdRoutine sysId; + private final Alert gyroDisconnectedAlert = + new Alert("Disconnected gyro, using kinematics as fallback.", AlertType.kError); + + private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(moduleTranslations); + private Rotation2d rawGyroRotation = new Rotation2d(); + private SwerveModulePosition[] lastModulePositions = // For delta tracking + new SwerveModulePosition[] { + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition() + }; + private SwerveDrivePoseEstimator poseEstimator = + new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, new Pose2d()); + + public Drive( + GyroIO gyroIO, + ModuleIO flModuleIO, + ModuleIO frModuleIO, + ModuleIO blModuleIO, + ModuleIO brModuleIO) { + this.gyroIO = gyroIO; + modules[0] = new Module(flModuleIO, 0); + modules[1] = new Module(frModuleIO, 1); + modules[2] = new Module(blModuleIO, 2); + modules[3] = new Module(brModuleIO, 3); + + // Usage reporting for swerve template + HAL.report(tResourceType.kResourceType_RobotDrive, tInstances.kRobotDriveSwerve_AdvantageKit); + + // Start odometry thread + SparkOdometryThread.getInstance().start(); + + // Configure AutoBuilder for PathPlanner + AutoBuilder.configure( + this::getPose, + this::setPose, + this::getChassisSpeeds, + this::runVelocity, + new PPHolonomicDriveController( + new PIDConstants(5.0, 0.0, 0.0), new PIDConstants(5.0, 0.0, 0.0)), + ppConfig, + () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, + this); + Pathfinding.setPathfinder(new LocalADStarAK()); + PathPlannerLogging.setLogActivePathCallback( + (activePath) -> { + Logger.recordOutput( + "Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()])); + }); + PathPlannerLogging.setLogTargetPoseCallback( + (targetPose) -> { + Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose); + }); + + // Configure SysId + sysId = + new SysIdRoutine( + new SysIdRoutine.Config( + null, + null, + null, + (state) -> Logger.recordOutput("Drive/SysIdState", state.toString())), + new SysIdRoutine.Mechanism( + (voltage) -> runCharacterization(voltage.in(Volts)), null, this)); + } + + @Override + public void periodic() { + odometryLock.lock(); // Prevents odometry updates while reading data + gyroIO.updateInputs(gyroInputs); + Logger.processInputs("Drive/Gyro", gyroInputs); + for (var module : modules) { + module.periodic(); + } + odometryLock.unlock(); + + // Stop moving when disabled + if (DriverStation.isDisabled()) { + for (var module : modules) { + module.stop(); + } + } + + // Log empty setpoint states when disabled + if (DriverStation.isDisabled()) { + Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {}); + Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {}); + } + + // Update odometry + double[] sampleTimestamps = + modules[0].getOdometryTimestamps(); // All signals are sampled together + int sampleCount = sampleTimestamps.length; + for (int i = 0; i < sampleCount; i++) { + // Read wheel positions and deltas from each module + SwerveModulePosition[] modulePositions = new SwerveModulePosition[4]; + SwerveModulePosition[] moduleDeltas = new SwerveModulePosition[4]; + for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) { + modulePositions[moduleIndex] = modules[moduleIndex].getOdometryPositions()[i]; + moduleDeltas[moduleIndex] = + new SwerveModulePosition( + modulePositions[moduleIndex].distanceMeters + - lastModulePositions[moduleIndex].distanceMeters, + modulePositions[moduleIndex].angle); + lastModulePositions[moduleIndex] = modulePositions[moduleIndex]; + } + + // Update gyro angle + if (gyroInputs.connected) { + // Use the real gyro angle + rawGyroRotation = gyroInputs.odometryYawPositions[i]; + } else { + // Use the angle delta from the kinematics and module deltas + Twist2d twist = kinematics.toTwist2d(moduleDeltas); + rawGyroRotation = rawGyroRotation.plus(new Rotation2d(twist.dtheta)); + } + + // Apply update + poseEstimator.updateWithTime(sampleTimestamps[i], rawGyroRotation, modulePositions); + } + + // Update gyro alert + gyroDisconnectedAlert.set(!gyroInputs.connected && Constants.currentMode != Mode.SIM); + } + + /** + * Runs the drive at the desired velocity. + * + * @param speeds Speeds in meters/sec + */ + public void runVelocity(ChassisSpeeds speeds) { + // Calculate module setpoints + ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02); + SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds); + SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, maxSpeedMetersPerSec); + + // Log unoptimized setpoints + Logger.recordOutput("SwerveStates/Setpoints", setpointStates); + Logger.recordOutput("SwerveChassisSpeeds/Setpoints", discreteSpeeds); + + // Send setpoints to modules + for (int i = 0; i < 4; i++) { + modules[i].runSetpoint(setpointStates[i]); + } + + // Log optimized setpoints (runSetpoint mutates each state) + Logger.recordOutput("SwerveStates/SetpointsOptimized", setpointStates); + } + + /** Runs the drive in a straight line with the specified drive output. */ + public void runCharacterization(double output) { + for (int i = 0; i < 4; i++) { + modules[i].runCharacterization(output); + } + } + + /** Stops the drive. */ + public void stop() { + runVelocity(new ChassisSpeeds()); + } + + /** + * Stops the drive and turns the modules to an X arrangement to resist movement. The modules will + * return to their normal orientations the next time a nonzero velocity is requested. + */ + public void stopWithX() { + Rotation2d[] headings = new Rotation2d[4]; + for (int i = 0; i < 4; i++) { + headings[i] = moduleTranslations[i].getAngle(); + } + kinematics.resetHeadings(headings); + stop(); + } + + /** Returns a command to run a quasistatic test in the specified direction. */ + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return run(() -> runCharacterization(0.0)) + .withTimeout(1.0) + .andThen(sysId.quasistatic(direction)); + } + + /** Returns a command to run a dynamic test in the specified direction. */ + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return run(() -> runCharacterization(0.0)).withTimeout(1.0).andThen(sysId.dynamic(direction)); + } + + /** Returns the module states (turn angles and drive velocities) for all of the modules. */ + @AutoLogOutput(key = "SwerveStates/Measured") + private SwerveModuleState[] getModuleStates() { + SwerveModuleState[] states = new SwerveModuleState[4]; + for (int i = 0; i < 4; i++) { + states[i] = modules[i].getState(); + } + return states; + } + + /** Returns the module positions (turn angles and drive positions) for all of the modules. */ + private SwerveModulePosition[] getModulePositions() { + SwerveModulePosition[] states = new SwerveModulePosition[4]; + for (int i = 0; i < 4; i++) { + states[i] = modules[i].getPosition(); + } + return states; + } + + /** Returns the measured chassis speeds of the robot. */ + @AutoLogOutput(key = "SwerveChassisSpeeds/Measured") + private ChassisSpeeds getChassisSpeeds() { + return kinematics.toChassisSpeeds(getModuleStates()); + } + + /** Returns the position of each module in radians. */ + public double[] getWheelRadiusCharacterizationPositions() { + double[] values = new double[4]; + for (int i = 0; i < 4; i++) { + values[i] = modules[i].getWheelRadiusCharacterizationPosition(); + } + return values; + } + + /** Returns the average velocity of the modules in rad/sec. */ + public double getFFCharacterizationVelocity() { + double output = 0.0; + for (int i = 0; i < 4; i++) { + output += modules[i].getFFCharacterizationVelocity() / 4.0; + } + return output; + } + + /** Returns the current odometry pose. */ + @AutoLogOutput(key = "Odometry/Robot") + public Pose2d getPose() { + return poseEstimator.getEstimatedPosition(); + } + + /** Returns the current odometry rotation. */ + public Rotation2d getRotation() { + return getPose().getRotation(); + } + + /** Resets the current odometry pose. */ + public void setPose(Pose2d pose) { + poseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose); + } + + /** Adds a new timestamped vision measurement. */ + public void addVisionMeasurement( + Pose2d visionRobotPoseMeters, + double timestampSeconds, + Matrix visionMeasurementStdDevs) { + poseEstimator.addVisionMeasurement( + visionRobotPoseMeters, timestampSeconds, visionMeasurementStdDevs); + } + + /** Returns the maximum linear speed in meters per sec. */ + public double getMaxLinearSpeedMetersPerSec() { + return maxSpeedMetersPerSec; + } + + /** Returns the maximum angular speed in radians per sec. */ + public double getMaxAngularSpeedRadPerSec() { + return maxSpeedMetersPerSec / driveBaseRadius; + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java new file mode 100644 index 0000000..f5e9abd --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -0,0 +1,114 @@ +// Copyright 2021-2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.subsystems.drive; + +import com.pathplanner.lib.config.ModuleConfig; +import com.pathplanner.lib.config.RobotConfig; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.util.Units; + +public class DriveConstants { + public static final double maxSpeedMetersPerSec = 4.8; + public static final double odometryFrequency = 100.0; // Hz + public static final double trackWidth = Units.inchesToMeters(26.5); + public static final double wheelBase = Units.inchesToMeters(26.5); + public static final double driveBaseRadius = Math.hypot(trackWidth / 2.0, wheelBase / 2.0); + public static final Translation2d[] moduleTranslations = + new Translation2d[] { + new Translation2d(trackWidth / 2.0, wheelBase / 2.0), + new Translation2d(trackWidth / 2.0, -wheelBase / 2.0), + new Translation2d(-trackWidth / 2.0, wheelBase / 2.0), + new Translation2d(-trackWidth / 2.0, -wheelBase / 2.0) + }; + + // Zeroed rotation values for each module, see setup instructions + public static final Rotation2d frontLeftZeroRotation = new Rotation2d(0.0); + public static final Rotation2d frontRightZeroRotation = new Rotation2d(0.0); + public static final Rotation2d backLeftZeroRotation = new Rotation2d(0.0); + public static final Rotation2d backRightZeroRotation = new Rotation2d(0.0); + + // Device CAN IDs + public static final int pigeonCanId = 9; + + public static final int frontLeftDriveCanId = 1; + public static final int backLeftDriveCanId = 3; + public static final int frontRightDriveCanId = 5; + public static final int backRightDriveCanId = 7; + + public static final int frontLeftTurnCanId = 2; + public static final int backLeftTurnCanId = 4; + public static final int frontRightTurnCanId = 6; + public static final int backRightTurnCanId = 8; + + // Drive motor configuration + public static final int driveMotorCurrentLimit = 50; + public static final double wheelRadiusMeters = Units.inchesToMeters(1.5); + public static final double driveMotorReduction = + (45.0 * 22.0) / (14.0 * 15.0); // MAXSwerve with 14 pinion teeth and 22 spur teeth + public static final DCMotor driveGearbox = DCMotor.getNeoVortex(1); + + // Drive encoder configuration + public static final double driveEncoderPositionFactor = + 2 * Math.PI / driveMotorReduction; // Rotor Rotations -> Wheel Radians + public static final double driveEncoderVelocityFactor = + (2 * Math.PI) / 60.0 / driveMotorReduction; // Rotor RPM -> Wheel Rad/Sec + + // Drive PID configuration + public static final double driveKp = 0.0; + public static final double driveKd = 0.0; + public static final double driveKs = 0.0; + public static final double driveKv = 0.1; + public static final double driveSimP = 0.05; + public static final double driveSimD = 0.0; + public static final double driveSimKs = 0.0; + public static final double driveSimKv = 0.0789; + + // Turn motor configuration + public static final boolean turnInverted = false; + public static final int turnMotorCurrentLimit = 20; + public static final double turnMotorReduction = 9424.0 / 203.0; + public static final DCMotor turnGearbox = DCMotor.getNeo550(1); + + // Turn encoder configuration + public static final boolean turnEncoderInverted = true; + public static final double turnEncoderPositionFactor = 2 * Math.PI; // Rotations -> Radians + public static final double turnEncoderVelocityFactor = (2 * Math.PI) / 60.0; // RPM -> Rad/Sec + + // Turn PID configuration + public static final double turnKp = 2.0; + public static final double turnKd = 0.0; + public static final double turnSimP = 8.0; + public static final double turnSimD = 0.0; + public static final double turnPIDMinInput = 0; // Radians + public static final double turnPIDMaxInput = 2 * Math.PI; // Radians + + // PathPlanner configuration + public static final double robotMassKg = 74.088; + public static final double robotMOI = 6.883; + public static final double wheelCOF = 1.2; + public static final RobotConfig ppConfig = + new RobotConfig( + robotMassKg, + robotMOI, + new ModuleConfig( + wheelRadiusMeters, + maxSpeedMetersPerSec, + wheelCOF, + driveGearbox.withReduction(driveMotorReduction), + driveMotorCurrentLimit, + 1), + moduleTranslations); +} diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIO.java b/src/main/java/frc/robot/subsystems/drive/GyroIO.java new file mode 100644 index 0000000..06d39db --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/GyroIO.java @@ -0,0 +1,30 @@ +// Copyright 2021-2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.subsystems.drive; + +import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.AutoLog; + +public interface GyroIO { + @AutoLog + public static class GyroIOInputs { + public boolean connected = false; + public Rotation2d yawPosition = new Rotation2d(); + public double yawVelocityRadPerSec = 0.0; + public double[] odometryYawTimestamps = new double[] {}; + public Rotation2d[] odometryYawPositions = new Rotation2d[] {}; + } + + public default void updateInputs(GyroIOInputs inputs) {} +} diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOCanandgyro.java b/src/main/java/frc/robot/subsystems/drive/GyroIOCanandgyro.java new file mode 100644 index 0000000..b82374a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOCanandgyro.java @@ -0,0 +1,52 @@ +// Copyright 2021-2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.subsystems.drive; + +//import static frc.robot.subsystems.drive.DriveConstants.*; + +import com.reduxrobotics.sensors.canandgyro.Canandgyro; +//import com.studica.frc.AHRS; +//import com.studica.frc.AHRS.NavXComType; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import java.util.Queue; + +/** IO implementation for NavX. */ +public class GyroIOCanandgyro implements GyroIO { + //private final AHRS navX = new AHRS(NavXComType.kMXP_SPI, (byte) odometryFrequency); + private final Queue yawPositionQueue; + private final Queue yawTimestampQueue; + private final Canandgyro canandgyro = new Canandgyro(9); + + public GyroIOCanandgyro() { + yawTimestampQueue = SparkOdometryThread.getInstance().makeTimestampQueue(); + yawPositionQueue = SparkOdometryThread.getInstance().registerSignal(canandgyro::getYaw); + } + + @Override + public void updateInputs(GyroIOInputs inputs) { + inputs.connected = canandgyro.isConnected(); + inputs.yawPosition = canandgyro.getRotation2d(); + inputs.yawVelocityRadPerSec = Units.degreesToRadians(2*canandgyro.getYaw()); + + inputs.odometryYawTimestamps = + yawTimestampQueue.stream().mapToDouble((Double value) -> value).toArray(); + inputs.odometryYawPositions = + yawPositionQueue.stream() + .map((Double value) -> Rotation2d.fromDegrees(-value)) + .toArray(Rotation2d[]::new); + yawTimestampQueue.clear(); + yawPositionQueue.clear(); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java b/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java new file mode 100644 index 0000000..068bb1e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java @@ -0,0 +1,50 @@ +// Copyright 2021-2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.subsystems.drive; + +import static frc.robot.subsystems.drive.DriveConstants.*; + +import com.studica.frc.AHRS; +import com.studica.frc.AHRS.NavXComType; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import java.util.Queue; + +/** IO implementation for NavX. */ +public class GyroIONavX implements GyroIO { + private final AHRS navX = new AHRS(NavXComType.kMXP_SPI, (byte) odometryFrequency); + private final Queue yawPositionQueue; + private final Queue yawTimestampQueue; + + public GyroIONavX() { + yawTimestampQueue = SparkOdometryThread.getInstance().makeTimestampQueue(); + yawPositionQueue = SparkOdometryThread.getInstance().registerSignal(navX::getAngle); + } + + @Override + public void updateInputs(GyroIOInputs inputs) { + inputs.connected = navX.isConnected(); + inputs.yawPosition = Rotation2d.fromDegrees(-navX.getAngle()); + inputs.yawVelocityRadPerSec = Units.degreesToRadians(-navX.getRawGyroZ()); + + inputs.odometryYawTimestamps = + yawTimestampQueue.stream().mapToDouble((Double value) -> value).toArray(); + inputs.odometryYawPositions = + yawPositionQueue.stream() + .map((Double value) -> Rotation2d.fromDegrees(-value)) + .toArray(Rotation2d[]::new); + yawTimestampQueue.clear(); + yawPositionQueue.clear(); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java new file mode 100644 index 0000000..e1fae3f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -0,0 +1,62 @@ +// Copyright 2021-2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.subsystems.drive; + +import static frc.robot.subsystems.drive.DriveConstants.*; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.Pigeon2Configuration; +import com.ctre.phoenix6.hardware.Pigeon2; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import java.util.Queue; + +/** IO implementation for Pigeon 2. */ +public class GyroIOPigeon2 implements GyroIO { + private final Pigeon2 pigeon = new Pigeon2(pigeonCanId); + private final StatusSignal yaw = pigeon.getYaw(); + private final Queue yawPositionQueue; + private final Queue yawTimestampQueue; + private final StatusSignal yawVelocity = pigeon.getAngularVelocityZWorld(); + + public GyroIOPigeon2() { + pigeon.getConfigurator().apply(new Pigeon2Configuration()); + pigeon.getConfigurator().setYaw(0.0); + yaw.setUpdateFrequency(odometryFrequency); + yawVelocity.setUpdateFrequency(50.0); + pigeon.optimizeBusUtilization(); + yawTimestampQueue = SparkOdometryThread.getInstance().makeTimestampQueue(); + yawPositionQueue = SparkOdometryThread.getInstance().registerSignal(yaw::getValueAsDouble); + } + + @Override + public void updateInputs(GyroIOInputs inputs) { + inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK); + inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble()); + inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble()); + + inputs.odometryYawTimestamps = + yawTimestampQueue.stream().mapToDouble((Double value) -> value).toArray(); + inputs.odometryYawPositions = + yawPositionQueue.stream() + .map((Double value) -> Rotation2d.fromDegrees(value)) + .toArray(Rotation2d[]::new); + yawTimestampQueue.clear(); + yawPositionQueue.clear(); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java new file mode 100644 index 0000000..24b19f4 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -0,0 +1,131 @@ +// Copyright 2021-2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.subsystems.drive; + +import static frc.robot.subsystems.drive.DriveConstants.*; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; +import org.littletonrobotics.junction.Logger; + +public class Module { + private final ModuleIO io; + private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); + private final int index; + + private final Alert driveDisconnectedAlert; + private final Alert turnDisconnectedAlert; + private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[] {}; + + public Module(ModuleIO io, int index) { + this.io = io; + this.index = index; + driveDisconnectedAlert = + new Alert( + "Disconnected drive motor on module " + Integer.toString(index) + ".", + AlertType.kError); + turnDisconnectedAlert = + new Alert( + "Disconnected turn motor on module " + Integer.toString(index) + ".", AlertType.kError); + } + + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Drive/Module" + Integer.toString(index), inputs); + + // Calculate positions for odometry + int sampleCount = inputs.odometryTimestamps.length; // All signals are sampled together + odometryPositions = new SwerveModulePosition[sampleCount]; + for (int i = 0; i < sampleCount; i++) { + double positionMeters = inputs.odometryDrivePositionsRad[i] * wheelRadiusMeters; + Rotation2d angle = inputs.odometryTurnPositions[i]; + odometryPositions[i] = new SwerveModulePosition(positionMeters, angle); + } + + // Update alerts + driveDisconnectedAlert.set(!inputs.driveConnected); + turnDisconnectedAlert.set(!inputs.turnConnected); + } + + /** Runs the module with the specified setpoint state. Mutates the state to optimize it. */ + public void runSetpoint(SwerveModuleState state) { + // Optimize velocity setpoint + state.optimize(getAngle()); + state.cosineScale(inputs.turnPosition); + + // Apply setpoints + io.setDriveVelocity(state.speedMetersPerSecond / wheelRadiusMeters); + io.setTurnPosition(state.angle); + } + + /** Runs the module with the specified output while controlling to zero degrees. */ + public void runCharacterization(double output) { + io.setDriveOpenLoop(output); + io.setTurnPosition(new Rotation2d()); + } + + /** Disables all outputs to motors. */ + public void stop() { + io.setDriveOpenLoop(0.0); + io.setTurnOpenLoop(0.0); + } + + /** Returns the current turn angle of the module. */ + public Rotation2d getAngle() { + return inputs.turnPosition; + } + + /** Returns the current drive position of the module in meters. */ + public double getPositionMeters() { + return inputs.drivePositionRad * wheelRadiusMeters; + } + + /** Returns the current drive velocity of the module in meters per second. */ + public double getVelocityMetersPerSec() { + return inputs.driveVelocityRadPerSec * wheelRadiusMeters; + } + + /** Returns the module position (turn angle and drive position). */ + public SwerveModulePosition getPosition() { + return new SwerveModulePosition(getPositionMeters(), getAngle()); + } + + /** Returns the module state (turn angle and drive velocity). */ + public SwerveModuleState getState() { + return new SwerveModuleState(getVelocityMetersPerSec(), getAngle()); + } + + /** Returns the module positions received this cycle. */ + public SwerveModulePosition[] getOdometryPositions() { + return odometryPositions; + } + + /** Returns the timestamps of the samples received this cycle. */ + public double[] getOdometryTimestamps() { + return inputs.odometryTimestamps; + } + + /** Returns the module position in radians. */ + public double getWheelRadiusCharacterizationPosition() { + return inputs.drivePositionRad; + } + + /** Returns the module velocity in rad/sec. */ + public double getFFCharacterizationVelocity() { + return inputs.driveVelocityRadPerSec; + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java new file mode 100644 index 0000000..424a1be --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java @@ -0,0 +1,53 @@ +// Copyright 2021-2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.subsystems.drive; + +import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.AutoLog; + +public interface ModuleIO { + @AutoLog + public static class ModuleIOInputs { + public boolean driveConnected = false; + public double drivePositionRad = 0.0; + public double driveVelocityRadPerSec = 0.0; + public double driveAppliedVolts = 0.0; + public double driveCurrentAmps = 0.0; + + public boolean turnConnected = false; + public Rotation2d turnPosition = new Rotation2d(); + public double turnVelocityRadPerSec = 0.0; + public double turnAppliedVolts = 0.0; + public double turnCurrentAmps = 0.0; + + public double[] odometryTimestamps = new double[] {}; + public double[] odometryDrivePositionsRad = new double[] {}; + public Rotation2d[] odometryTurnPositions = new Rotation2d[] {}; + } + + /** Updates the set of loggable inputs. */ + public default void updateInputs(ModuleIOInputs inputs) {} + + /** Run the drive motor at the specified open loop value. */ + public default void setDriveOpenLoop(double output) {} + + /** Run the turn motor at the specified open loop value. */ + public default void setTurnOpenLoop(double output) {} + + /** Run the drive motor at the specified velocity. */ + public default void setDriveVelocity(double velocityRadPerSec) {} + + /** Run the turn motor to the specified rotation. */ + public default void setTurnPosition(Rotation2d rotation) {} +} diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java new file mode 100644 index 0000000..f069b5a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java @@ -0,0 +1,118 @@ +// Copyright 2021-2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.subsystems.drive; + +import static frc.robot.subsystems.drive.DriveConstants.*; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; + +/** Physics sim implementation of module IO. */ +public class ModuleIOSim implements ModuleIO { + private final DCMotorSim driveSim; + private final DCMotorSim turnSim; + + private boolean driveClosedLoop = false; + private boolean turnClosedLoop = false; + private PIDController driveController = new PIDController(driveSimP, 0, driveSimD); + private PIDController turnController = new PIDController(turnSimP, 0, turnSimD); + private double driveFFVolts = 0.0; + private double driveAppliedVolts = 0.0; + private double turnAppliedVolts = 0.0; + + public ModuleIOSim() { + // Create drive and turn sim models + driveSim = + new DCMotorSim( + LinearSystemId.createDCMotorSystem(driveGearbox, 0.025, driveMotorReduction), + driveGearbox); + turnSim = + new DCMotorSim( + LinearSystemId.createDCMotorSystem(turnGearbox, 0.004, turnMotorReduction), + turnGearbox); + + // Enable wrapping for turn PID + turnController.enableContinuousInput(-Math.PI, Math.PI); + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + // Run closed-loop control + if (driveClosedLoop) { + driveAppliedVolts = + driveFFVolts + driveController.calculate(driveSim.getAngularVelocityRadPerSec()); + } else { + driveController.reset(); + } + if (turnClosedLoop) { + turnAppliedVolts = turnController.calculate(turnSim.getAngularPositionRad()); + } else { + turnController.reset(); + } + + // Update simulation state + driveSim.setInputVoltage(MathUtil.clamp(driveAppliedVolts, -12.0, 12.0)); + turnSim.setInputVoltage(MathUtil.clamp(turnAppliedVolts, -12.0, 12.0)); + driveSim.update(0.02); + turnSim.update(0.02); + + // Update drive inputs + inputs.driveConnected = true; + inputs.drivePositionRad = driveSim.getAngularPositionRad(); + inputs.driveVelocityRadPerSec = driveSim.getAngularVelocityRadPerSec(); + inputs.driveAppliedVolts = driveAppliedVolts; + inputs.driveCurrentAmps = Math.abs(driveSim.getCurrentDrawAmps()); + + // Update turn inputs + inputs.turnConnected = true; + inputs.turnPosition = new Rotation2d(turnSim.getAngularPositionRad()); + inputs.turnVelocityRadPerSec = turnSim.getAngularVelocityRadPerSec(); + inputs.turnAppliedVolts = turnAppliedVolts; + inputs.turnCurrentAmps = Math.abs(turnSim.getCurrentDrawAmps()); + + // Update odometry inputs (50Hz because high-frequency odometry in sim doesn't matter) + inputs.odometryTimestamps = new double[] {Timer.getFPGATimestamp()}; + inputs.odometryDrivePositionsRad = new double[] {inputs.drivePositionRad}; + inputs.odometryTurnPositions = new Rotation2d[] {inputs.turnPosition}; + } + + @Override + public void setDriveOpenLoop(double output) { + driveClosedLoop = false; + driveAppliedVolts = output; + } + + @Override + public void setTurnOpenLoop(double output) { + turnClosedLoop = false; + turnAppliedVolts = output; + } + + @Override + public void setDriveVelocity(double velocityRadPerSec) { + driveClosedLoop = true; + driveFFVolts = driveSimKs * Math.signum(velocityRadPerSec) + driveSimKv * velocityRadPerSec; + driveController.setSetpoint(velocityRadPerSec); + } + + @Override + public void setTurnPosition(Rotation2d rotation) { + turnClosedLoop = true; + turnController.setSetpoint(rotation.getRadians()); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java new file mode 100644 index 0000000..1318598 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java @@ -0,0 +1,248 @@ +// Copyright 2021-2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.subsystems.drive; + +import static frc.robot.subsystems.drive.DriveConstants.*; +import static frc.robot.util.SparkUtil.*; + +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.SparkBase; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkClosedLoopController.ArbFFUnits; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkFlexConfig; +import com.revrobotics.spark.config.SparkMaxConfig; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.math.geometry.Rotation2d; +import java.util.Queue; +import java.util.function.DoubleSupplier; + +/** + * Module IO implementation for Spark Flex drive motor controller, Spark Max turn motor controller, + * and duty cycle absolute encoder. + */ +public class ModuleIOSpark implements ModuleIO { + private final Rotation2d zeroRotation; + + // Hardware objects + private final SparkBase driveSpark; + private final SparkBase turnSpark; + private final RelativeEncoder driveEncoder; + private final AbsoluteEncoder turnEncoder; + + // Closed loop controllers + private final SparkClosedLoopController driveController; + private final SparkClosedLoopController turnController; + + // Queue inputs from odometry thread + private final Queue timestampQueue; + private final Queue drivePositionQueue; + private final Queue turnPositionQueue; + + // Connection debouncers + private final Debouncer driveConnectedDebounce = new Debouncer(0.5); + private final Debouncer turnConnectedDebounce = new Debouncer(0.5); + + public ModuleIOSpark(int module) { + zeroRotation = + switch (module) { + case 0 -> frontLeftZeroRotation; + case 1 -> frontRightZeroRotation; + case 2 -> backLeftZeroRotation; + case 3 -> backRightZeroRotation; + default -> new Rotation2d(); + }; + driveSpark = + new SparkFlex( + switch (module) { + case 0 -> frontLeftDriveCanId; + case 1 -> frontRightDriveCanId; + case 2 -> backLeftDriveCanId; + case 3 -> backRightDriveCanId; + default -> 0; + }, + MotorType.kBrushless); + turnSpark = + new SparkMax( + switch (module) { + case 0 -> frontLeftTurnCanId; + case 1 -> frontRightTurnCanId; + case 2 -> backLeftTurnCanId; + case 3 -> backRightTurnCanId; + default -> 0; + }, + MotorType.kBrushless); + driveEncoder = driveSpark.getEncoder(); + turnEncoder = turnSpark.getAbsoluteEncoder(); + driveController = driveSpark.getClosedLoopController(); + turnController = turnSpark.getClosedLoopController(); + + // Configure drive motor + var driveConfig = new SparkFlexConfig(); + driveConfig + .idleMode(IdleMode.kBrake) + .smartCurrentLimit(driveMotorCurrentLimit) + .voltageCompensation(12.0); + driveConfig + .encoder + .positionConversionFactor(driveEncoderPositionFactor) + .velocityConversionFactor(driveEncoderVelocityFactor) + .uvwMeasurementPeriod(10) + .uvwAverageDepth(2); + driveConfig + .closedLoop + .feedbackSensor(FeedbackSensor.kPrimaryEncoder) + .pidf( + driveKp, 0.0, + driveKd, 0.0); + driveConfig + .signals + .primaryEncoderPositionAlwaysOn(true) + .primaryEncoderPositionPeriodMs((int) (1000.0 / odometryFrequency)) + .primaryEncoderVelocityAlwaysOn(true) + .primaryEncoderVelocityPeriodMs(20) + .appliedOutputPeriodMs(20) + .busVoltagePeriodMs(20) + .outputCurrentPeriodMs(20); + tryUntilOk( + driveSpark, + 5, + () -> + driveSpark.configure( + driveConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); + tryUntilOk(driveSpark, 5, () -> driveEncoder.setPosition(0.0)); + + // Configure turn motor + var turnConfig = new SparkMaxConfig(); + turnConfig + .inverted(turnInverted) + .idleMode(IdleMode.kBrake) + .smartCurrentLimit(turnMotorCurrentLimit) + .voltageCompensation(12.0); + turnConfig + .absoluteEncoder + .inverted(turnEncoderInverted) + .positionConversionFactor(turnEncoderPositionFactor) + .velocityConversionFactor(turnEncoderVelocityFactor) + .averageDepth(2); + turnConfig + .closedLoop + .feedbackSensor(FeedbackSensor.kAbsoluteEncoder) + .positionWrappingEnabled(true) + .positionWrappingInputRange(turnPIDMinInput, turnPIDMaxInput) + .pidf(turnKp, 0.0, turnKd, 0.0); + turnConfig + .signals + .absoluteEncoderPositionAlwaysOn(true) + .absoluteEncoderPositionPeriodMs((int) (1000.0 / odometryFrequency)) + .absoluteEncoderVelocityAlwaysOn(true) + .absoluteEncoderVelocityPeriodMs(20) + .appliedOutputPeriodMs(20) + .busVoltagePeriodMs(20) + .outputCurrentPeriodMs(20); + tryUntilOk( + turnSpark, + 5, + () -> + turnSpark.configure( + turnConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); + + // Create odometry queues + timestampQueue = SparkOdometryThread.getInstance().makeTimestampQueue(); + drivePositionQueue = + SparkOdometryThread.getInstance().registerSignal(driveSpark, driveEncoder::getPosition); + turnPositionQueue = + SparkOdometryThread.getInstance().registerSignal(turnSpark, turnEncoder::getPosition); + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + // Update drive inputs + sparkStickyFault = false; + ifOk(driveSpark, driveEncoder::getPosition, (value) -> inputs.drivePositionRad = value); + ifOk(driveSpark, driveEncoder::getVelocity, (value) -> inputs.driveVelocityRadPerSec = value); + ifOk( + driveSpark, + new DoubleSupplier[] {driveSpark::getAppliedOutput, driveSpark::getBusVoltage}, + (values) -> inputs.driveAppliedVolts = values[0] * values[1]); + ifOk(driveSpark, driveSpark::getOutputCurrent, (value) -> inputs.driveCurrentAmps = value); + inputs.driveConnected = driveConnectedDebounce.calculate(!sparkStickyFault); + + // Update turn inputs + sparkStickyFault = false; + ifOk( + turnSpark, + turnEncoder::getPosition, + (value) -> inputs.turnPosition = new Rotation2d(value).minus(zeroRotation)); + ifOk(turnSpark, turnEncoder::getVelocity, (value) -> inputs.turnVelocityRadPerSec = value); + ifOk( + turnSpark, + new DoubleSupplier[] {turnSpark::getAppliedOutput, turnSpark::getBusVoltage}, + (values) -> inputs.turnAppliedVolts = values[0] * values[1]); + ifOk(turnSpark, turnSpark::getOutputCurrent, (value) -> inputs.turnCurrentAmps = value); + inputs.turnConnected = turnConnectedDebounce.calculate(!sparkStickyFault); + + // Update odometry inputs + inputs.odometryTimestamps = + timestampQueue.stream().mapToDouble((Double value) -> value).toArray(); + inputs.odometryDrivePositionsRad = + drivePositionQueue.stream().mapToDouble((Double value) -> value).toArray(); + inputs.odometryTurnPositions = + turnPositionQueue.stream() + .map((Double value) -> new Rotation2d(value).minus(zeroRotation)) + .toArray(Rotation2d[]::new); + timestampQueue.clear(); + drivePositionQueue.clear(); + turnPositionQueue.clear(); + } + + @Override + public void setDriveOpenLoop(double output) { + driveSpark.setVoltage(output); + } + + @Override + public void setTurnOpenLoop(double output) { + turnSpark.setVoltage(output); + } + + @Override + public void setDriveVelocity(double velocityRadPerSec) { + double ffVolts = driveKs * Math.signum(velocityRadPerSec) + driveKv * velocityRadPerSec; + driveController.setReference( + velocityRadPerSec, + ControlType.kVelocity, + ClosedLoopSlot.kSlot0, + ffVolts, + ArbFFUnits.kVoltage); + } + + @Override + public void setTurnPosition(Rotation2d rotation) { + double setpoint = + MathUtil.inputModulus( + rotation.plus(zeroRotation).getRadians(), turnPIDMinInput, turnPIDMaxInput); + turnController.setReference(setpoint, ControlType.kPosition); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/SparkOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/SparkOdometryThread.java new file mode 100644 index 0000000..32c877a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/SparkOdometryThread.java @@ -0,0 +1,132 @@ +// Copyright 2021-2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.subsystems.drive; + +import com.revrobotics.REVLibError; +import com.revrobotics.spark.SparkBase; +import edu.wpi.first.wpilibj.Notifier; +import edu.wpi.first.wpilibj.RobotController; +import java.util.ArrayList; +import java.util.List; +import java.util.Queue; +import java.util.concurrent.ArrayBlockingQueue; +import java.util.function.DoubleSupplier; + +/** + * Provides an interface for asynchronously reading high-frequency measurements to a set of queues. + * + *

This version includes an overload for Spark signals, which checks for errors to ensure that + * all measurements in the sample are valid. + */ +public class SparkOdometryThread { + private final List sparks = new ArrayList<>(); + private final List sparkSignals = new ArrayList<>(); + private final List genericSignals = new ArrayList<>(); + private final List> sparkQueues = new ArrayList<>(); + private final List> genericQueues = new ArrayList<>(); + private final List> timestampQueues = new ArrayList<>(); + + private static SparkOdometryThread instance = null; + private Notifier notifier = new Notifier(this::run); + + public static SparkOdometryThread getInstance() { + if (instance == null) { + instance = new SparkOdometryThread(); + } + return instance; + } + + private SparkOdometryThread() { + notifier.setName("OdometryThread"); + } + + public void start() { + if (timestampQueues.size() > 0) { + notifier.startPeriodic(1.0 / DriveConstants.odometryFrequency); + } + } + + /** Registers a Spark signal to be read from the thread. */ + public Queue registerSignal(SparkBase spark, DoubleSupplier signal) { + Queue queue = new ArrayBlockingQueue<>(20); + Drive.odometryLock.lock(); + try { + sparks.add(spark); + sparkSignals.add(signal); + sparkQueues.add(queue); + } finally { + Drive.odometryLock.unlock(); + } + return queue; + } + + /** Registers a generic signal to be read from the thread. */ + public Queue registerSignal(DoubleSupplier signal) { + Queue queue = new ArrayBlockingQueue<>(20); + Drive.odometryLock.lock(); + try { + genericSignals.add(signal); + genericQueues.add(queue); + } finally { + Drive.odometryLock.unlock(); + } + return queue; + } + + /** Returns a new queue that returns timestamp values for each sample. */ + public Queue makeTimestampQueue() { + Queue queue = new ArrayBlockingQueue<>(20); + Drive.odometryLock.lock(); + try { + timestampQueues.add(queue); + } finally { + Drive.odometryLock.unlock(); + } + return queue; + } + + private void run() { + // Save new data to queues + Drive.odometryLock.lock(); + try { + // Get sample timestamp + double timestamp = RobotController.getFPGATime() / 1e6; + + // Read Spark values, mark invalid in case of error + double[] sparkValues = new double[sparkSignals.size()]; + boolean isValid = true; + for (int i = 0; i < sparkSignals.size(); i++) { + sparkValues[i] = sparkSignals.get(i).getAsDouble(); + if (sparks.get(i).getLastError() != REVLibError.kOk) { + isValid = false; + } + } + + // If valid, add values to queues + if (isValid) { + for (int i = 0; i < sparkSignals.size(); i++) { + sparkQueues.get(i).offer(sparkValues[i]); + } + for (int i = 0; i < genericSignals.size(); i++) { + genericQueues.get(i).offer(genericSignals.get(i).getAsDouble()); + } + for (int i = 0; i < timestampQueues.size(); i++) { + timestampQueues.get(i).offer(timestamp); + } + } + } finally { + Drive.odometryLock.unlock(); + } + } +} diff --git a/src/main/java/frc/robot/util/LocalADStarAK.java b/src/main/java/frc/robot/util/LocalADStarAK.java new file mode 100644 index 0000000..191b54f --- /dev/null +++ b/src/main/java/frc/robot/util/LocalADStarAK.java @@ -0,0 +1,153 @@ +package frc.robot.util; + +import com.pathplanner.lib.path.GoalEndState; +import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.path.PathPoint; +import com.pathplanner.lib.pathfinding.LocalADStar; +import com.pathplanner.lib.pathfinding.Pathfinder; +import edu.wpi.first.math.Pair; +import edu.wpi.first.math.geometry.Translation2d; +import java.util.ArrayList; +import java.util.Collections; +import java.util.List; +import org.littletonrobotics.junction.LogTable; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.inputs.LoggableInputs; + +// NOTE: This file is available at +// https://gist.github.com/mjansen4857/a8024b55eb427184dbd10ae8923bd57d + +public class LocalADStarAK implements Pathfinder { + private final ADStarIO io = new ADStarIO(); + + /** + * Get if a new path has been calculated since the last time a path was retrieved + * + * @return True if a new path is available + */ + @Override + public boolean isNewPathAvailable() { + if (!Logger.hasReplaySource()) { + io.updateIsNewPathAvailable(); + } + + Logger.processInputs("LocalADStarAK", io); + + return io.isNewPathAvailable; + } + + /** + * Get the most recently calculated path + * + * @param constraints The path constraints to use when creating the path + * @param goalEndState The goal end state to use when creating the path + * @return The PathPlannerPath created from the points calculated by the pathfinder + */ + @Override + public PathPlannerPath getCurrentPath(PathConstraints constraints, GoalEndState goalEndState) { + if (!Logger.hasReplaySource()) { + io.updateCurrentPathPoints(constraints, goalEndState); + } + + Logger.processInputs("LocalADStarAK", io); + + if (io.currentPathPoints.isEmpty()) { + return null; + } + + return PathPlannerPath.fromPathPoints(io.currentPathPoints, constraints, goalEndState); + } + + /** + * Set the start position to pathfind from + * + * @param startPosition Start position on the field. If this is within an obstacle it will be + * moved to the nearest non-obstacle node. + */ + @Override + public void setStartPosition(Translation2d startPosition) { + if (!Logger.hasReplaySource()) { + io.adStar.setStartPosition(startPosition); + } + } + + /** + * Set the goal position to pathfind to + * + * @param goalPosition Goal position on the field. f this is within an obstacle it will be moved + * to the nearest non-obstacle node. + */ + @Override + public void setGoalPosition(Translation2d goalPosition) { + if (!Logger.hasReplaySource()) { + io.adStar.setGoalPosition(goalPosition); + } + } + + /** + * Set the dynamic obstacles that should be avoided while pathfinding. + * + * @param obs A List of Translation2d pairs representing obstacles. Each Translation2d represents + * opposite corners of a bounding box. + * @param currentRobotPos The current position of the robot. This is needed to change the start + * position of the path to properly avoid obstacles + */ + @Override + public void setDynamicObstacles( + List> obs, Translation2d currentRobotPos) { + if (!Logger.hasReplaySource()) { + io.adStar.setDynamicObstacles(obs, currentRobotPos); + } + } + + private static class ADStarIO implements LoggableInputs { + public LocalADStar adStar = new LocalADStar(); + public boolean isNewPathAvailable = false; + public List currentPathPoints = Collections.emptyList(); + + @Override + public void toLog(LogTable table) { + table.put("IsNewPathAvailable", isNewPathAvailable); + + double[] pointsLogged = new double[currentPathPoints.size() * 2]; + int idx = 0; + for (PathPoint point : currentPathPoints) { + pointsLogged[idx] = point.position.getX(); + pointsLogged[idx + 1] = point.position.getY(); + idx += 2; + } + + table.put("CurrentPathPoints", pointsLogged); + } + + @Override + public void fromLog(LogTable table) { + isNewPathAvailable = table.get("IsNewPathAvailable", false); + + double[] pointsLogged = table.get("CurrentPathPoints", new double[0]); + + List pathPoints = new ArrayList<>(); + for (int i = 0; i < pointsLogged.length; i += 2) { + pathPoints.add( + new PathPoint(new Translation2d(pointsLogged[i], pointsLogged[i + 1]), null)); + } + + currentPathPoints = pathPoints; + } + + public void updateIsNewPathAvailable() { + isNewPathAvailable = adStar.isNewPathAvailable(); + } + + public void updateCurrentPathPoints(PathConstraints constraints, GoalEndState goalEndState) { + PathPlannerPath currentPath = adStar.getCurrentPath(constraints, goalEndState); + + if (currentPath != null) { + currentPathPoints = currentPath.getAllPathPoints(); + } else { + currentPathPoints = Collections.emptyList(); + } + } + } +} diff --git a/src/main/java/frc/robot/util/SparkUtil.java b/src/main/java/frc/robot/util/SparkUtil.java new file mode 100644 index 0000000..2767bbb --- /dev/null +++ b/src/main/java/frc/robot/util/SparkUtil.java @@ -0,0 +1,62 @@ +// Copyright 2021-2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.util; + +import com.revrobotics.REVLibError; +import com.revrobotics.spark.SparkBase; +import java.util.function.Consumer; +import java.util.function.DoubleConsumer; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; + +public class SparkUtil { + /** Stores whether any error was has been detected by other utility methods. */ + public static boolean sparkStickyFault = false; + + /** Processes a value from a Spark only if the value is valid. */ + public static void ifOk(SparkBase spark, DoubleSupplier supplier, DoubleConsumer consumer) { + double value = supplier.getAsDouble(); + if (spark.getLastError() == REVLibError.kOk) { + consumer.accept(value); + } else { + sparkStickyFault = true; + } + } + + /** Processes a value from a Spark only if the value is valid. */ + public static void ifOk( + SparkBase spark, DoubleSupplier[] suppliers, Consumer consumer) { + double[] values = new double[suppliers.length]; + for (int i = 0; i < suppliers.length; i++) { + values[i] = suppliers[i].getAsDouble(); + if (spark.getLastError() != REVLibError.kOk) { + sparkStickyFault = true; + return; + } + } + consumer.accept(values); + } + + /** Attempts to run the command until no error is produced. */ + public static void tryUntilOk(SparkBase spark, int maxAttempts, Supplier command) { + for (int i = 0; i < maxAttempts; i++) { + var error = command.get(); + if (error == REVLibError.kOk) { + break; + } else { + sparkStickyFault = true; + } + } + } +} diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json new file mode 100644 index 0000000..fa81b2f --- /dev/null +++ b/vendordeps/AdvantageKit.json @@ -0,0 +1,35 @@ +{ + "fileName": "AdvantageKit.json", + "name": "AdvantageKit", + "version": "4.1.0", + "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", + "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", + "artifactId": "akit-java", + "version": "4.1.0" + } + ], + "jniDependencies": [ + { + "groupId": "org.littletonrobotics.akit", + "artifactId": "akit-wpilibio", + "version": "4.1.0", + "skipInvalidPlatforms": false, + "isJar": false, + "validPlatforms": [ + "linuxathena", + "linuxx86-64", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] + } + ], + "cppDependencies": [] +} diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json new file mode 100644 index 0000000..1371a40 --- /dev/null +++ b/vendordeps/PathplannerLib.json @@ -0,0 +1,38 @@ +{ + "fileName": "PathplannerLib.json", + "name": "PathplannerLib", + "version": "2025.2.1", + "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", + "frcYear": "2025", + "mavenUrls": [ + "https://3015rangerrobotics.github.io/pathplannerlib/repo" + ], + "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", + "javaDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-java", + "version": "2025.2.1" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-cpp", + "version": "2025.2.1", + "libName": "PathplannerLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal", + "linuxathena", + "linuxarm32", + "linuxarm64" + ] + } + ] +} diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6.json new file mode 100644 index 0000000..820c61a --- /dev/null +++ b/vendordeps/Phoenix6.json @@ -0,0 +1,419 @@ +{ + "fileName": "Phoenix6-frc2025-latest.json", + "name": "CTRE-Phoenix (v6)", + "version": "25.2.1", + "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-frc2025-latest.json", + "conflictsWith": [ + { + "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": "25.2.1" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "api-cpp", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "api-cpp-sim", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-cpp", + "version": "25.2.1", + "libName": "CTRE_Phoenix6_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "25.2.1", + "libName": "CTRE_PhoenixTools", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "25.2.1", + "libName": "CTRE_Phoenix6_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "25.2.1", + "libName": "CTRE_PhoenixTools_Sim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "25.2.1", + "libName": "CTRE_SimTalonSRX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "25.2.1", + "libName": "CTRE_SimVictorSPX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "25.2.1", + "libName": "CTRE_SimPigeonIMU", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "25.2.1", + "libName": "CTRE_SimCANCoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "25.2.1", + "libName": "CTRE_SimProTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "25.2.1", + "libName": "CTRE_SimProTalonFXS", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "25.2.1", + "libName": "CTRE_SimProCANcoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "25.2.1", + "libName": "CTRE_SimProPigeon2", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "25.2.1", + "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.json new file mode 100644 index 0000000..552a3b0 --- /dev/null +++ b/vendordeps/REVLib.json @@ -0,0 +1,71 @@ +{ + "fileName": "REVLib.json", + "name": "REVLib", + "version": "2025.0.1", + "frcYear": "2025", + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "mavenUrls": [ + "https://maven.revrobotics.com/" + ], + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2025.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-java", + "version": "2025.0.1" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2025.0.1", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-cpp", + "version": "2025.0.1", + "libName": "REVLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2025.0.1", + "libName": "REVLibDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] +} diff --git a/vendordeps/ReduxLib-2025.0.1.json b/vendordeps/ReduxLib-2025.0.1.json new file mode 100644 index 0000000..3a66d72 --- /dev/null +++ b/vendordeps/ReduxLib-2025.0.1.json @@ -0,0 +1,72 @@ +{ + "fileName": "ReduxLib-2025.0.1.json", + "name": "ReduxLib", + "version": "2025.0.1", + "frcYear": "2025", + "uuid": "151ecca8-670b-4026-8160-cdd2679ef2bd", + "mavenUrls": [ + "https://maven.reduxrobotics.com/" + ], + "jsonUrl": "https://frcsdk.reduxrobotics.com/ReduxLib_2025.json", + "javaDependencies": [ + { + "groupId": "com.reduxrobotics.frc", + "artifactId": "ReduxLib-java", + "version": "2025.0.1" + } + ], + "jniDependencies": [ + { + "groupId": "com.reduxrobotics.frc", + "artifactId": "ReduxLib-driver", + "version": "2025.0.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxathena", + "linuxx86-64", + "linuxarm32", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.reduxrobotics.frc", + "artifactId": "ReduxLib-cpp", + "version": "2025.0.1", + "libName": "ReduxLib", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxx86-64", + "linuxarm32", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] + }, + { + "groupId": "com.reduxrobotics.frc", + "artifactId": "ReduxLib-driver", + "version": "2025.0.1", + "libName": "ReduxCore", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxx86-64", + "linuxarm32", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] + } + ] +} diff --git a/vendordeps/Studica.json b/vendordeps/Studica.json new file mode 100644 index 0000000..a3ed1fe --- /dev/null +++ b/vendordeps/Studica.json @@ -0,0 +1,71 @@ +{ + "fileName": "Studica-2025.0.1.json", + "name": "Studica", + "version": "2025.0.1", + "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.1.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.1" + }, + { + "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.1" + } + ], + "javaDependencies": [ + { + "artifactId": "Studica-java", + "groupId": "com.studica.frc", + "version": "2025.0.1" + } + ], + "jniDependencies": [ + { + "artifactId": "Studica-driver", + "groupId": "com.studica.frc", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "linuxx86-64", + "osxuniversal", + "windowsx86-64" + ], + "version": "2025.0.1" + } + ] +} diff --git a/vendordeps/URCL.json b/vendordeps/URCL.json new file mode 100644 index 0000000..c991b6a --- /dev/null +++ b/vendordeps/URCL.json @@ -0,0 +1,65 @@ +{ + "fileName": "URCL.json", + "name": "URCL", + "version": "2025.0.0", + "frcYear": "2025", + "uuid": "84246d17-a797-4d1e-bd9f-c59cd8d2477c", + "mavenUrls": [ + "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/" + ], + "jsonUrl": "https://raw.githubusercontent.com/Mechanical-Advantage/URCL/main/URCL.json", + "javaDependencies": [ + { + "groupId": "org.littletonrobotics.urcl", + "artifactId": "URCL-java", + "version": "2025.0.0" + } + ], + "jniDependencies": [ + { + "groupId": "org.littletonrobotics.urcl", + "artifactId": "URCL-driver", + "version": "2025.0.0", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "org.littletonrobotics.urcl", + "artifactId": "URCL-cpp", + "version": "2025.0.0", + "libName": "URCL", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena", + "osxuniversal" + ] + }, + { + "groupId": "org.littletonrobotics.urcl", + "artifactId": "URCL-driver", + "version": "2025.0.0", + "libName": "URCLDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena", + "osxuniversal" + ] + } + ] +}