diff --git a/.gitignore b/.gitignore index 10712449..2e9afc3e 100644 --- a/.gitignore +++ b/.gitignore @@ -1,166 +1,176 @@ -src/main/java/frc/robot/BuildConstants.java - -# This gitignore has been specially created by the WPILib team. -# If you remove items from this file, intellisense might break. - -### C++ ### -# Prerequisites -*.d - -# Compiled Object files -*.slo -*.lo -*.o -*.obj - -# Precompiled Headers -*.gch -*.pch - -# Compiled Dynamic libraries -*.so -*.dylib -*.dll - -# Fortran module files -*.mod -*.smod - -# Compiled Static libraries -*.lai -*.la -*.a -*.lib - -# Executables -*.exe -*.out -*.app - -### Java ### -# Compiled class file -*.class - -# Log file -*.log -*.wpilog - -# BlueJ files -*.ctxt - -# Mobile Tools for Java (J2ME) -.mtj.tmp/ - -# Package Files # -*.jar -*.war -*.nar -*.ear -*.zip -*.tar.gz -*.rar - -# virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml -hs_err_pid* - -### Linux ### -*~ - -# temporary files which can be created if a process still has a handle open of a deleted file -.fuse_hidden* - -# KDE directory preferences -.directory - -# Linux trash folder which might appear on any partition or disk -.Trash-* - -# .nfs files are created when an open file is removed but is still being accessed -.nfs* - -### macOS ### -# General -.DS_Store -.AppleDouble -.LSOverride - -# Icon must end with two \r -Icon - -# Thumbnails -._* - -# Files that might appear in the root of a volume -.DocumentRevisions-V100 -.fseventsd -.Spotlight-V100 -.TemporaryItems -.Trashes -.VolumeIcon.icns -.com.apple.timemachine.donotpresent - -# Directories potentially created on remote AFP share -.AppleDB -.AppleDesktop -Network Trash Folder -Temporary Items -.apdisk - -### VisualStudioCode ### -.vscode/* -!.vscode/settings.json -!.vscode/tasks.json -!.vscode/launch.json -!.vscode/extensions.json - -### Windows ### -# Windows thumbnail cache files -Thumbs.db -ehthumbs.db -ehthumbs_vista.db - -# Dump file -*.stackdump - -# Folder config file -[Dd]esktop.ini - -# Recycle Bin used on file shares -$RECYCLE.BIN/ - -# Windows Installer files -*.cab -*.msi -*.msix -*.msm -*.msp - -# Windows shortcuts -*.lnk - -### Gradle ### -.gradle -/build/ - -# Ignore Gradle GUI config -gradle-app.setting - -# Avoid ignoring Gradle wrapper jar file (.jar files are usually ignored) -!gradle-wrapper.jar - -# Cache of project -.gradletasknamecache - -# # Work around https://youtrack.jetbrains.com/issue/IDEA-116898 -# gradle/wrapper/gradle-wrapper.properties - -# # VS Code Specific Java Settings -# DO NOT REMOVE .classpath and .project -.classpath -.project -.settings/ -bin/ - -# Simulation GUI and other tools window save file -*-window.json - +src/main/java/frc/robot/BuildConstants.java + +ctre_sim/* + +# This gitignore has been specially created by the WPILib team. +# If you remove items from this file, intellisense might break. + +### C++ ### +# Prerequisites +*.d + +# Compiled Object files +*.slo +*.lo +*.o +*.obj + +# Precompiled Headers +*.gch +*.pch + +# Compiled Dynamic libraries +*.so +*.dylib +*.dll + +# Fortran module files +*.mod +*.smod + +# Compiled Static libraries +*.lai +*.la +*.a +*.lib + +# Executables +*.exe +*.out +*.app + +### Java ### +# Compiled class file +*.class + +# Log file +*.log + +# BlueJ files +*.ctxt + +# Mobile Tools for Java (J2ME) +.mtj.tmp/ + +# Package Files # +*.jar +*.war +*.nar +*.ear +*.zip +*.tar.gz +*.rar + +# virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml +hs_err_pid* + +### Linux ### +*~ + +# temporary files which can be created if a process still has a handle open of a deleted file +.fuse_hidden* + +# KDE directory preferences +.directory + +# Linux trash folder which might appear on any partition or disk +.Trash-* + +# .nfs files are created when an open file is removed but is still being accessed +.nfs* + +### macOS ### +# General +.DS_Store +.AppleDouble +.LSOverride + +# Icon must end with two \r +Icon + +# Thumbnails +._* + +# Files that might appear in the root of a volume +.DocumentRevisions-V100 +.fseventsd +.Spotlight-V100 +.TemporaryItems +.Trashes +.VolumeIcon.icns +.com.apple.timemachine.donotpresent + +# Directories potentially created on remote AFP share +.AppleDB +.AppleDesktop +Network Trash Folder +Temporary Items +.apdisk + +### VisualStudioCode ### +.vscode/* +!.vscode/settings.json +!.vscode/tasks.json +!.vscode/launch.json +!.vscode/extensions.json + +### Windows ### +# Windows thumbnail cache files +Thumbs.db +ehthumbs.db +ehthumbs_vista.db + +# Dump file +*.stackdump + +# Folder config file +[Dd]esktop.ini + +# Recycle Bin used on file shares +$RECYCLE.BIN/ + +# Windows Installer files +*.cab +*.msi +*.msix +*.msm +*.msp + +# Windows shortcuts +*.lnk + +### Gradle ### +.gradle +/build/ + +# Ignore Gradle GUI config +gradle-app.setting + +# Avoid ignoring Gradle wrapper jar file (.jar files are usually ignored) +!gradle-wrapper.jar + +# Cache of project +.gradletasknamecache + +# # Work around https://youtrack.jetbrains.com/issue/IDEA-116898 +# gradle/wrapper/gradle-wrapper.properties + +# # VS Code Specific Java Settings +# DO NOT REMOVE .classpath and .project +.classpath +.project +.settings/ +bin/ + +# IntelliJ +*.iml +*.ipr +*.iws +.idea/ +out/ + +# Fleet +.fleet + +# Simulation GUI and other tools window save file +*-window.json diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 14b9588a..eaba73b5 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -1,7 +1,13 @@ { - "robotWidth": 0.89, - "robotLength": 0.91, + "robotWidth": 0.8382, + "robotLength": 0.8382, "holonomicMode": true, - "generateJSON": false, - "generateCSV": false + "pathFolders": [], + "autoFolders": [], + "defaultMaxVel": 4.0, + "defaultMaxAccel": 4.0, + "defaultMaxAngVel": 540.0, + "defaultMaxAngAccel": 720.0, + "maxModuleSpeed": 4.78, + "choreoProjectPath": null } \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json index c9f593f2..0bdac6e7 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -34,6 +34,7 @@ "cSpell.words": [ "algs", "APRILTAG", + "bezier", "brushless", "canbus", "Cancoder", @@ -44,18 +45,22 @@ "CTRE", "curr", "DEADBAND", + "Devs", "dtheta", "Feedforward", "gamepad", "gradlew", "holonomic", "Huskie", + "IOCTRE", "jama", "javac", "leds", "Nivore", "odometry", "openloop", + "pathfind", + "pathfinding", "Penrose", "photonvision", "PIDF", diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index cb5a05be..f02c10da 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -1,6 +1,6 @@ { "enableCppIntellisense": false, "currentLanguage": "java", - "projectYear": "2023", + "projectYear": "2024beta", "teamNumber": 3061 } \ No newline at end of file diff --git a/Log_23-06-23_19-38-49.wpilog b/Log_23-06-23_19-38-49.wpilog new file mode 100644 index 00000000..abe5659f Binary files /dev/null and b/Log_23-06-23_19-38-49.wpilog differ diff --git a/Log_23-06-23_19-45-26.wpilog b/Log_23-06-23_19-45-26.wpilog new file mode 100644 index 00000000..caf31e13 Binary files /dev/null and b/Log_23-06-23_19-45-26.wpilog differ diff --git a/WPILib-License.md b/WPILib-License.md index 3d5a824c..43b62ec2 100644 --- a/WPILib-License.md +++ b/WPILib-License.md @@ -1,4 +1,4 @@ -Copyright (c) 2009-2021 FIRST and other WPILib contributors +Copyright (c) 2009-2023 FIRST and other WPILib contributors All rights reserved. Redistribution and use in source and binary forms, with or without diff --git a/build.gradle b/build.gradle index e4679cc9..14f2a122 100644 --- a/build.gradle +++ b/build.gradle @@ -1,12 +1,14 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2023.4.3" + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-4" id 'com.diffplug.spotless' version '6.11.0' id "com.peterabeles.gversion" version "1.10" } -sourceCompatibility = JavaVersion.VERSION_11 -targetCompatibility = JavaVersion.VERSION_11 +java { + sourceCompatibility = JavaVersion.VERSION_17 + targetCompatibility = JavaVersion.VERSION_17 +} def ROBOT_MAIN_CLASS = "frc.robot.Main" @@ -91,6 +93,7 @@ wpi.sim.addDriverstation() // knows where to look for our Robot Class. jar { from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } } + from sourceSets.main.allSource manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) duplicatesStrategy = DuplicatesStrategy.INCLUDE } diff --git a/gradle/wrapper/gradle-wrapper.jar b/gradle/wrapper/gradle-wrapper.jar index 249e5832..7f93135c 100644 Binary files a/gradle/wrapper/gradle-wrapper.jar and b/gradle/wrapper/gradle-wrapper.jar differ diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties index 74147692..c5c27066 100644 --- a/gradle/wrapper/gradle-wrapper.properties +++ b/gradle/wrapper/gradle-wrapper.properties @@ -1,5 +1,7 @@ distributionBase=GRADLE_USER_HOME distributionPath=permwrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-7.5.1-bin.zip +distributionUrl=https\://services.gradle.org/distributions/gradle-8.4-bin.zip +networkTimeout=10000 +validateDistributionUrl=true zipStoreBase=GRADLE_USER_HOME zipStorePath=permwrapper/dists diff --git a/gradlew b/gradlew index a69d9cb6..1aa94a42 100755 --- a/gradlew +++ b/gradlew @@ -55,7 +55,7 @@ # Darwin, MinGW, and NonStop. # # (3) This script is generated from the Groovy template -# https://github.com/gradle/gradle/blob/master/subprojects/plugins/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/. @@ -80,13 +80,11 @@ do esac done -APP_HOME=$( cd "${APP_HOME:-./}" && pwd -P ) || exit - -APP_NAME="Gradle" +# This is normally unused +# shellcheck disable=SC2034 APP_BASE_NAME=${0##*/} - -# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. -DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"' +# Discard cd standard output in case $CDPATH is set (https://github.com/gradle/gradle/issues/25036) +APP_HOME=$( cd "${APP_HOME:-./}" > /dev/null && pwd -P ) || exit # Use the maximum available, or set MAX_FD != -1 to use that value. MAX_FD=maximum @@ -133,22 +131,29 @@ location of your Java installation." fi else JAVACMD=java - which java >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. + if ! command -v java >/dev/null 2>&1 + then + die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. Please set the JAVA_HOME variable in your environment to match the location of your Java installation." + fi fi # Increase the maximum file descriptors if we can. if ! "$cygwin" && ! "$darwin" && ! "$nonstop" ; then case $MAX_FD in #( max*) + # In POSIX sh, ulimit -H is undefined. That's why the result is checked to see if it worked. + # shellcheck disable=SC2039,SC3045 MAX_FD=$( ulimit -H -n ) || warn "Could not query maximum file descriptor limit" esac case $MAX_FD in #( '' | soft) :;; #( *) + # In POSIX sh, ulimit -n is undefined. That's why the result is checked to see if it worked. + # shellcheck disable=SC2039,SC3045 ulimit -n "$MAX_FD" || warn "Could not set maximum file descriptor limit to $MAX_FD" esac @@ -193,11 +198,15 @@ if "$cygwin" || "$msys" ; then done fi -# Collect all arguments for the java command; -# * $DEFAULT_JVM_OPTS, $JAVA_OPTS, and $GRADLE_OPTS can contain fragments of -# shell script including quotes and variable substitutions, so put them in -# double quotes to make sure that they get re-expanded; and -# * put everything else in single quotes, so that it's not re-expanded. + +# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. +DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"' + +# Collect all arguments for the java command: +# * DEFAULT_JVM_OPTS, JAVA_OPTS, JAVA_OPTS, and optsEnvironmentVar are not allowed to contain shell fragments, +# and any embedded shellness will be escaped. +# * For example: A user cannot expect ${Hostname} to be expanded, as it is an environment variable and will be +# treated as '${Hostname}' itself on the command line. set -- \ "-Dorg.gradle.appname=$APP_BASE_NAME" \ diff --git a/gradlew.bat b/gradlew.bat index 53a6b238..6689b85b 100644 --- a/gradlew.bat +++ b/gradlew.bat @@ -26,6 +26,7 @@ if "%OS%"=="Windows_NT" setlocal set DIRNAME=%~dp0 if "%DIRNAME%"=="" set DIRNAME=. +@rem This is normally unused set APP_BASE_NAME=%~n0 set APP_HOME=%DIRNAME% diff --git a/networktables.json b/networktables.json index fe51488c..41b42e67 100644 --- a/networktables.json +++ b/networktables.json @@ -1 +1,3 @@ -[] +[ + +] diff --git a/networktables.json.bck b/networktables.json.bck new file mode 100644 index 00000000..41b42e67 --- /dev/null +++ b/networktables.json.bck @@ -0,0 +1,3 @@ +[ + +] diff --git a/settings.gradle b/settings.gradle index 48c039ed..d94f73c6 100644 --- a/settings.gradle +++ b/settings.gradle @@ -4,7 +4,7 @@ pluginManagement { repositories { mavenLocal() gradlePluginPortal() - String frcYear = '2023' + String frcYear = '2024' File frcHome if (OperatingSystem.current().isWindows()) { String publicFolder = System.getenv('PUBLIC') @@ -25,3 +25,6 @@ pluginManagement { } } } + +Properties props = System.getProperties(); +props.setProperty("org.gradle.internal.native.headers.unresolved.dependencies.ignore", "true"); diff --git a/simgui.json b/simgui.json index 6d612087..bc765ba5 100644 --- a/simgui.json +++ b/simgui.json @@ -2,33 +2,15 @@ "NTProvider": { "types": { "/FMSInfo": "FMSInfo", - "/LiveWindow/Ungrouped/PIDController[10]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[11]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[1]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[2]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[3]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[4]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[5]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[6]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[7]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[8]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[9]": "PIDController", - "/LiveWindow/Ungrouped/Scheduler": "Scheduler", "/Shuffleboard/MAIN/SendableChooser[0]": "String Chooser", - "/Shuffleboard/Subsystem/Subsystem": "Subsystem", - "/SmartDashboard//SystemStatus/Drivetrain/SystemCheck": "Command", - "/SmartDashboard//SystemStatus/SwerveModule0/SystemCheck": "Command", - "/SmartDashboard//SystemStatus/SwerveModule1/SystemCheck": "Command", - "/SmartDashboard//SystemStatus/SwerveModule2/SystemCheck": "Command", - "/SmartDashboard//SystemStatus/SwerveModule3/SystemCheck": "Command", - "/SmartDashboard//SystemStatusDrivetrain/SystemCheck": "Command", - "/SmartDashboard//SystemStatusSwerveModule0/SystemCheck": "Command", - "/SmartDashboard//SystemStatusSwerveModule1/SystemCheck": "Command", - "/SmartDashboard//SystemStatusSwerveModule2/SystemCheck": "Command", - "/SmartDashboard//SystemStatusSwerveModule3/SystemCheck": "Command", "/SmartDashboard/Auto Routine": "String Chooser", - "/SmartDashboard/PPSwerveControllerCommand_field": "Field2d", - "/SmartDashboard/simCamera Sim Field": "Field2d" + "/SmartDashboard/SystemStatus/Drivetrain/SystemCheck": "Command", + "/SmartDashboard/SystemStatus/Subsystem/SystemCheck": "Command", + "/SmartDashboard/SystemStatus/SwerveModule0/SystemCheck": "Command", + "/SmartDashboard/SystemStatus/SwerveModule1/SystemCheck": "Command", + "/SmartDashboard/SystemStatus/SwerveModule2/SystemCheck": "Command", + "/SmartDashboard/SystemStatus/SwerveModule3/SystemCheck": "Command", + "/SmartDashboard/VisionSystemSim-simCamera/Sim Field": "Field2d" } } } diff --git a/src/main/deploy/pathplanner/DistanceTest.path b/src/main/deploy/pathplanner/DistanceTest.path deleted file mode 100644 index bfbb3c47..00000000 --- a/src/main/deploy/pathplanner/DistanceTest.path +++ /dev/null @@ -1,49 +0,0 @@ -{ - "waypoints": [ - { - "anchorPoint": { - "x": 1.8184265428359916, - "y": 4.396379732930406 - }, - "prevControl": null, - "nextControl": { - "x": 2.818426542835991, - "y": 4.396379732930406 - }, - "holonomicAngle": 0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 5.82, - "y": 4.4 - }, - "prevControl": { - "x": 4.59818164446877, - "y": 4.4 - }, - "nextControl": null, - "holonomicAngle": 0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - } - ], - "markers": [] -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/StartPoint.path b/src/main/deploy/pathplanner/StartPoint.path deleted file mode 100644 index 842c5f23..00000000 --- a/src/main/deploy/pathplanner/StartPoint.path +++ /dev/null @@ -1,49 +0,0 @@ -{ - "waypoints": [ - { - "anchorPoint": { - "x": 4.480580999076691, - "y": 1.0827644404103984 - }, - "prevControl": null, - "nextControl": { - "x": 5.480580999076694, - "y": 1.0827644404103984 - }, - "holonomicAngle": 0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 6.902554089468368, - "y": 1.1492499762250707 - }, - "prevControl": { - "x": 5.902554089468368, - "y": 1.1492499762250707 - }, - "nextControl": null, - "holonomicAngle": 0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - } - ], - "markers": [] -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/TestPath.path b/src/main/deploy/pathplanner/TestPath.path deleted file mode 100644 index 0bea6bbe..00000000 --- a/src/main/deploy/pathplanner/TestPath.path +++ /dev/null @@ -1,140 +0,0 @@ -{ - "waypoints": [ - { - "anchorPoint": { - "x": 2.0, - "y": 3.0 - }, - "prevControl": null, - "nextControl": { - "x": 3.0, - "y": 3.0 - }, - "holonomicAngle": 0.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 3.5, - "y": 5.0 - }, - "prevControl": { - "x": 3.5, - "y": 4.0 - }, - "nextControl": { - "x": 3.5, - "y": 4.0 - }, - "holonomicAngle": 90.0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 5.0, - "y": 3.0 - }, - "prevControl": { - "x": 4.0, - "y": 3.0 - }, - "nextControl": { - "x": 4.0, - "y": 3.0 - }, - "holonomicAngle": 180.0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": true, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 3.5, - "y": 5.0 - }, - "prevControl": { - "x": 3.4925261115582478, - "y": 4.02025233267334 - }, - "nextControl": { - "x": 3.4925261115582478, - "y": 4.02025233267334 - }, - "holonomicAngle": -90.0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 2.0, - "y": 3.0 - }, - "prevControl": { - "x": 2.97559299937877, - "y": 2.975283149361479 - }, - "nextControl": null, - "holonomicAngle": 0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - } - ], - "maxVelocity": 2.0, - "maxAcceleration": 2.0, - "isReversed": null, - "markers": [ - { - "position": 1.0029829545454547, - "names": [ - "event1" - ] - }, - { - "position": 3.5, - "names": [ - "event2" - ] - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/Tuning.path b/src/main/deploy/pathplanner/Tuning.path deleted file mode 100644 index bc332ca3..00000000 --- a/src/main/deploy/pathplanner/Tuning.path +++ /dev/null @@ -1,74 +0,0 @@ -{ - "waypoints": [ - { - "anchorPoint": { - "x": 7.070195642127396, - "y": 0.34885558660006205 - }, - "prevControl": null, - "nextControl": { - "x": 8.937466045070979, - "y": 1.0829252127254254 - }, - "holonomicAngle": 90.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 7.090433441283549, - "y": 5.118539523537381 - }, - "prevControl": { - "x": 8.486203715462986, - "y": 4.754665054468998 - }, - "nextControl": { - "x": 8.486203715462986, - "y": 4.754665054468998 - }, - "holonomicAngle": -90.0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 7.07, - "y": 0.35 - }, - "prevControl": { - "x": 8.94280093768521, - "y": 1.0549170265007195 - }, - "nextControl": null, - "holonomicAngle": 90.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - } - ], - "markers": [] -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/DistanceTest.auto b/src/main/deploy/pathplanner/autos/DistanceTest.auto new file mode 100644 index 00000000..18af39c8 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/DistanceTest.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.82, + "y": 4.4 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "DistanceTest" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/TestAuto.auto b/src/main/deploy/pathplanner/autos/TestAuto.auto new file mode 100644 index 00000000..b9784941 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/TestAuto.auto @@ -0,0 +1,56 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 2, + "y": 3.0 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "TestPath1" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "enableXStance" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "named", + "data": { + "name": "disableXStance" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "TestPath2" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Tuning.auto b/src/main/deploy/pathplanner/autos/Tuning.auto new file mode 100644 index 00000000..65764812 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Tuning.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 7.07, + "y": 0.35 + }, + "rotation": 90.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Tuning" + } + } + ] + } + }, + "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 00000000..7026fb97 --- /dev/null +++ b/src/main/deploy/pathplanner/navgrid.json @@ -0,0 +1 @@ +{"field_size":{"x":16.54,"height":8.02},"nodeSizeMeters":0.2,"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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,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,true,true,true,true,true,true,true,true,true,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,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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,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,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/DistanceTest.path b/src/main/deploy/pathplanner/paths/DistanceTest.path new file mode 100644 index 00000000..f4d4b381 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/DistanceTest.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.82, + "y": 4.4 + }, + "prevControl": null, + "nextControl": { + "x": 2.938033988749895, + "y": 4.4 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.82, + "y": 4.4 + }, + "prevControl": { + "x": 4.299309367425446, + "y": 4.4 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/StartPoint.path b/src/main/deploy/pathplanner/paths/StartPoint.path new file mode 100644 index 00000000..4beb119b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/StartPoint.path @@ -0,0 +1,55 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 4.48, + "y": 1.08 + }, + "prevControl": null, + "nextControl": { + "x": 5.6000000000000005, + "y": 1.08 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.0, + "y": 1.08 + }, + "prevControl": { + "x": 4.479309367425445, + "y": 1.08 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0, + "rotationDegrees": 0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TestPath1.path b/src/main/deploy/pathplanner/paths/TestPath1.path new file mode 100644 index 00000000..eafbc844 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/TestPath1.path @@ -0,0 +1,106 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.0, + "y": 3.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.2503786170628963, + "y": 3.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.5, + "y": 5.0 + }, + "prevControl": { + "x": 3.5, + "y": 4.9 + }, + "nextControl": { + "x": 3.5, + "y": 5.1 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.0, + "y": 3.0 + }, + "prevControl": { + "x": 3.9103856889537556, + "y": 3.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": 90.0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 0.35, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "command1" + } + } + ] + } + } + }, + { + "name": "New Event Marker", + "waypointRelativePos": 1.5, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "command2" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TestPath2.path b/src/main/deploy/pathplanner/paths/TestPath2.path new file mode 100644 index 00000000..64b791c0 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/TestPath2.path @@ -0,0 +1,109 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 5.0, + "y": 3.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.9407474320842137, + "y": 3.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.5, + "y": 5.0 + }, + "prevControl": { + "x": 3.5, + "y": 4.8999999999999995 + }, + "nextControl": { + "x": 3.5, + "y": 5.1000000000000005 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.0, + "y": 3.0 + }, + "prevControl": { + "x": 3.208472036667854, + "y": 3.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": -90.0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 0.4, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "command2" + } + } + ] + } + } + }, + { + "name": "New Event Marker", + "waypointRelativePos": 1.7, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "command1" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 180.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Tuning.path b/src/main/deploy/pathplanner/paths/Tuning.path new file mode 100644 index 00000000..d8731eea --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Tuning.path @@ -0,0 +1,74 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.07, + "y": 0.35 + }, + "prevControl": null, + "nextControl": { + "x": 8.67696902421635, + "y": 2.265111107797446 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.09, + "y": 5.12 + }, + "prevControl": { + "x": 7.086964281102219, + "y": 5.219953911433088 + }, + "nextControl": { + "x": 7.093035718897781, + "y": 5.020046088566912 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.07, + "y": 0.35 + }, + "prevControl": { + "x": 8.676969024216348, + "y": 2.265111107797445 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": -90.0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 90.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/lib/team254/drivers/LazyTalonFX.java b/src/main/java/frc/lib/team254/drivers/LazyTalonFX.java deleted file mode 100644 index 2a3585ba..00000000 --- a/src/main/java/frc/lib/team254/drivers/LazyTalonFX.java +++ /dev/null @@ -1,34 +0,0 @@ -/* - * Initially from https://github.com/frc1678/C2022 - */ - -package frc.lib.team254.drivers; - -import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.can.TalonFX; - -/** - * This class is a thin wrapper around the CANTalon that reduces CAN bus / CPU overhead by skipping - * duplicate set commands. (By default the Talon flushes the Tx buffer on every set call). - */ -public class LazyTalonFX extends TalonFX { - protected double mLastSet = Double.NaN; - protected ControlMode mLastControlMode = null; - - public LazyTalonFX(int deviceNumber) { - super(deviceNumber); - } - - public double getLastSet() { - return mLastSet; - } - - @Override - public void set(ControlMode mode, double value) { - if (value != mLastSet || mode != mLastControlMode) { - mLastSet = value; - mLastControlMode = mode; - super.set(mode, value); - } - } -} diff --git a/src/main/java/frc/lib/team254/drivers/LazyTalonSRX.java b/src/main/java/frc/lib/team254/drivers/LazyTalonSRX.java deleted file mode 100644 index 2f5efbfb..00000000 --- a/src/main/java/frc/lib/team254/drivers/LazyTalonSRX.java +++ /dev/null @@ -1,34 +0,0 @@ -/* - * Initially from https://github.com/frc1678/C2022 - */ - -package frc.lib.team254.drivers; - -import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.can.TalonSRX; - -/** - * This class is a thin wrapper around the CANTalon that reduces CAN bus / CPU overhead by skipping - * duplicate set commands. (By default the Talon flushes the Tx buffer on every set call). - */ -public class LazyTalonSRX extends TalonSRX { - protected double mLastSet = Double.NaN; - protected ControlMode mLastControlMode = null; - - public LazyTalonSRX(int deviceNumber) { - super(deviceNumber); - } - - public double getLastSet() { - return mLastSet; - } - - @Override - public void set(ControlMode mode, double value) { - if (value != mLastSet || mode != mLastControlMode) { - mLastSet = value; - mLastControlMode = mode; - super.set(mode, value); - } - } -} diff --git a/src/main/java/frc/lib/team254/drivers/TalonFXFactory.java b/src/main/java/frc/lib/team254/drivers/TalonFXFactory.java deleted file mode 100644 index dfe7ae51..00000000 --- a/src/main/java/frc/lib/team254/drivers/TalonFXFactory.java +++ /dev/null @@ -1,239 +0,0 @@ -/* - * Initially from https://github.com/frc1678/C2022 - */ - -package frc.lib.team254.drivers; - -import com.ctre.phoenix.motorcontrol.*; -import com.ctre.phoenix.motorcontrol.can.TalonFX; -import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration; -import com.ctre.phoenix.sensors.SensorInitializationStrategy; -import com.ctre.phoenix.sensors.SensorVelocityMeasPeriod; - -@java.lang.SuppressWarnings({"java:S1104", "java:S116"}) - -/** - * Creates CANTalon objects and configures all the parameters we care about to factory defaults. - * Closed-loop and sensor parameters are not set, as these are expected to be set by the - * application. - */ -public class TalonFXFactory { - - private static final int TIMEOUT_MS = 100; - - // These periods don't share any common factors, so they shouldn't run at the same time. 255 is - // max. (initially from https://github.com/Mechanical-Advantage/RobotCode2022) - private static int[] kPrimePeriods = - new int[] {255, 254, 253, 251, 247, 241, 239, 233, 229, 227, 223, 217, 211, 199, 197}; - - public static class Configuration { - public NeutralMode NEUTRAL_MODE = NeutralMode.Coast; - - // factory default - public double NEUTRAL_DEADBAND = 0.04; - - public boolean ENABLE_SOFT_LIMIT = false; - public boolean ENABLE_LIMIT_SWITCH = false; - public int FORWARD_SOFT_LIMIT = 0; - public int REVERSE_SOFT_LIMIT = 0; - - public double PEAK_OUTPUT_FORWARD = 1.0; - public double PEAK_OUTPUT_REVERSE = -1.0; - - public boolean INVERTED = false; - public boolean SENSOR_PHASE = false; - public SensorInitializationStrategy SENSOR_INITIALIZATION_STRATEGY = - SensorInitializationStrategy.BootToZero; - - public int CONTROL_FRAME_PERIOD_MS = 20; // 10 - public int MOTION_CONTROL_FRAME_PERIOD_MS = 3; - - public int GENERAL_STATUS_FRAME_RATE_MS = 10; - public int FEEDBACK_STATUS_FRAME_RATE_MS = 49; - public int QUAD_ENCODER_STATUS_FRAME_RATE_MS = kPrimePeriods[0]; - public int ANALOG_TEMP_VBAT_STATUS_FRAME_RATE_MS = kPrimePeriods[1]; - public int PULSE_WIDTH_STATUS_FRAME_RATE_MS = kPrimePeriods[2]; - public int MOTION_MAGIC_STATUS_FRAME_RATE_MS = kPrimePeriods[3]; - public int FEEDBACK_1_STATUS_FRAME_RATE_MS = kPrimePeriods[4]; - public int BASE_PIDF0_STATUS_FRAME_RATE_MS = kPrimePeriods[5]; - public int TURN_PIDF1_STATUS_FRAME_RATE_MS = kPrimePeriods[6]; - public int FEEDBACK_INTEGRATED_STATUS_FRAME_RATE_MS = kPrimePeriods[7]; - public int BRUSHLESS_CURRENT_STATUS_FRAME_RATE_MS = kPrimePeriods[8]; - - public SensorVelocityMeasPeriod VELOCITY_MEASUREMENT_PERIOD = - SensorVelocityMeasPeriod.Period_100Ms; - public int VELOCITY_MEASUREMENT_ROLLING_AVERAGE_WINDOW = 64; - - public StatorCurrentLimitConfiguration STATOR_CURRENT_LIMIT = - new StatorCurrentLimitConfiguration(false, 300, 700, 1); - public SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT = - new SupplyCurrentLimitConfiguration(false, 40, 100, 1); - - public double OPEN_LOOP_RAMP_RATE = 0.0; - public double CLOSED_LOOP_RAMP_RATE = 0.0; - - public double SLOT0_KP = 0.0; - public double SLOT0_KI = 0.0; - public double SLOT0_KD = 0.0; - public double SLOT0_KF = 0.0; - - public int REMOTE_SENSOR_DEVICE_ID = 0; - public RemoteSensorSource REMOTE_SENSOR_SOURCE = RemoteSensorSource.Off; - - public double MOTION_ACCELERATION = 0.0; - public double MOTION_CRUISE_VELOCITY = 0.0; - public int MOTION_CURVE_STRENGTH = 0; - } - - private static final Configuration kDefaultConfiguration = new Configuration(); - private static final Configuration kFollowerConfiguration = new Configuration(); - - static { - // This control frame value seems to need to be something reasonable to avoid the Talon's - // LEDs behaving erratically. Potentially try to increase as much as possible. - kFollowerConfiguration.CONTROL_FRAME_PERIOD_MS = 100; - kFollowerConfiguration.MOTION_CONTROL_FRAME_PERIOD_MS = 1000; - kFollowerConfiguration.GENERAL_STATUS_FRAME_RATE_MS = 1000; - kFollowerConfiguration.FEEDBACK_STATUS_FRAME_RATE_MS = 1000; - kFollowerConfiguration.QUAD_ENCODER_STATUS_FRAME_RATE_MS = 1000; - kFollowerConfiguration.ANALOG_TEMP_VBAT_STATUS_FRAME_RATE_MS = 1000; - kFollowerConfiguration.PULSE_WIDTH_STATUS_FRAME_RATE_MS = 1000; - kFollowerConfiguration.ENABLE_SOFT_LIMIT = false; - } - - // create a CANTalon with the default (out of the box) configuration - public static TalonFX createDefaultTalon(int id, String canBusName) { - return createTalon(id, canBusName, kDefaultConfiguration); - } - - public static TalonFX createPermanentFollowerTalon(int id, String canBusName, int leaderID) { - final TalonFX talon = createTalon(id, canBusName, kFollowerConfiguration); - talon.set(ControlMode.Follower, leaderID); - return talon; - } - - public static TalonFX createTalon(int id, String canBusName, Configuration config) { - TalonFX talon = new TalonFX(id, canBusName); - TalonFXConfiguration talonFXConfig = new TalonFXConfiguration(); - - talon.configFactoryDefault(); - - talon.set(ControlMode.PercentOutput, 0.0); - - talonFXConfig.forwardLimitSwitchSource = LimitSwitchSource.FeedbackConnector; - talonFXConfig.forwardLimitSwitchNormal = LimitSwitchNormal.Disabled; - talonFXConfig.reverseLimitSwitchSource = LimitSwitchSource.FeedbackConnector; - talonFXConfig.reverseLimitSwitchNormal = LimitSwitchNormal.Disabled; - - talonFXConfig.clearPositionOnLimitF = false; - talonFXConfig.clearPositionOnLimitR = false; - - talonFXConfig.nominalOutputForward = 0.0; - talonFXConfig.nominalOutputReverse = 0.0; - talonFXConfig.neutralDeadband = config.NEUTRAL_DEADBAND; - - talonFXConfig.peakOutputForward = config.PEAK_OUTPUT_FORWARD; - talonFXConfig.peakOutputReverse = config.PEAK_OUTPUT_REVERSE; - - talonFXConfig.forwardSoftLimitThreshold = config.FORWARD_SOFT_LIMIT; - talonFXConfig.forwardSoftLimitEnable = config.ENABLE_SOFT_LIMIT; - talonFXConfig.reverseSoftLimitThreshold = config.REVERSE_SOFT_LIMIT; - talonFXConfig.reverseSoftLimitEnable = config.ENABLE_SOFT_LIMIT; - - talonFXConfig.initializationStrategy = config.SENSOR_INITIALIZATION_STRATEGY; - - talonFXConfig.velocityMeasurementPeriod = config.VELOCITY_MEASUREMENT_PERIOD; - talonFXConfig.velocityMeasurementWindow = config.VELOCITY_MEASUREMENT_ROLLING_AVERAGE_WINDOW; - - talonFXConfig.openloopRamp = config.OPEN_LOOP_RAMP_RATE; - talonFXConfig.closedloopRamp = config.CLOSED_LOOP_RAMP_RATE; - - talonFXConfig.voltageCompSaturation = 0.0; - talonFXConfig.voltageMeasurementFilter = 32; - - talonFXConfig.statorCurrLimit = config.STATOR_CURRENT_LIMIT; - talonFXConfig.supplyCurrLimit = config.SUPPLY_CURRENT_LIMIT; - - talonFXConfig.slot0.kP = config.SLOT0_KP; - talonFXConfig.slot0.kI = config.SLOT0_KI; - talonFXConfig.slot0.kD = config.SLOT0_KD; - talonFXConfig.slot0.kF = config.SLOT0_KF; - - talonFXConfig.motionAcceleration = config.MOTION_ACCELERATION; - talonFXConfig.motionCruiseVelocity = config.MOTION_CRUISE_VELOCITY; - talonFXConfig.motionCurveStrength = config.MOTION_CURVE_STRENGTH; - - talonFXConfig.remoteFilter0.remoteSensorDeviceID = config.REMOTE_SENSOR_DEVICE_ID; - talonFXConfig.remoteFilter0.remoteSensorSource = config.REMOTE_SENSOR_SOURCE; - - talon.configAllSettings(talonFXConfig, TIMEOUT_MS); - - talon.configSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor); - - talon.changeMotionControlFramePeriod(config.MOTION_CONTROL_FRAME_PERIOD_MS); - talon.clearMotionProfileHasUnderrun(TIMEOUT_MS); - talon.clearMotionProfileTrajectories(); - - talon.clearStickyFaults(TIMEOUT_MS); - - talon.overrideLimitSwitchesEnable(config.ENABLE_LIMIT_SWITCH); - - talon.setNeutralMode(config.NEUTRAL_MODE); - - talon.overrideSoftLimitsEnable(config.ENABLE_SOFT_LIMIT); - - talon.setSensorPhase(config.SENSOR_PHASE); - talon.setInverted(config.INVERTED); - - talon.selectProfileSlot(0, 0); - - talon.enableVoltageCompensation(false); - - talon.setStatusFramePeriod( - StatusFrameEnhanced.Status_1_General, config.GENERAL_STATUS_FRAME_RATE_MS, TIMEOUT_MS); - talon.setStatusFramePeriod( - StatusFrameEnhanced.Status_2_Feedback0, config.FEEDBACK_STATUS_FRAME_RATE_MS, TIMEOUT_MS); - talon.setStatusFramePeriod( - StatusFrameEnhanced.Status_3_Quadrature, - config.QUAD_ENCODER_STATUS_FRAME_RATE_MS, - TIMEOUT_MS); - talon.setStatusFramePeriod( - StatusFrameEnhanced.Status_4_AinTempVbat, - config.ANALOG_TEMP_VBAT_STATUS_FRAME_RATE_MS, - TIMEOUT_MS); - talon.setStatusFramePeriod( - StatusFrameEnhanced.Status_8_PulseWidth, - config.PULSE_WIDTH_STATUS_FRAME_RATE_MS, - TIMEOUT_MS); - talon.setStatusFramePeriod( - StatusFrameEnhanced.Status_10_MotionMagic, - config.MOTION_MAGIC_STATUS_FRAME_RATE_MS, - TIMEOUT_MS); - talon.setStatusFramePeriod( - StatusFrameEnhanced.Status_12_Feedback1, - config.FEEDBACK_1_STATUS_FRAME_RATE_MS, - TIMEOUT_MS); - talon.setStatusFramePeriod( - StatusFrameEnhanced.Status_13_Base_PIDF0, - config.BASE_PIDF0_STATUS_FRAME_RATE_MS, - TIMEOUT_MS); - talon.setStatusFramePeriod( - StatusFrameEnhanced.Status_14_Turn_PIDF1, - config.TURN_PIDF1_STATUS_FRAME_RATE_MS, - TIMEOUT_MS); - talon.setStatusFramePeriod( - StatusFrameEnhanced.Status_21_FeedbackIntegrated, - config.FEEDBACK_INTEGRATED_STATUS_FRAME_RATE_MS, - TIMEOUT_MS); - talon.setStatusFramePeriod( - StatusFrameEnhanced.Status_Brushless_Current, - config.BRUSHLESS_CURRENT_STATUS_FRAME_RATE_MS, - TIMEOUT_MS); - - talon.setControlFramePeriod(ControlFrame.Control_3_General, config.CONTROL_FRAME_PERIOD_MS); - - return talon; - } - - private TalonFXFactory() {} -} diff --git a/src/main/java/frc/lib/team254/drivers/TalonSRXFactory.java b/src/main/java/frc/lib/team254/drivers/TalonSRXFactory.java deleted file mode 100644 index 0e15abdc..00000000 --- a/src/main/java/frc/lib/team254/drivers/TalonSRXFactory.java +++ /dev/null @@ -1,175 +0,0 @@ -/* - * Initially from https://github.com/frc1678/C2022 - */ - -package frc.lib.team254.drivers; - -import com.ctre.phoenix.motorcontrol.*; -import com.ctre.phoenix.motorcontrol.can.TalonSRX; -import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration; -import com.ctre.phoenix.sensors.SensorVelocityMeasPeriod; - -@java.lang.SuppressWarnings({"java:S1104", "java:S116"}) - -/** - * Creates CANTalon objects and configures all the parameters we care about to factory defaults. - * Closed-loop and sensor parameters are not set, as these are expected to be set by the - * application. - */ -public class TalonSRXFactory { - - private static final int TIMEOUT_MS = 100; - - public static class Configuration { - public NeutralMode NEUTRAL_MODE = NeutralMode.Coast; - // factory default - public double NEUTRAL_DEADBAND = 0.04; - - public boolean ENABLE_SOFT_LIMIT = false; - public boolean ENABLE_LIMIT_SWITCH = false; - public int FORWARD_SOFT_LIMIT = 0; - public int REVERSE_SOFT_LIMIT = 0; - - public boolean INVERTED = false; - public boolean SENSOR_PHASE = false; - - public int CONTROL_FRAME_PERIOD_MS = 5; - public int MOTION_CONTROL_FRAME_PERIOD_MS = 3; - public int GENERAL_STATUS_FRAME_RATE_MS = 5; - public int FEEDBACK_STATUS_FRAME_RATE_MS = 100; - public int QUAD_ENCODER_STATUS_FRAME_RATE_MS = 1000; - public int ANALOG_TEMP_VBAT_STATUS_FRAME_RATE_MS = 1000; - public int PULSE_WIDTH_STATUS_FRAME_RATE_MS = 1000; - - public SensorVelocityMeasPeriod VELOCITY_MEASUREMENT_PERIOD = - SensorVelocityMeasPeriod.Period_100Ms; - public int VELOCITY_MEASUREMENT_ROLLING_AVERAGE_WINDOW = 64; - - public SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT = - new SupplyCurrentLimitConfiguration(false, 0, 40, 0.2); - - public double OPEN_LOOP_RAMP_RATE = 0.0; - public double CLOSED_LOOP_RAMP_RATE = 0.0; - - public double SLOT0_KP = 0.0; - public double SLOT0_KI = 0.0; - public double SLOT0_KD = 0.0; - public double SLOT0_KF = 0.0; - } - - private static final Configuration kDefaultConfiguration = new Configuration(); - private static final Configuration kFollowerConfiguration = new Configuration(); - - static { - // This control frame value seems to need to be something reasonable to avoid the Talon's - // LEDs behaving erratically. Potentially try to increase as much as possible. - kFollowerConfiguration.CONTROL_FRAME_PERIOD_MS = 100; - kFollowerConfiguration.MOTION_CONTROL_FRAME_PERIOD_MS = 1000; - kFollowerConfiguration.GENERAL_STATUS_FRAME_RATE_MS = 1000; - kFollowerConfiguration.FEEDBACK_STATUS_FRAME_RATE_MS = 1000; - kFollowerConfiguration.QUAD_ENCODER_STATUS_FRAME_RATE_MS = 1000; - kFollowerConfiguration.ANALOG_TEMP_VBAT_STATUS_FRAME_RATE_MS = 1000; - kFollowerConfiguration.PULSE_WIDTH_STATUS_FRAME_RATE_MS = 1000; - kFollowerConfiguration.ENABLE_SOFT_LIMIT = false; - } - - // create a CANTalon with the default (out of the box) configuration - public static TalonSRX createDefaultTalon(int id) { - return createTalon(id, kDefaultConfiguration); - } - - public static TalonSRX createPermanentFollowerTalon(int id, int leaderID) { - final TalonSRX talon = createTalon(id, kFollowerConfiguration); - talon.set(ControlMode.Follower, leaderID); - return talon; - } - - public static TalonSRX createTalon(int id, Configuration config) { - TalonSRX talon = new LazyTalonSRX(id); - TalonSRXConfiguration talonSRXConfig = new TalonSRXConfiguration(); - - talon.configFactoryDefault(); - - talon.set(ControlMode.PercentOutput, 0.0); - - talonSRXConfig.forwardLimitSwitchSource = LimitSwitchSource.FeedbackConnector; - talonSRXConfig.forwardLimitSwitchNormal = LimitSwitchNormal.Disabled; - talonSRXConfig.reverseLimitSwitchSource = LimitSwitchSource.FeedbackConnector; - talonSRXConfig.reverseLimitSwitchNormal = LimitSwitchNormal.Disabled; - - talonSRXConfig.clearPositionOnLimitF = false; - talonSRXConfig.clearPositionOnLimitR = false; - - talonSRXConfig.nominalOutputForward = 0.0; - talonSRXConfig.nominalOutputReverse = 0.0; - talonSRXConfig.neutralDeadband = config.NEUTRAL_DEADBAND; - - talonSRXConfig.peakOutputForward = 1.0; - talonSRXConfig.peakOutputReverse = -1.0; - - talonSRXConfig.forwardSoftLimitThreshold = config.FORWARD_SOFT_LIMIT; - talonSRXConfig.forwardSoftLimitEnable = config.ENABLE_SOFT_LIMIT; - talonSRXConfig.reverseSoftLimitThreshold = config.REVERSE_SOFT_LIMIT; - talonSRXConfig.reverseSoftLimitEnable = config.ENABLE_SOFT_LIMIT; - - talonSRXConfig.velocityMeasurementPeriod = config.VELOCITY_MEASUREMENT_PERIOD; - talonSRXConfig.velocityMeasurementWindow = config.VELOCITY_MEASUREMENT_ROLLING_AVERAGE_WINDOW; - - talonSRXConfig.openloopRamp = config.OPEN_LOOP_RAMP_RATE; - talonSRXConfig.closedloopRamp = config.CLOSED_LOOP_RAMP_RATE; - - talonSRXConfig.voltageCompSaturation = 0.0; - talonSRXConfig.voltageMeasurementFilter = 32; - - talonSRXConfig.slot0.kP = config.SLOT0_KP; - talonSRXConfig.slot0.kI = config.SLOT0_KI; - talonSRXConfig.slot0.kD = config.SLOT0_KD; - talonSRXConfig.slot0.kF = config.SLOT0_KF; - - talon.configAllSettings(talonSRXConfig, TIMEOUT_MS); - - talon.changeMotionControlFramePeriod(config.MOTION_CONTROL_FRAME_PERIOD_MS); - talon.clearMotionProfileHasUnderrun(TIMEOUT_MS); - talon.clearMotionProfileTrajectories(); - - talon.clearStickyFaults(TIMEOUT_MS); - - talon.overrideLimitSwitchesEnable(config.ENABLE_LIMIT_SWITCH); - - talon.setNeutralMode(config.NEUTRAL_MODE); - - talon.overrideSoftLimitsEnable(config.ENABLE_SOFT_LIMIT); - - talon.setInverted(config.INVERTED); - talon.setSensorPhase(config.SENSOR_PHASE); - - talon.selectProfileSlot(0, 0); - - talon.enableVoltageCompensation(false); - - talon.configSupplyCurrentLimit(config.SUPPLY_CURRENT_LIMIT); - - talon.setStatusFramePeriod( - StatusFrameEnhanced.Status_1_General, config.GENERAL_STATUS_FRAME_RATE_MS, TIMEOUT_MS); - talon.setStatusFramePeriod( - StatusFrameEnhanced.Status_2_Feedback0, config.FEEDBACK_STATUS_FRAME_RATE_MS, TIMEOUT_MS); - talon.setStatusFramePeriod( - StatusFrameEnhanced.Status_3_Quadrature, - config.QUAD_ENCODER_STATUS_FRAME_RATE_MS, - TIMEOUT_MS); - talon.setStatusFramePeriod( - StatusFrameEnhanced.Status_4_AinTempVbat, - config.ANALOG_TEMP_VBAT_STATUS_FRAME_RATE_MS, - TIMEOUT_MS); - talon.setStatusFramePeriod( - StatusFrameEnhanced.Status_8_PulseWidth, - config.PULSE_WIDTH_STATUS_FRAME_RATE_MS, - TIMEOUT_MS); - - talon.setControlFramePeriod(ControlFrame.Control_3_General, config.CONTROL_FRAME_PERIOD_MS); - - return talon; - } - - private TalonSRXFactory() {} -} diff --git a/src/main/java/frc/lib/team254/drivers/TalonUtil.java b/src/main/java/frc/lib/team254/drivers/TalonUtil.java deleted file mode 100644 index 1b888b77..00000000 --- a/src/main/java/frc/lib/team254/drivers/TalonUtil.java +++ /dev/null @@ -1,24 +0,0 @@ -/* - * Initially from https://github.com/frc1678/C2022 - */ - -package frc.lib.team254.drivers; - -import com.ctre.phoenix.ErrorCode; -import edu.wpi.first.wpilibj.DriverStation; - -public class TalonUtil { - /** - * checks the specified error code for issues - * - * @param errorCode error code - * @param message message to print if error happens - */ - public static void checkError(ErrorCode errorCode, String message) { - if (errorCode != ErrorCode.OK) { - DriverStation.reportError(message + errorCode, false); - } - } - - private TalonUtil() {} -} diff --git a/src/main/java/frc/lib/team3015/subsystem/FaultReporter.java b/src/main/java/frc/lib/team3015/subsystem/FaultReporter.java index 7e89cec2..0c55f140 100644 --- a/src/main/java/frc/lib/team3015/subsystem/FaultReporter.java +++ b/src/main/java/frc/lib/team3015/subsystem/FaultReporter.java @@ -8,7 +8,7 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.motorcontrol.PWMMotorController; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; import frc.lib.team3015.subsystem.selfcheck.*; @@ -54,28 +54,27 @@ public static FaultReporter getInstance() { return instance; } - public CommandBase registerSystemCheck(String subsystemName, CommandBase systemCheckCommand) { + public Command registerSystemCheck(String subsystemName, Command systemCheckCommand) { String statusTable = SYSTEM_STATUS + subsystemName; SubsystemFaults subsystemFaults = subsystemsFaults.getOrDefault(subsystemName, new SubsystemFaults()); - CommandBase wrappedSystemCheckCommand = - wrapSystemCheckCommand(subsystemName, systemCheckCommand); + Command wrappedSystemCheckCommand = wrapSystemCheckCommand(subsystemName, systemCheckCommand); wrappedSystemCheckCommand.setName(subsystemName + "Check"); SmartDashboard.putData(statusTable + "/SystemCheck", wrappedSystemCheckCommand); - Logger.getInstance().recordOutput(statusTable + CHECK_RAN, false); + Logger.recordOutput(statusTable + CHECK_RAN, false); subsystemsFaults.put(subsystemName, subsystemFaults); return wrappedSystemCheckCommand; } - private CommandBase wrapSystemCheckCommand(String subsystemName, CommandBase systemCheckCommand) { + private Command wrapSystemCheckCommand(String subsystemName, Command systemCheckCommand) { String statusTable = SYSTEM_STATUS + subsystemName; return Commands.sequence( Commands.runOnce( () -> { - Logger.getInstance().recordOutput(statusTable + CHECK_RAN, false); + Logger.recordOutput(statusTable + CHECK_RAN, false); clearFaults(subsystemName); publishStatus(); }), @@ -83,7 +82,7 @@ private CommandBase wrapSystemCheckCommand(String subsystemName, CommandBase sys Commands.runOnce( () -> { publishStatus(); - Logger.getInstance().recordOutput(statusTable + CHECK_RAN, true); + Logger.recordOutput(statusTable + CHECK_RAN, true); })); } @@ -109,21 +108,20 @@ private void publishStatus() { SystemStatus status = getSystemStatus(subsystemFaults.faults); String statusTable = SYSTEM_STATUS + subsystemName; - Logger.getInstance().recordOutput(statusTable + "/Status", status.name()); - Logger.getInstance().recordOutput(statusTable + "/SystemOK", status == SystemStatus.OK); + Logger.recordOutput(statusTable + "/Status", status.name()); + Logger.recordOutput(statusTable + "/SystemOK", status == SystemStatus.OK); String[] faultStrings = new String[subsystemFaults.faults.size()]; for (int i = 0; i < subsystemFaults.faults.size(); i++) { SubsystemFault fault = subsystemFaults.faults.get(i); faultStrings[i] = String.format("[%.2f] %s", fault.timestamp, fault.description); } - Logger.getInstance().recordOutput(statusTable + "/Faults", faultStrings); + Logger.recordOutput(statusTable + "/Faults", faultStrings); if (faultStrings.length > 0) { - Logger.getInstance() - .recordOutput(statusTable + "/LastFault", faultStrings[faultStrings.length - 1]); + Logger.recordOutput(statusTable + "/LastFault", faultStrings[faultStrings.length - 1]); } else { - Logger.getInstance().recordOutput(statusTable + "/LastFault", ""); + Logger.recordOutput(statusTable + "/LastFault", ""); } } } diff --git a/src/main/java/frc/lib/team3015/subsystem/selfcheck/SelfCheckingCANCoder.java b/src/main/java/frc/lib/team3015/subsystem/selfcheck/SelfCheckingCANCoder.java index a0ed4ae0..d5acd292 100644 --- a/src/main/java/frc/lib/team3015/subsystem/selfcheck/SelfCheckingCANCoder.java +++ b/src/main/java/frc/lib/team3015/subsystem/selfcheck/SelfCheckingCANCoder.java @@ -15,7 +15,7 @@ public class SelfCheckingCANCoder implements SelfChecking { public SelfCheckingCANCoder(String label, CANcoder canCoder) { this.label = label; this.canCoder = canCoder; - this.statusSignal = this.canCoder.getSupplyVoltage(); + this.statusSignal = this.canCoder.getSupplyVoltage().clone(); } @Override @@ -40,7 +40,7 @@ public List checkForFaults() { } this.statusSignal.refresh(); - if (this.statusSignal.getError() != StatusCode.OK) { + if (this.statusSignal.getStatus() != StatusCode.OK) { faults.add(new SubsystemFault(String.format("[%s]: device is unreachable", label))); } diff --git a/src/main/java/frc/lib/team3015/subsystem/selfcheck/SelfCheckingPhoenixMotor.java b/src/main/java/frc/lib/team3015/subsystem/selfcheck/SelfCheckingPhoenixMotor.java index 43ef1ace..297908aa 100644 --- a/src/main/java/frc/lib/team3015/subsystem/selfcheck/SelfCheckingPhoenixMotor.java +++ b/src/main/java/frc/lib/team3015/subsystem/selfcheck/SelfCheckingPhoenixMotor.java @@ -15,7 +15,7 @@ public class SelfCheckingPhoenixMotor implements SelfChecking { public SelfCheckingPhoenixMotor(String label, TalonFX motor) { this.label = label; this.motor = motor; - this.statusSignal = this.motor.getSupplyVoltage(); + this.statusSignal = this.motor.getSupplyVoltage().clone(); } @Override @@ -36,9 +36,6 @@ public List checkForFaults() { if (motor.getFault_FusedSensorOutOfSync().getValue() == Boolean.TRUE) { faults.add(new SubsystemFault(String.format("[%s]: Remote sensor is out of sync", label))); } - if (motor.getFault_MissingRemoteSensor().getValue() == Boolean.TRUE) { - faults.add(new SubsystemFault(String.format("[%s]: Remote sensor is missing", label))); - } if (motor.getFault_OverSupplyV().getValue() == Boolean.TRUE) { faults.add(new SubsystemFault(String.format("[%s]: Supply voltage exceeded limit", label))); } @@ -58,7 +55,7 @@ public List checkForFaults() { } this.statusSignal.refresh(); - if (this.statusSignal.getError() != StatusCode.OK) { + if (this.statusSignal.getStatus() != StatusCode.OK) { faults.add(new SubsystemFault(String.format("[%s]: device is unreachable", label))); } diff --git a/src/main/java/frc/lib/team3015/subsystem/selfcheck/SelfCheckingPigeon2.java b/src/main/java/frc/lib/team3015/subsystem/selfcheck/SelfCheckingPigeon2.java index 4aac721a..0f1cab4d 100644 --- a/src/main/java/frc/lib/team3015/subsystem/selfcheck/SelfCheckingPigeon2.java +++ b/src/main/java/frc/lib/team3015/subsystem/selfcheck/SelfCheckingPigeon2.java @@ -15,7 +15,7 @@ public class SelfCheckingPigeon2 implements SelfChecking { public SelfCheckingPigeon2(String label, Pigeon2 pigeon) { this.label = label; this.pigeon = pigeon; - this.statusSignal = this.pigeon.getSupplyVoltage(); + this.statusSignal = this.pigeon.getSupplyVoltage().clone(); } @Override @@ -57,10 +57,10 @@ public List checkForFaults() { faults.add( new SubsystemFault(String.format("[%s]: Accelerometer values are saturated", label))); } - if (pigeon.getFault_SaturatedGyrosscope().getValue() == Boolean.TRUE) { + if (pigeon.getFault_SaturatedGyroscope().getValue() == Boolean.TRUE) { faults.add(new SubsystemFault(String.format("[%s]: Gyro values are saturated", label))); } - if (pigeon.getFault_SaturatedMagneter().getValue() == Boolean.TRUE) { + if (pigeon.getFault_SaturatedMagnetometer().getValue() == Boolean.TRUE) { faults.add( new SubsystemFault(String.format("[%s]: Magnetometer values are saturated", label))); } @@ -73,7 +73,7 @@ public List checkForFaults() { } this.statusSignal.refresh(); - if (this.statusSignal.getError() != StatusCode.OK) { + if (this.statusSignal.getStatus() != StatusCode.OK) { faults.add(new SubsystemFault(String.format("[%s]: device is unreachable", label))); } diff --git a/src/main/java/frc/lib/team3061/RobotConfig.java b/src/main/java/frc/lib/team3061/RobotConfig.java index 3f1aca55..0977b0cb 100644 --- a/src/main/java/frc/lib/team3061/RobotConfig.java +++ b/src/main/java/frc/lib/team3061/RobotConfig.java @@ -3,7 +3,7 @@ import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import frc.lib.team3061.swerve.SwerveModuleConstants.SwerveType; +import frc.lib.team3061.drivetrain.swerve.SwerveConstants.SwerveType; @java.lang.SuppressWarnings({"java:S3010", "java:S3400"}) public abstract class RobotConfig { @@ -612,4 +612,8 @@ public double getDriveToPoseThetaTolerance() { public double getMoveToPathFinalVelocity() { return 0; } + + public double getOdometryUpdateFrequency() { + return 250.0; + } } diff --git a/src/main/java/frc/lib/team3061/drivers/SparkMAXFactory.java b/src/main/java/frc/lib/team3061/drivers/SparkMAXFactory.java deleted file mode 100644 index a7042740..00000000 --- a/src/main/java/frc/lib/team3061/drivers/SparkMAXFactory.java +++ /dev/null @@ -1,110 +0,0 @@ -package frc.lib.team3061.drivers; - -import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; -import com.revrobotics.CANSparkMaxLowLevel.PeriodicFrame; - -@java.lang.SuppressWarnings({"java:S1104", "java:S116"}) - -/** - * Creates SparkMAX objects and configures all the parameters we care about to factory defaults. - * Closed-loop and sensor parameters are not set, as these are expected to be set by the - * application. - */ -public class SparkMAXFactory { - - // These periods don't share any common factors, so they shouldn't run at the same time. 255 is - // max. (initially from https://github.com/Mechanical-Advantage/RobotCode2022) - private static int[] kPrimePeriods = - new int[] {255, 254, 253, 251, 247, 241, 239, 233, 229, 227, 223, 217, 211, 199, 197}; - - public static class Configuration { - public boolean ENABLE_VOLTAGE_COMPENSATION = true; - public double VOLTAGE_COMPENSATION = 12.0; - - public boolean ENABLE_SOFT_LIMIT = false; - public int FORWARD_SOFT_LIMIT = 0; - public int REVERSE_SOFT_LIMIT = 0; - - public double CLOSED_LOOP_RAMP_RATE = 0.0; - public double OPEN_LOOP_RAMP_RATE = 0.0; - - public CANSparkMax.IdleMode IDLE_MODE = CANSparkMax.IdleMode.kCoast; - - public boolean INVERTED = false; - - public int SMART_FREE_CURRENT_LIMIT = 30; - public int SMART_STALL_CURRENT_LIMIT = 20; - - public int CAN_TIMEOUT_MS = 0; - - public int STATUS0_FRAME_RATE_MS = kPrimePeriods[0]; - public int STATUS1_FRAME_RATE_MS = kPrimePeriods[1]; - public int STATUS2_FRAME_RATE_MS = kPrimePeriods[2]; - public int STATUS3_FRAME_RATE_MS = kPrimePeriods[3]; - public int STATUS4_FRAME_RATE_MS = kPrimePeriods[4]; - public int STATUS5_FRAME_RATE_MS = kPrimePeriods[5]; - public int STATUS6_FRAME_RATE_MS = kPrimePeriods[6]; - public int CONTROL_FRAME_RATE_MS = 10; - } - - private static final Configuration kDefaultConfiguration = new Configuration(); - private static final Configuration kFollowerConfiguration = new Configuration(); - - // create a CANTalon with the default (out of the box) configuration - public static CANSparkMax createDefaultSparkMax(int id) { - return createSparkMax(id, kDefaultConfiguration); - } - - public static CANSparkMax createPermanentFollowerTalon(int id, CANSparkMax leader) { - final CANSparkMax sparkMax = createSparkMax(id, kFollowerConfiguration); - sparkMax.follow(leader); - return sparkMax; - } - - public static CANSparkMax createSparkMax(int id, Configuration config) { - CANSparkMax sparkMax = new CANSparkMax(id, MotorType.kBrushless); - - sparkMax.restoreFactoryDefaults(); - - sparkMax.setCANTimeout(config.CAN_TIMEOUT_MS); - - if (config.ENABLE_VOLTAGE_COMPENSATION) { - sparkMax.enableVoltageCompensation(config.VOLTAGE_COMPENSATION); - } - - if (config.ENABLE_SOFT_LIMIT) { - sparkMax.setSoftLimit(CANSparkMax.SoftLimitDirection.kForward, config.FORWARD_SOFT_LIMIT); - sparkMax.setSoftLimit(CANSparkMax.SoftLimitDirection.kReverse, config.REVERSE_SOFT_LIMIT); - } - - sparkMax.setClosedLoopRampRate(config.CLOSED_LOOP_RAMP_RATE); - sparkMax.setOpenLoopRampRate(config.OPEN_LOOP_RAMP_RATE); - - sparkMax.setIdleMode(config.IDLE_MODE); - - sparkMax.setInverted(config.INVERTED); - - sparkMax.setSmartCurrentLimit( - config.SMART_STALL_CURRENT_LIMIT, config.SMART_FREE_CURRENT_LIMIT); - - sparkMax.clearFaults(); - - sparkMax.setPeriodicFramePeriod(PeriodicFrame.kStatus0, config.STATUS0_FRAME_RATE_MS); - sparkMax.setPeriodicFramePeriod(PeriodicFrame.kStatus1, config.STATUS1_FRAME_RATE_MS); - sparkMax.setPeriodicFramePeriod(PeriodicFrame.kStatus2, config.STATUS2_FRAME_RATE_MS); - sparkMax.setPeriodicFramePeriod(PeriodicFrame.kStatus3, config.STATUS3_FRAME_RATE_MS); - sparkMax.setPeriodicFramePeriod(PeriodicFrame.kStatus4, config.STATUS4_FRAME_RATE_MS); - sparkMax.setPeriodicFramePeriod(PeriodicFrame.kStatus5, config.STATUS5_FRAME_RATE_MS); - sparkMax.setPeriodicFramePeriod(PeriodicFrame.kStatus6, config.STATUS6_FRAME_RATE_MS); - - sparkMax.setControlFramePeriodMs(config.CONTROL_FRAME_RATE_MS); - - // invoke set last which updates control frame period - sparkMax.set(0.0); - - return sparkMax; - } - - private SparkMAXFactory() {} -} diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/frc/lib/team3061/drivetrain/Drivetrain.java similarity index 53% rename from src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java rename to src/main/java/frc/lib/team3061/drivetrain/Drivetrain.java index f1bf16a9..83928de0 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/frc/lib/team3061/drivetrain/Drivetrain.java @@ -2,45 +2,35 @@ // 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.drivetrain; +package frc.lib.team3061.drivetrain; +import static frc.lib.team3061.drivetrain.DrivetrainConstants.*; import static frc.robot.Constants.*; -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusSignal; -import com.pathplanner.lib.PathPlannerTrajectory.PathPlannerState; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.util.HolonomicPathFollowerConfig; +import com.pathplanner.lib.util.PIDConstants; +import com.pathplanner.lib.util.ReplanningConfig; import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; -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.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; -import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.lib.team3015.subsystem.FaultReporter; import frc.lib.team3061.RobotConfig; -import frc.lib.team3061.gyro.GyroIO; -import frc.lib.team3061.gyro.GyroIOInputsAutoLogged; -import frc.lib.team3061.swerve.SwerveModule; -import frc.lib.team3061.util.GeometryUtils; -import frc.lib.team3061.util.RobotOdometry; +import frc.lib.team3061.drivetrain.DrivetrainIO.SwerveIOInputs; import frc.lib.team6328.util.Alert; import frc.lib.team6328.util.Alert.AlertType; import frc.lib.team6328.util.TunableNumber; import frc.robot.Constants; -import java.util.ArrayList; -import java.util.List; import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; @@ -51,8 +41,9 @@ */ public class Drivetrain extends SubsystemBase { - private final GyroIO gyroIO; - private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); + private final DrivetrainIO io; + private final DrivetrainIO.DrivetrainIOInputsCollection inputs = + new DrivetrainIO.DrivetrainIOInputsCollection(); private final TunableNumber autoDriveKp = new TunableNumber("AutoDrive/DriveKp", RobotConfig.getInstance().getAutoDriveKP()); @@ -74,61 +65,11 @@ public class Drivetrain extends SubsystemBase { private final PIDController autoThetaController = new PIDController(autoTurnKp.get(), autoTurnKi.get(), autoTurnKd.get()); - private final double trackwidthMeters = RobotConfig.getInstance().getTrackwidth(); - private final double wheelbaseMeters = RobotConfig.getInstance().getWheelbase(); - private final SwerveDriveKinematics kinematics = - RobotConfig.getInstance().getSwerveDriveKinematics(); - - private final SwerveModule[] swerveModules = new SwerveModule[4]; // FL, FR, BL, BR - - // some of this code is from the SDS example code - - private Translation2d centerGravity; - - private final SwerveModulePosition[] swerveModulePositions = - new SwerveModulePosition[] { - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition() - }; - private final SwerveModulePosition[] prevSwerveModulePositions = - new SwerveModulePosition[] { - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition() - }; - private final SwerveModuleState[] swerveModuleStates = - new SwerveModuleState[] { - new SwerveModuleState(), - new SwerveModuleState(), - new SwerveModuleState(), - new SwerveModuleState() - }; - private final SwerveModuleState[] prevSwerveModuleStates = - new SwerveModuleState[] { - new SwerveModuleState(), - new SwerveModuleState(), - new SwerveModuleState(), - new SwerveModuleState() - }; - private Pose2d estimatedPoseWithoutGyro = new Pose2d(); - private boolean isFieldRelative; private boolean isTranslationSlowMode = false; private boolean isRotationSlowMode = false; - private ChassisSpeeds chassisSpeeds; - - private static final String SUBSYSTEM_NAME = "Drivetrain"; - private static final boolean TESTING = false; - private static final boolean DEBUGGING = false; - - private final SwerveDrivePoseEstimator poseEstimator; - private final List> odometrySignals = new ArrayList<>(); - private boolean brakeMode; private Timer brakeModeTimer = new Timer(); private static final double BREAK_MODE_DELAY_SEC = 10.0; @@ -142,44 +83,21 @@ public class Drivetrain extends SubsystemBase { private Alert noPoseAlert = new Alert("Attempted to reset pose from vision, but no pose was found.", AlertType.WARNING); + private ChassisSpeeds prevSpeeds = new ChassisSpeeds(); + private double[] prevSteerVelocitiesRevPerMin = new double[4]; + /** * Creates a new Drivetrain subsystem. * - * @param gyroIO the abstracted interface for the gyro for the drivetrain - * @param flModule the front left swerve module - * @param frModule the front right swerve module - * @param blModule the back left swerve module - * @param brModule the back right swerve module - */ - public Drivetrain( - GyroIO gyroIO, - SwerveModule flModule, - SwerveModule frModule, - SwerveModule blModule, - SwerveModule brModule) { - this.gyroIO = gyroIO; - this.swerveModules[0] = flModule; - this.swerveModules[1] = frModule; - this.swerveModules[2] = blModule; - this.swerveModules[3] = brModule; + * @param io the abstracted interface for the drivetrain + */ + public Drivetrain(DrivetrainIO io) { + this.io = io; this.autoThetaController.enableContinuousInput(-Math.PI, Math.PI); - this.centerGravity = new Translation2d(); // default to (0,0) - - this.zeroGyroscope(); - this.isFieldRelative = false; - this.chassisSpeeds = new ChassisSpeeds(0.0, 0.0, 0.0); - - this.poseEstimator = RobotOdometry.getInstance().getPoseEstimator(); - - this.odometrySignals.addAll(this.gyroIO.getOdometryStatusSignals()); - for (SwerveModule swerveModule : swerveModules) { - this.odometrySignals.addAll(swerveModule.getOdometryStatusSignals()); - } - // based on testing we can drive in turbo mode all the time this.isTurbo = true; @@ -196,27 +114,53 @@ public Drivetrain( .withPosition(8, 0) .withSize(1, 1); - if (DEBUGGING) { - ShuffleboardTab tab = Shuffleboard.getTab(SUBSYSTEM_NAME); - tab.add(SUBSYSTEM_NAME, this); - tab.addNumber("vx", this::getVelocityX); - tab.addNumber("vy", this::getVelocityY); - tab.addNumber("Pose Est X", () -> poseEstimator.getEstimatedPosition().getX()); - tab.addNumber("Pose Est Y", () -> poseEstimator.getEstimatedPosition().getY()); - tab.addNumber( - "Pose Est Rot", () -> poseEstimator.getEstimatedPosition().getRotation().getDegrees()); - tab.addNumber("CoG X", () -> this.centerGravity.getX()); - tab.addNumber("CoG Y", () -> this.centerGravity.getY()); - } - - if (TESTING) { - ShuffleboardTab tab = Shuffleboard.getTab(SUBSYSTEM_NAME); - tab.add("Enable XStance", new InstantCommand(this::enableXstance)); - tab.add("Disable XStance", new InstantCommand(this::disableXstance)); - } - FaultReporter faultReporter = FaultReporter.getInstance(); faultReporter.registerSystemCheck(SUBSYSTEM_NAME, getSystemCheckCommand()); + + AutoBuilder.configureHolonomic( + this::getPose, // Robot pose supplier + this::resetPose, // Method to reset odometry (will be called if your auto has a starting + // pose) + this::getRobotRelativeSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE + this::driveRobotRelative, // Method that will drive the robot given ROBOT RELATIVE + // ChassisSpeeds + new HolonomicPathFollowerConfig( // HolonomicPathFollowerConfig, this should likely live in + // your Constants class + new PIDConstants( + RobotConfig.getInstance().getAutoDriveKP(), + RobotConfig.getInstance().getAutoDriveKI(), + RobotConfig.getInstance().getAutoDriveKD()), // Translation PID constants + new PIDConstants( + RobotConfig.getInstance().getAutoTurnKP(), + RobotConfig.getInstance().getAutoTurnKI(), + RobotConfig.getInstance().getAutoTurnKD()), // Rotation PID constants + RobotConfig.getInstance().getAutoMaxSpeed(), // Max module speed, in m/s + new Translation2d( + RobotConfig.getInstance().getWheelbase(), + RobotConfig.getInstance().getTrackwidth()) + .getNorm(), // Drive base radius in meters. Distance from robot center to furthest + // module. + new ReplanningConfig() // Default path replanning config. See the API for the options + // here + ), + this // Reference to this subsystem to set requirements + ); + } + + public ChassisSpeeds getRobotRelativeSpeeds() { + return new ChassisSpeeds( + this.inputs.drivetrain.measuredVXMetersPerSec, + this.inputs.drivetrain.measuredVYMetersPerSec, + this.inputs.drivetrain.measuredAngularVelocityRadPerSec); + } + + public void driveRobotRelative(ChassisSpeeds chassisSpeeds) { + this.drive( + chassisSpeeds.vxMetersPerSecond, + chassisSpeeds.vyMetersPerSecond, + chassisSpeeds.omegaRadiansPerSecond, + false, + false); } /** Enables "turbo" mode (i.e., acceleration is not limited by software) */ @@ -258,11 +202,7 @@ public void zeroGyroscope() { * @return the rotation of the robot */ public Rotation2d getRotation() { - if (gyroInputs.connected) { - return Rotation2d.fromDegrees(gyroInputs.yawDeg); - } else { - return estimatedPoseWithoutGyro.getRotation(); - } + return this.inputs.drivetrain.rotation; } /** @@ -272,7 +212,7 @@ public Rotation2d getRotation() { * @return the yaw of the drivetrain as reported by the gyro in degrees */ public double getYaw() { - return gyroInputs.yawDeg; + return this.inputs.gyro.yawDeg; } /** @@ -281,7 +221,7 @@ public double getYaw() { * @return the pitch of the drivetrain as reported by the gyro in degrees */ public double getPitch() { - return gyroInputs.pitchDeg; + return this.inputs.gyro.pitchDeg; } /** @@ -290,7 +230,7 @@ public double getPitch() { * @return the roll of the drivetrain as reported by the gyro in degrees */ public double getRoll() { - return gyroInputs.rollDeg; + return this.inputs.gyro.rollDeg; } /** @@ -301,15 +241,7 @@ public double getRoll() { * @param expectedYaw the rotation of the robot (in degrees) */ public void setGyroOffset(double expectedYaw) { - if (gyroInputs.connected) { - this.gyroIO.setYaw(expectedYaw); - } else { - this.estimatedPoseWithoutGyro = - new Pose2d( - estimatedPoseWithoutGyro.getX(), - estimatedPoseWithoutGyro.getY(), - Rotation2d.fromDegrees(expectedYaw)); - } + this.io.setGyroOffset(expectedYaw); } /** @@ -320,7 +252,7 @@ public void setGyroOffset(double expectedYaw) { * @return the pose of the robot */ public Pose2d getPose() { - return poseEstimator.getEstimatedPosition(); + return this.inputs.drivetrain.robotPose; } /** @@ -331,19 +263,8 @@ public Pose2d getPose() { * * @param state the specified PathPlanner state to which is set the odometry */ - public void resetOdometry(PathPlannerState state) { - setGyroOffset(state.holonomicRotation.getDegrees()); - - for (int i = 0; i < 4; i++) { - swerveModulePositions[i] = swerveModules[i].getPosition(); - } - - estimatedPoseWithoutGyro = - new Pose2d(state.poseMeters.getTranslation(), state.holonomicRotation); - poseEstimator.resetPosition( - this.getRotation(), - swerveModulePositions, - new Pose2d(state.poseMeters.getTranslation(), state.holonomicRotation)); + public void resetPose(Pose2d pose) { + this.io.resetPose(pose); } /** @@ -352,14 +273,7 @@ public void resetOdometry(PathPlannerState state) { * estimator. */ public void resetPoseRotationToGyro() { - for (int i = 0; i < 4; i++) { - swerveModulePositions[i] = swerveModules[i].getPosition(); - } - - poseEstimator.resetPosition( - this.getRotation(), - swerveModulePositions, - new Pose2d(this.getPose().getTranslation(), this.getRotation())); + this.io.resetPose(new Pose2d(this.getPose().getTranslation(), this.getRotation())); } /** @@ -373,8 +287,7 @@ public void resetPoseToVision(Supplier poseSupplier) { Pose3d pose = poseSupplier.get(); if (pose != null) { noPoseAlert.set(false); - setGyroOffset(pose.toPose2d().getRotation().getDegrees()); - poseEstimator.resetPosition(this.getRotation(), swerveModulePositions, pose.toPose2d()); + this.io.resetPose(pose.toPose2d()); } else { noPoseAlert.set(true); } @@ -411,46 +324,31 @@ public void drive( double yVelocity, double rotationalVelocity, boolean isOpenLoop, - boolean overrideFieldRelative) { + boolean isFieldRelative) { if (driveMode == DriveMode.NORMAL) { // get the slow-mode multiplier from the config double slowModeMultiplier = RobotConfig.getInstance().getRobotSlowModeMultiplier(); + // if translation or rotation is in slow mode, multiply the x and y velocities by the // slow-mode multiplier if (isTranslationSlowMode) { xVelocity *= slowModeMultiplier; yVelocity *= slowModeMultiplier; } + // if rotation is in slow mode, multiply the rotational velocity by the slow-mode multiplier if (isRotationSlowMode) { rotationalVelocity *= slowModeMultiplier; } - if (isFieldRelative || overrideFieldRelative) { - chassisSpeeds = - ChassisSpeeds.fromFieldRelativeSpeeds( - xVelocity, yVelocity, rotationalVelocity, getRotation()); - + if (isFieldRelative) { + this.io.driveFieldRelative(xVelocity, yVelocity, rotationalVelocity, isOpenLoop); } else { - chassisSpeeds = new ChassisSpeeds(xVelocity, yVelocity, rotationalVelocity); + this.io.driveRobotRelative(xVelocity, yVelocity, rotationalVelocity, isOpenLoop); } - - chassisSpeeds = convertFromDiscreteChassisSpeedsToContinuous(chassisSpeeds); - - Logger.getInstance() - .recordOutput("Drivetrain/ChassisSpeedVx", chassisSpeeds.vxMetersPerSecond); - Logger.getInstance() - .recordOutput("Drivetrain/ChassisSpeedVy", chassisSpeeds.vyMetersPerSecond); - Logger.getInstance() - .recordOutput("Drivetrain/ChassisSpeedVo", chassisSpeeds.omegaRadiansPerSecond); - - SwerveModuleState[] newSwerveModuleStates = - kinematics.toSwerveModuleStates(chassisSpeeds, centerGravity); - - setSwerveModuleStates(newSwerveModuleStates, isOpenLoop, false); - } else { // X stance - this.setXStance(); + } else { + this.io.holdXStance(); } } @@ -459,9 +357,7 @@ public void drive( * after this method is invoked. */ public void stop() { - chassisSpeeds = new ChassisSpeeds(0.0, 0.0, 0.0); - SwerveModuleState[] states = kinematics.toSwerveModuleStates(chassisSpeeds, centerGravity); - setSwerveModuleStates(states); + this.io.driveRobotRelative(0.0, 0.0, 0.0, false); } /** @@ -472,56 +368,25 @@ public void stop() { */ @Override public void periodic() { - - // synchronize all of the signals related to pose estimation - if (RobotConfig.getInstance().getPhoenix6Licensed()) { - // this is a licensed method - BaseStatusSignal.waitForAll( - Constants.LOOP_PERIOD_SECS, this.odometrySignals.toArray(new BaseStatusSignal[0])); - } - - // update and log gyro inputs - gyroIO.updateInputs(gyroInputs); - Logger.getInstance().processInputs("Drivetrain/Gyro", gyroInputs); - - // update and log the swerve modules inputs - for (SwerveModule swerveModule : swerveModules) { - swerveModule.updateAndProcessInputs(); - } - Logger.getInstance().recordOutput("Drivetrain/AvgDriveCurrent", this.getAverageDriveCurrent()); - - // update swerve module states and positions - for (int i = 0; i < 4; i++) { - prevSwerveModuleStates[i] = swerveModuleStates[i]; - swerveModuleStates[i] = swerveModules[i].getState(); - prevSwerveModulePositions[i] = swerveModulePositions[i]; - swerveModulePositions[i] = swerveModules[i].getPosition(); - } - - // if the gyro is not connected, use the swerve module positions to estimate the robot's - // rotation - if (!gyroInputs.connected || Constants.getMode() == Constants.Mode.SIM) { - SwerveModulePosition[] moduleDeltas = new SwerveModulePosition[4]; - for (int index = 0; index < moduleDeltas.length; index++) { - SwerveModulePosition current = swerveModulePositions[index]; - SwerveModulePosition previous = prevSwerveModulePositions[index]; - - moduleDeltas[index] = - new SwerveModulePosition( - current.distanceMeters - previous.distanceMeters, current.angle); - // FIXME: I don't think this assignment is needed... - previous.distanceMeters = current.distanceMeters; + // FIXME: document that TUNING_MODE must be set to true when doing characterization + if (Constants.TUNING_MODE) { + this.prevSpeeds = + new ChassisSpeeds( + this.inputs.drivetrain.measuredVXMetersPerSec, + this.inputs.drivetrain.measuredVYMetersPerSec, + this.inputs.drivetrain.measuredAngularVelocityRadPerSec); + for (int i = 0; i < this.inputs.swerve.length; i++) { + this.prevSteerVelocitiesRevPerMin[i] = this.inputs.swerve[i].steerVelocityRevPerMin; } - - Twist2d twist = kinematics.toTwist2d(moduleDeltas); - this.gyroIO.addYaw(Math.toDegrees(twist.dtheta)); - - estimatedPoseWithoutGyro = estimatedPoseWithoutGyro.exp(twist); } - // update the pose estimator based on the gyro and swerve module positions - poseEstimator.updateWithTime( - Logger.getInstance().getRealTimestamp() / 1e6, this.getRotation(), swerveModulePositions); + this.io.updateInputs(this.inputs); + Logger.processInputs(SUBSYSTEM_NAME, this.inputs.drivetrain); + Logger.processInputs(SUBSYSTEM_NAME + "/Gyro", this.inputs.gyro); + Logger.processInputs(SUBSYSTEM_NAME + "/FL", this.inputs.swerve[0]); + Logger.processInputs(SUBSYSTEM_NAME + "/FR", this.inputs.swerve[1]); + Logger.processInputs(SUBSYSTEM_NAME + "/BL", this.inputs.swerve[2]); + Logger.processInputs(SUBSYSTEM_NAME + "/BR", this.inputs.swerve[3]); // update the brake mode based on the robot's velocity and state (enabled/disabled) updateBrakeMode(); @@ -534,38 +399,6 @@ public void periodic() { if (autoTurnKp.hasChanged() || autoTurnKi.hasChanged() || autoTurnKd.hasChanged()) { autoThetaController.setPID(autoTurnKp.get(), autoTurnKi.get(), autoTurnKd.get()); } - - // log poses, 3D geometry, and swerve module states, gyro offset - Pose2d poseEstimatorPose = poseEstimator.getEstimatedPosition(); - Logger.getInstance().recordOutput("Odometry/RobotNoGyro", estimatedPoseWithoutGyro); - Logger.getInstance().recordOutput("Odometry/Robot", poseEstimatorPose); - Logger.getInstance().recordOutput("3DField", new Pose3d(poseEstimatorPose)); - Logger.getInstance().recordOutput("SwerveModuleStates/Measured", swerveModuleStates); - } - - /** - * Sets each of the swerve modules based on the specified corresponding swerve module state. - * Incorporates the configured feedforward when setting each swerve module. The order of the - * states in the array must be front left, front right, back left, back right. - * - *

This method is invoked by the FollowPath autonomous command. - * - * @param states the specified swerve module state for each swerve module - */ - public void setSwerveModuleStates(SwerveModuleState[] states) { - setSwerveModuleStates(states, false, false); - } - - private void setSwerveModuleStates( - SwerveModuleState[] states, boolean isOpenLoop, boolean forceAngle) { - SwerveDriveKinematics.desaturateWheelSpeeds( - states, RobotConfig.getInstance().getRobotMaxVelocity()); - - for (SwerveModule swerveModule : swerveModules) { - swerveModule.setDesiredState(states[swerveModule.getModuleNumber()], isOpenLoop, forceAngle); - } - - Logger.getInstance().recordOutput("SwerveModuleStates/Setpoints", states); } /** @@ -625,22 +458,6 @@ public void disableRotationSlowMode() { this.isRotationSlowMode = false; } - /** - * Sets the swerve modules in the x-stance orientation. In this orientation the wheels are aligned - * to make an 'X'. This prevents the robot from rolling on an inclined surface and makes it more - * difficult for other robots to push the robot, which is useful when shooting. - */ - public void setXStance() { - chassisSpeeds = new ChassisSpeeds(0.0, 0.0, 0.0); - SwerveModuleState[] states = kinematics.toSwerveModuleStates(chassisSpeeds, centerGravity); - states[0].angle = new Rotation2d(Math.PI / 2 - Math.atan(trackwidthMeters / wheelbaseMeters)); - states[1].angle = new Rotation2d(Math.PI / 2 + Math.atan(trackwidthMeters / wheelbaseMeters)); - states[2].angle = new Rotation2d(Math.PI / 2 + Math.atan(trackwidthMeters / wheelbaseMeters)); - states[3].angle = - new Rotation2d(3.0 / 2.0 * Math.PI - Math.atan(trackwidthMeters / wheelbaseMeters)); - setSwerveModuleStates(states, true, true); - } - /** * Puts the drivetrain into the x-stance orientation. In this orientation the wheels are aligned * to make an 'X'. This prevents the robot from rolling on an inclined surface and makes it more @@ -649,7 +466,7 @@ public void setXStance() { */ public void enableXstance() { this.driveMode = DriveMode.X; - this.setXStance(); + this.io.holdXStance(); } /** Disables x-stance, allowing the robot to be driven. */ @@ -667,19 +484,19 @@ public boolean isXstance() { } /** - * Sets the robot's center of gravity about which it will rotate. The origin is at the center of - * the robot. The positive x direction is forward; the positive y direction, left. + * Sets the robot's center of rotation. The origin is at the center of the robot. The positive x + * direction is forward; the positive y direction, left. * - * @param x the x coordinate of the robot's center of gravity (in meters) - * @param y the y coordinate of the robot's center of gravity (in meters) + * @param x the x coordinate of the robot's center of rotation (in meters) + * @param y the y coordinate of the robot's center of rotation (in meters) */ - public void setCenterGravity(double x, double y) { - this.centerGravity = new Translation2d(x, y); + public void setCenterOfRotation(double x, double y) { + io.setCenterOfRotation(new Translation2d(x, y)); } - /** Resets the robot's center of gravity about which it will rotate to the center of the robot. */ - public void resetCenterGravity() { - setCenterGravity(0.0, 0.0); + /** Resets the robot's center of rotation to the center of the robot. */ + public void resetCenterOfRotation() { + setCenterOfRotation(0.0, 0.0); } /** @@ -688,7 +505,7 @@ public void resetCenterGravity() { * @return the desired velocity of the drivetrain in the x direction (units of m/s) */ public double getVelocityX() { - return chassisSpeeds.vxMetersPerSecond; + return this.inputs.drivetrain.measuredVXMetersPerSec; } /** @@ -697,7 +514,7 @@ public double getVelocityX() { * @return the desired velocity of the drivetrain in the y direction (units of m/s) */ public double getVelocityY() { - return chassisSpeeds.vyMetersPerSecond; + return this.inputs.drivetrain.measuredVYMetersPerSec; } /** @@ -706,11 +523,7 @@ public double getVelocityY() { * @return the average current of the swerve module drive motors in amps */ public double getAverageDriveCurrent() { - double totalCurrent = 0.0; - for (SwerveModule module : swerveModules) { - totalCurrent += Math.abs(module.getDriveCurrent()); - } - return totalCurrent / swerveModules.length; + return this.inputs.drivetrain.averageDriveCurrent; } /** @@ -719,7 +532,7 @@ public double getAverageDriveCurrent() { * @return the PID controller used to control the robot's x position during autonomous */ public PIDController getAutoXController() { - return autoXController; + return this.autoXController; } /** @@ -728,7 +541,7 @@ public PIDController getAutoXController() { * @return the PID controller used to control the robot's y position during autonomous */ public PIDController getAutoYController() { - return autoYController; + return this.autoYController; } /** @@ -737,7 +550,7 @@ public PIDController getAutoYController() { * @return the PID controller used to control the robot's rotation during autonomous */ public PIDController getAutoThetaController() { - return autoThetaController; + return this.autoThetaController; } /** @@ -746,9 +559,7 @@ public PIDController getAutoThetaController() { * @param volts the commanded voltage */ public void runDriveCharacterizationVolts(double volts) { - for (SwerveModule swerveModule : swerveModules) { - swerveModule.setVoltageForDriveCharacterization(volts); - } + this.io.setDriveMotorVoltage(volts); } /** @@ -757,8 +568,9 @@ public void runDriveCharacterizationVolts(double volts) { * @return the average drive velocity in meters/sec */ public double getDriveCharacterizationVelocity() { - ChassisSpeeds speeds = kinematics.toChassisSpeeds(swerveModuleStates); - return Math.sqrt(Math.pow(speeds.vxMetersPerSecond, 2) + Math.pow(speeds.vyMetersPerSecond, 2)); + return Math.sqrt( + Math.pow(this.inputs.drivetrain.measuredVXMetersPerSec, 2) + + Math.pow(this.inputs.drivetrain.measuredVYMetersPerSec, 2)); } /** @@ -767,11 +579,15 @@ public double getDriveCharacterizationVelocity() { * @return the average drive velocity in meters/sec */ public double getDriveCharacterizationAcceleration() { - ChassisSpeeds prevSpeeds = kinematics.toChassisSpeeds(prevSwerveModuleStates); - ChassisSpeeds speeds = kinematics.toChassisSpeeds(swerveModuleStates); return Math.sqrt( - Math.pow((speeds.vxMetersPerSecond - prevSpeeds.vxMetersPerSecond), 2) - + Math.pow((speeds.vyMetersPerSecond - prevSpeeds.vyMetersPerSecond), 2)) + Math.pow( + (this.inputs.drivetrain.measuredVXMetersPerSec + - this.prevSpeeds.vxMetersPerSecond), + 2) + + Math.pow( + (this.inputs.drivetrain.measuredVYMetersPerSec + - this.prevSpeeds.vyMetersPerSecond), + 2)) / LOOP_PERIOD_SECS; } @@ -781,9 +597,7 @@ public double getDriveCharacterizationAcceleration() { * @param volts the commanded voltage */ public void runRotateCharacterizationVolts(double volts) { - for (SwerveModule swerveModule : swerveModules) { - swerveModule.setVoltageForRotateCharacterization(volts); - } + this.io.setSteerMotorVoltage(volts); } /** @@ -793,12 +607,11 @@ public void runRotateCharacterizationVolts(double volts) { */ public double getRotateCharacterizationVelocity() { double avgVelocity = 0.0; - - for (SwerveModule mod : swerveModules) { - avgVelocity += mod.getAngleMotorVelocity(); + for (SwerveIOInputs swerveInputs : this.inputs.swerve) { + avgVelocity += swerveInputs.steerVelocityRevPerMin; } - - avgVelocity /= swerveModules.length; + avgVelocity /= this.inputs.swerve.length; + avgVelocity *= (2.0 * Math.PI) / 60.0; return avgVelocity; } @@ -809,13 +622,14 @@ public double getRotateCharacterizationVelocity() { */ public double getRotateCharacterizationAcceleration() { double avgAcceleration = 0.0; - - for (SwerveModule mod : swerveModules) { - avgAcceleration += mod.getAngleMotorAcceleration(); + for (int i = 0; i < this.inputs.swerve.length; i++) { + avgAcceleration += + Math.abs( + this.inputs.swerve[i].steerVelocityRevPerMin - this.prevSteerVelocitiesRevPerMin[i]); } - - avgAcceleration /= swerveModules.length; - return avgAcceleration; + avgAcceleration /= this.inputs.swerve.length; + avgAcceleration *= (2.0 * Math.PI) / 60.0; + return avgAcceleration / LOOP_PERIOD_SECS; } /** @@ -837,13 +651,9 @@ public boolean isMoveToPoseEnabled() { return this.isMoveToPoseEnabled; } - private CommandBase getSystemCheckCommand() { - return Commands.sequence( - Commands.sequence( - swerveModules[0].getCheckCommand(), - swerveModules[1].getCheckCommand(), - swerveModules[2].getCheckCommand(), - swerveModules[3].getCheckCommand())) + private Command getSystemCheckCommand() { + // FIXME: add system check commands + return Commands.sequence(Commands.sequence(Commands.waitSeconds(0.25))) .until(() -> !FaultReporter.getInstance().getFaults(SUBSYSTEM_NAME).isEmpty()) .andThen(Commands.runOnce(() -> drive(0, 0, 0, true, false), this)); } @@ -860,12 +670,11 @@ private void updateBrakeMode() { } else if (!DriverStation.isEnabled()) { boolean stillMoving = false; - for (SwerveModule mod : swerveModules) { - if (Math.abs(mod.getState().speedMetersPerSecond) - > RobotConfig.getInstance().getRobotMaxCoastVelocity()) { - stillMoving = true; - brakeModeTimer.restart(); - } + double velocityLimit = RobotConfig.getInstance().getRobotMaxCoastVelocity(); + if (Math.abs(this.inputs.drivetrain.measuredVXMetersPerSec) > velocityLimit + || Math.abs(this.inputs.drivetrain.measuredVYMetersPerSec) > velocityLimit) { + stillMoving = true; + brakeModeTimer.restart(); } if (brakeMode && !stillMoving && brakeModeTimer.hasElapsed(BREAK_MODE_DELAY_SEC)) { @@ -876,36 +685,7 @@ private void updateBrakeMode() { } private void setBrakeMode(boolean enable) { - for (SwerveModule mod : swerveModules) { - mod.setAngleBrakeMode(enable); - mod.setDriveBrakeMode(enable); - } - } - - /** - * Correction for swerve second order dynamics issue. From Cache Money: - * https://github.com/cachemoney8096/2023-charged-up/blob/main/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java#L182 - * who borrowed it from 254: - * https://github.com/Team254/FRC-2022-Public/blob/main/src/main/java/com/team254/frc2022/subsystems/Drive.java#L325 - * Discussion: - * https://www.chiefdelphi.com/t/whitepaper-swerve-drive-skew-and-second-order-kinematics/416964 - * - *

FIXME: remove this method and replace with ChassisSpeeds.fromDiscreteSpeeds once released in - * WPILib - */ - private static ChassisSpeeds convertFromDiscreteChassisSpeedsToContinuous( - ChassisSpeeds discreteChassisSpeeds) { - - Pose2d futureRobotPose = - new Pose2d( - discreteChassisSpeeds.vxMetersPerSecond * LOOP_PERIOD_SECS, - discreteChassisSpeeds.vyMetersPerSecond * LOOP_PERIOD_SECS, - Rotation2d.fromRadians(discreteChassisSpeeds.omegaRadiansPerSecond * LOOP_PERIOD_SECS)); - Twist2d twistForPose = GeometryUtils.log(futureRobotPose); - return new ChassisSpeeds( - twistForPose.dx / LOOP_PERIOD_SECS, - twistForPose.dy / LOOP_PERIOD_SECS, - twistForPose.dtheta / LOOP_PERIOD_SECS); + this.io.setBrakeMode(enable); } private enum DriveMode { diff --git a/src/main/java/frc/lib/team3061/drivetrain/DrivetrainConstants.java b/src/main/java/frc/lib/team3061/drivetrain/DrivetrainConstants.java new file mode 100644 index 00000000..802320ec --- /dev/null +++ b/src/main/java/frc/lib/team3061/drivetrain/DrivetrainConstants.java @@ -0,0 +1,14 @@ +package frc.lib.team3061.drivetrain; + +public class DrivetrainConstants { + + private static final String CONSTRUCTOR_EXCEPTION = "constant class"; + + private DrivetrainConstants() { + throw new IllegalStateException(CONSTRUCTOR_EXCEPTION); + } + + public static final boolean DEBUGGING = false; + public static final boolean TESTING = false; + public static final String SUBSYSTEM_NAME = "Drivetrain"; +} diff --git a/src/main/java/frc/lib/team3061/drivetrain/DrivetrainIO.java b/src/main/java/frc/lib/team3061/drivetrain/DrivetrainIO.java new file mode 100644 index 00000000..05ec0ba2 --- /dev/null +++ b/src/main/java/frc/lib/team3061/drivetrain/DrivetrainIO.java @@ -0,0 +1,148 @@ +package frc.lib.team3061.drivetrain; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import frc.lib.team3061.gyro.GyroIOInputsAutoLogged; +import org.littletonrobotics.junction.AutoLog; + +@java.lang.SuppressWarnings({"java:S1104"}) +public interface DrivetrainIO { + @AutoLog + public static class SwerveIOInputs { + public double driveDistanceMeters = 0.0; + public double driveVelocityMetersPerSec = 0.0; + public double driveVelocityReferenceMetersPerSec = 0.0; + public double driveVelocityErrorMetersPerSec = 0.0; + public double driveAppliedVolts = 0.0; + public double driveStatorCurrentAmps = 0.0; + public double driveSupplyCurrentAmps = 0.0; + public double driveTempCelsius = 0.0; + + public double steerAbsolutePositionDeg = 0.0; + public double steerPositionDeg = 0.0; + public double steerPositionReferenceDeg = 0.0; + public double steerPositionErrorDeg = 0.0; + public double steerVelocityRevPerMin = 0.0; + public double steerAppliedVolts = 0.0; + public double steerStatorCurrentAmps = 0.0; + public double steerSupplyCurrentAmps = 0.0; + public double steerTempCelsius = 0.0; + } + + /** Contains all of the input data received from hardware. */ + @AutoLog + public static class DrivetrainIOInputs { + double targetVXMetersPerSec = 0.0; + double targetVYMetersPerSec = 0.0; + double targetAngularVelocityRadPerSec = 0.0; + + double measuredVXMetersPerSec = 0.0; + double measuredVYMetersPerSec = 0.0; + double measuredAngularVelocityRadPerSec = 0.0; + + SwerveModuleState[] swerveReferenceStates = new SwerveModuleState[4]; + SwerveModuleState[] swerveMeasuredStates = new SwerveModuleState[4]; + + Pose2d robotPoseWithoutGyro = new Pose2d(); + Pose2d robotPose = new Pose2d(); + Pose3d robotPose3D = new Pose3d(); + + double averageDriveCurrent = 0.0; + Rotation2d rotation = new Rotation2d(); + } + + public static class DrivetrainIOInputsCollection { + SwerveIOInputsAutoLogged[] swerve = { + new SwerveIOInputsAutoLogged(), + new SwerveIOInputsAutoLogged(), + new SwerveIOInputsAutoLogged(), + new SwerveIOInputsAutoLogged() + }; // FL, FR, BL, BR + + GyroIOInputsAutoLogged gyro = new GyroIOInputsAutoLogged(); + + DrivetrainIOInputsAutoLogged drivetrain = new DrivetrainIOInputsAutoLogged(); + } + + /** Updates the set of loggable inputs. */ + public default void updateInputs(DrivetrainIOInputsCollection inputs) {} + + /** + * Sets the swerve modules in the x-stance orientation. In this orientation the wheels are aligned + * to make an 'X'. This prevents the robot from rolling on an inclined surface and makes it more + * difficult for other robots to push the robot, which is useful when shooting. + */ + public default void holdXStance() {} + + /** + * Controls the drivetrain to move the robot with the desired velocities in the x, y, and + * rotational directions. The velocities are specified from the field's frame of reference. In the + * field frame of reference, the origin of the field to the lower left corner (i.e., the corner of + * the field to the driver's right). Zero degrees is away from the driver and increases in the CCW + * direction. + * + * @param xVelocity the desired velocity in the x direction (m/s) + * @param yVelocity the desired velocity in the y direction (m/s) + * @param rotationalVelocity the desired rotational velocity (rad/s) + * @param isOpenLoop true for open-loop control; false for closed-loop control + */ + public default void driveFieldRelative( + double xVelocity, double yVelocity, double rotationalVelocity, boolean isOpenLoop) {} + + public default void driveFieldRelativeFacingAngle( + double xVelocity, double yVelocity, Rotation2d targetDirection, boolean isOpenLoop) {} + + public default void pointWheelsAt(Rotation2d targetDirection) {} + + public default void driveRobotRelative( + double xVelocity, double yVelocity, double rotationalVelocity, boolean isOpenLoop) {} + + public default void setChassisSpeeds(ChassisSpeeds speeds, boolean isOpenLoop) {} + + /** + * Sets the rotation of the robot to the specified value. This method should only be invoked when + * the rotation of the robot is known (e.g., at the start of an autonomous path). Zero degrees is + * facing away from the driver station; CCW is positive. + * + * @param expectedYaw the rotation of the robot (in degrees) + */ + public default void setGyroOffset(double expectedYaw) {} + + /** + * Sets the robot's center of rotation. The origin is at the center of the robot. The positive x + * direction is forward; the positive y direction, left. + * + * @param centerOfRotation the center of rotation of the robot (in units of meters) + */ + public default void setCenterOfRotation(Translation2d centerOfRotation) {} + + /** + * Sets the odometry of the robot to the specified pose. This method should only be invoked when + * the rotation of the robot is known (e.g., at the start of an autonomous path). The origin of + * the field to the lower left corner (i.e., the corner of the field to the driver's right). Zero + * degrees is away from the driver and increases in the CCW direction. + * + * @param pose the specified pose to which is set the odometry + */ + public default void resetPose(Pose2d pose) {} + + /** + * Supplies the drive motors with the specified voltage. Used for drivetrain characterization. + * + * @param volts the commanded voltage + */ + public default void setDriveMotorVoltage(double volts) {} + + /** + * Supplies the steer motors with the specified voltage. Used for drivetrain characterization. + * + * @param volts the commanded voltage + */ + public default void setSteerMotorVoltage(double volts) {} + + public default void setBrakeMode(boolean enable) {} +} diff --git a/src/main/java/frc/lib/team3061/drivetrain/DrivetrainIOCTRE.java b/src/main/java/frc/lib/team3061/drivetrain/DrivetrainIOCTRE.java new file mode 100644 index 00000000..b69e24ae --- /dev/null +++ b/src/main/java/frc/lib/team3061/drivetrain/DrivetrainIOCTRE.java @@ -0,0 +1,606 @@ +package frc.lib.team3061.drivetrain; + +import static frc.lib.team3061.drivetrain.swerve.SwerveConstants.*; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain; +import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants; +import com.ctre.phoenix6.mechanisms.swerve.SwerveModule; +import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.ClosedLoopOutputType; +import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants; +import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants.SteerFeedbackType; +import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstantsFactory; +import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.util.Units; +import frc.lib.team3061.RobotConfig; +import frc.lib.team3061.drivetrain.swerve.Conversions; +import frc.lib.team3061.drivetrain.swerve.SwerveConstants; +import frc.lib.team3061.gyro.GyroIO.GyroIOInputs; +import frc.lib.team3061.util.RobotOdometry; +import frc.lib.team6328.util.TunableNumber; +import frc.robot.Constants; +import org.littletonrobotics.junction.Logger; + +public class DrivetrainIOCTRE extends SwerveDrivetrain implements DrivetrainIO { + + static class CustomSlotGains extends Slot0Configs { + public CustomSlotGains(double kP, double kI, double kD, double kV, double kS) { + this.kP = kP; + this.kI = kI; + this.kD = kD; + this.kV = kV; + this.kS = kS; + } + } + + static class SwerveModuleSignals { + public SwerveModuleSignals(TalonFX driveMotor, TalonFX steerMotor) { + this.steerVelocityStatusSignal = steerMotor.getVelocity().clone(); + this.steerPositionErrorStatusSignal = steerMotor.getClosedLoopError().clone(); + this.steerPositionReferenceStatusSignal = steerMotor.getClosedLoopReference().clone(); + this.drivePositionStatusSignal = driveMotor.getPosition().clone(); + this.driveVelocityErrorStatusSignal = driveMotor.getClosedLoopError().clone(); + this.driveVelocityReferenceStatusSignal = driveMotor.getClosedLoopReference().clone(); + } + + StatusSignal steerVelocityStatusSignal; + StatusSignal steerPositionErrorStatusSignal; + StatusSignal steerPositionReferenceStatusSignal; + StatusSignal drivePositionStatusSignal; + StatusSignal driveVelocityErrorStatusSignal; + StatusSignal driveVelocityReferenceStatusSignal; + } + + private final TunableNumber driveKp = + new TunableNumber("Drive/DriveKp", RobotConfig.getInstance().getSwerveDriveKP()); + private final TunableNumber driveKi = + new TunableNumber("Drive/DriveKi", RobotConfig.getInstance().getSwerveDriveKI()); + private final TunableNumber driveKd = + new TunableNumber("Drive/DriveKd", RobotConfig.getInstance().getSwerveDriveKD()); + private final TunableNumber steerKp = + new TunableNumber("Drive/TurnKp", RobotConfig.getInstance().getSwerveAngleKP()); + private final TunableNumber steerKi = + new TunableNumber("Drive/TurnKi", RobotConfig.getInstance().getSwerveAngleKI()); + private final TunableNumber steerKd = + new TunableNumber("Drive/TurnKd", RobotConfig.getInstance().getSwerveAngleKD()); + + private static final CustomSlotGains steerGains = + new CustomSlotGains( + RobotConfig.getInstance().getSwerveAngleKP(), + RobotConfig.getInstance().getSwerveAngleKI(), + RobotConfig.getInstance().getSwerveAngleKD(), + RobotConfig.getInstance().getSwerveAngleKV() + * 2 + * Math.PI, // convert from V/(radians/s) to V/(rotations/s) + RobotConfig.getInstance().getSwerveAngleKS()); + // FIXME: clean this up + private static final CustomSlotGains driveGains = + new CustomSlotGains( + RobotConfig.getInstance().getSwerveDriveKP(), + RobotConfig.getInstance().getSwerveDriveKI(), + RobotConfig.getInstance().getSwerveDriveKD(), + RobotConfig.getInstance().getDriveKV() + / Conversions.mpsToFalconRPS( + 1.0, + MK4I_L2_WHEEL_CIRCUMFERENCE, + MK4I_L2_DRIVE_GEAR_RATIO), // convert from V/(m/s) to V/(rotations/s) + RobotConfig.getInstance().getDriveKS()); + + // The closed-loop output type to use for the steer motors + // This affects the PID/FF gains for the steer motors + private static final ClosedLoopOutputType steerClosedLoopOutput = ClosedLoopOutputType.Voltage; + // The closed-loop output type to use for the drive motors + // This affects the PID/FF gains for the drive motors + private static final ClosedLoopOutputType driveClosedLoopOutput = ClosedLoopOutputType.Voltage; + + private static final double COUPLE_RATIO = 0.0; + private static final double STEER_INERTIA = 0.00001; + private static final double DRIVE_INERTIA = 0.001; + + private static final SwerveDrivetrainConstants drivetrainConstants = + new SwerveDrivetrainConstants() + .withPigeon2Id(RobotConfig.getInstance().getGyroCANID()) + .withCANbusName(RobotConfig.getInstance().getCANBusName()); + + private static final SwerveModuleConstantsFactory constantCreator = + new SwerveModuleConstantsFactory() + .withDriveMotorGearRatio(SwerveConstants.MK4I_L2_DRIVE_GEAR_RATIO) + .withSteerMotorGearRatio(SwerveConstants.MK4I_L2_ANGLE_GEAR_RATIO) + .withWheelRadius( + Units.metersToInches(SwerveConstants.MK4I_L2_WHEEL_DIAMETER_METERS / 2.0)) + .withSlipCurrent(800) + .withSteerMotorGains(steerGains) + .withDriveMotorGains(driveGains) + .withSteerMotorClosedLoopOutput(steerClosedLoopOutput) + .withDriveMotorClosedLoopOutput(driveClosedLoopOutput) + .withSpeedAt12VoltsMps(RobotConfig.getInstance().getRobotMaxVelocity()) + .withSteerInertia(STEER_INERTIA) + .withDriveInertia(DRIVE_INERTIA) + .withFeedbackSource(SteerFeedbackType.FusedCANcoder) + .withCouplingGearRatio( + COUPLE_RATIO) // Every 1 rotation of the azimuth results in couple ratio drive turns + .withSteerMotorInverted(SwerveConstants.MK4I_L2_ANGLE_MOTOR_INVERTED); + + private static final SwerveModuleConstants frontLeft = + constantCreator.createModuleConstants( + RobotConfig.getInstance().getSwerveSteerMotorCANIDs()[0], + RobotConfig.getInstance().getSwerveDriveMotorCANIDs()[0], + RobotConfig.getInstance().getSwerveSteerEncoderCANIDs()[0], + RobotConfig.getInstance().getSwerveSteerOffsets()[0], + RobotConfig.getInstance().getWheelbase() / 2.0, + RobotConfig.getInstance().getTrackwidth() / 2.0, + !SwerveConstants.MK4I_L2_DRIVE_MOTOR_INVERTED); + private static final SwerveModuleConstants frontRight = + constantCreator.createModuleConstants( + RobotConfig.getInstance().getSwerveSteerMotorCANIDs()[1], + RobotConfig.getInstance().getSwerveDriveMotorCANIDs()[1], + RobotConfig.getInstance().getSwerveSteerEncoderCANIDs()[1], + RobotConfig.getInstance().getSwerveSteerOffsets()[1], + RobotConfig.getInstance().getWheelbase() / 2.0, + -RobotConfig.getInstance().getTrackwidth() / 2.0, + SwerveConstants.MK4I_L2_DRIVE_MOTOR_INVERTED); + private static final SwerveModuleConstants backLeft = + constantCreator.createModuleConstants( + RobotConfig.getInstance().getSwerveSteerMotorCANIDs()[2], + RobotConfig.getInstance().getSwerveDriveMotorCANIDs()[2], + RobotConfig.getInstance().getSwerveSteerEncoderCANIDs()[2], + RobotConfig.getInstance().getSwerveSteerOffsets()[2], + -RobotConfig.getInstance().getWheelbase() / 2.0, + RobotConfig.getInstance().getTrackwidth() / 2.0, + !SwerveConstants.MK4I_L2_DRIVE_MOTOR_INVERTED); + private static final SwerveModuleConstants backRight = + constantCreator.createModuleConstants( + RobotConfig.getInstance().getSwerveSteerMotorCANIDs()[3], + RobotConfig.getInstance().getSwerveDriveMotorCANIDs()[3], + RobotConfig.getInstance().getSwerveSteerEncoderCANIDs()[3], + RobotConfig.getInstance().getSwerveSteerOffsets()[3], + -RobotConfig.getInstance().getWheelbase() / 2.0, + -RobotConfig.getInstance().getTrackwidth() / 2.0, + SwerveConstants.MK4I_L2_DRIVE_MOTOR_INVERTED); + + // gyro signals + private final StatusSignal pitchStatusSignal; + private final StatusSignal rollStatusSignal; + private final StatusSignal angularVelocityXStatusSignal; + private final StatusSignal angularVelocityYStatusSignal; + + // swerve module signals + SwerveModuleSignals[] swerveModulesSignals = new SwerveModuleSignals[4]; + + private Translation2d centerOfRotation; + private ChassisSpeeds targetChassisSpeeds; + private SwerveModuleState[] swerveReferenceStates = + new SwerveModuleState[] { + new SwerveModuleState(), + new SwerveModuleState(), + new SwerveModuleState(), + new SwerveModuleState() + }; + + private SwerveRequest.RobotCentric driveRobotCentricRequest = new SwerveRequest.RobotCentric(); + private SwerveRequest.FieldCentric driveFieldCentricRequest = new SwerveRequest.FieldCentric(); + private SwerveRequest.FieldCentricFacingAngle driveFacingAngleRequest = + new SwerveRequest.FieldCentricFacingAngle(); + private SwerveRequest.SwerveDriveBrake brakeRequest = new SwerveRequest.SwerveDriveBrake(); + private SwerveRequest.PointWheelsAt pointRequest = new SwerveRequest.PointWheelsAt(); + private SwerveRequest.ApplyChassisSpeeds applyChassisSpeedsRequest = + new SwerveRequest.ApplyChassisSpeeds(); + + /** + * Creates a new Drivetrain subsystem. + * + * @param gyroIO the abstracted interface for the gyro for the drivetrain + * @param flModule the front left swerve module + * @param frModule the front right swerve module + * @param blModule the back left swerve module + * @param brModule the back right swerve module + */ + public DrivetrainIOCTRE() { + super( + drivetrainConstants, + RobotConfig.getInstance().getOdometryUpdateFrequency(), + frontLeft, + frontRight, + backLeft, + backRight); + + // configure current limits + for (SwerveModule swerveModule : this.Modules) { + CurrentLimitsConfigs currentLimits = new CurrentLimitsConfigs(); + swerveModule.getDriveMotor().getConfigurator().refresh(currentLimits); + currentLimits.SupplyCurrentLimit = SwerveConstants.DRIVE_CONTINUOUS_CURRENT_LIMIT; + currentLimits.SupplyCurrentThreshold = SwerveConstants.DRIVE_PEAK_CURRENT_LIMIT; + currentLimits.SupplyTimeThreshold = SwerveConstants.DRIVE_PEAK_CURRENT_DURATION; + currentLimits.SupplyCurrentLimitEnable = SwerveConstants.DRIVE_ENABLE_CURRENT_LIMIT; + swerveModule.getDriveMotor().getConfigurator().apply(currentLimits); + + currentLimits = new CurrentLimitsConfigs(); + swerveModule.getSteerMotor().getConfigurator().refresh(currentLimits); + currentLimits.SupplyCurrentLimit = SwerveConstants.ANGLE_CONTINUOUS_CURRENT_LIMIT; + currentLimits.SupplyCurrentThreshold = SwerveConstants.ANGLE_PEAK_CURRENT_LIMIT; + currentLimits.SupplyTimeThreshold = SwerveConstants.ANGLE_PEAK_CURRENT_DURATION; + currentLimits.SupplyCurrentLimitEnable = SwerveConstants.ANGLE_ENABLE_CURRENT_LIMIT; + swerveModule.getSteerMotor().getConfigurator().apply(currentLimits); + } + + this.pitchStatusSignal = this.m_pigeon2.getPitch().clone(); + this.pitchStatusSignal.setUpdateFrequency(100); + this.rollStatusSignal = this.m_pigeon2.getRoll().clone(); + this.rollStatusSignal.setUpdateFrequency(100); + this.angularVelocityXStatusSignal = this.m_pigeon2.getAngularVelocityXWorld().clone(); + this.angularVelocityXStatusSignal.setUpdateFrequency(100); + this.angularVelocityYStatusSignal = this.m_pigeon2.getAngularVelocityYWorld().clone(); + this.angularVelocityYStatusSignal.setUpdateFrequency(100); + + for (int i = 0; i < swerveModulesSignals.length; i++) { + swerveModulesSignals[i] = + new SwerveModuleSignals(this.Modules[i].getSteerMotor(), this.Modules[i].getDriveMotor()); + } + + this.centerOfRotation = new Translation2d(); // default to (0,0) + + this.targetChassisSpeeds = new ChassisSpeeds(0.0, 0.0, 0.0); + + // specify that we will be using CTRE's custom odometry instead of 3061 lib's default + RobotOdometry.getInstance().setCustomOdometry(this); + } + + @Override + public void updateInputs(DrivetrainIOInputsCollection inputs) { + + // update and log gyro inputs + double timestamp = Logger.getRealTimestamp(); + this.updateGyroInputs(inputs.gyro); + Logger.recordOutput("Drivetrain/gyroInputsTime", Logger.getRealTimestamp() - timestamp); + + // update and log the swerve modules inputs + timestamp = Logger.getRealTimestamp(); + for (int i = 0; i < swerveModulesSignals.length; i++) { + this.updateSwerveModuleInputs(inputs.swerve[i], this.Modules[i], swerveModulesSignals[i]); + } + Logger.recordOutput("Drivetrain/swerveInputsTime", Logger.getRealTimestamp() - timestamp); + + inputs.drivetrain.swerveMeasuredStates = this.getState().ModuleStates; + inputs.drivetrain.swerveReferenceStates = swerveReferenceStates; + + timestamp = Logger.getRealTimestamp(); + + // log poses, 3D geometry, and swerve module states, gyro offset + inputs.drivetrain.robotPose = + new Pose2d(this.getState().Pose.getTranslation(), this.getState().Pose.getRotation()); + inputs.drivetrain.robotPose3D = new Pose3d(inputs.drivetrain.robotPose); + + inputs.drivetrain.targetVXMetersPerSec = this.targetChassisSpeeds.vxMetersPerSecond; + inputs.drivetrain.targetVYMetersPerSec = this.targetChassisSpeeds.vyMetersPerSecond; + inputs.drivetrain.targetAngularVelocityRadPerSec = + this.targetChassisSpeeds.omegaRadiansPerSecond; + + ChassisSpeeds measuredChassisSpeeds = + m_kinematics.toChassisSpeeds(this.getState().ModuleStates); + inputs.drivetrain.measuredVXMetersPerSec = measuredChassisSpeeds.vxMetersPerSecond; + inputs.drivetrain.measuredVYMetersPerSec = measuredChassisSpeeds.vyMetersPerSecond; + inputs.drivetrain.measuredAngularVelocityRadPerSec = + measuredChassisSpeeds.omegaRadiansPerSecond; + + inputs.drivetrain.averageDriveCurrent = this.getAverageDriveCurrent(inputs); + + inputs.drivetrain.rotation = this.getState().Pose.getRotation(); + + Logger.recordOutput("Drivetrain/inputsTime", Logger.getRealTimestamp() - timestamp); + + if (Constants.getMode() == Constants.Mode.SIM) { + updateSimState(Constants.LOOP_PERIOD_SECS, 12.0); + } + + // update tunables + if (driveKp.hasChanged() + || driveKi.hasChanged() + || driveKd.hasChanged() + || steerKp.hasChanged() + || steerKi.hasChanged() + || steerKd.hasChanged()) { + for (SwerveModule swerveModule : this.Modules) { + Slot0Configs driveSlot0 = new Slot0Configs(); + swerveModule.getDriveMotor().getConfigurator().refresh(driveSlot0); + driveSlot0.kP = driveKp.get(); + driveSlot0.kI = driveKi.get(); + driveSlot0.kD = driveKd.get(); + swerveModule.getDriveMotor().getConfigurator().apply(driveSlot0); + + Slot0Configs steerSlot0 = new Slot0Configs(); + swerveModule.getSteerMotor().getConfigurator().refresh(steerSlot0); + steerSlot0.kP = steerKp.get(); + steerSlot0.kI = steerKi.get(); + steerSlot0.kD = steerKd.get(); + swerveModule.getSteerMotor().getConfigurator().apply(steerSlot0); + } + } + } + + private void updateGyroInputs(GyroIOInputs inputs) { + BaseStatusSignal.refreshAll( + this.pitchStatusSignal, + this.rollStatusSignal, + this.angularVelocityXStatusSignal, + this.angularVelocityYStatusSignal); + + inputs.connected = (this.m_yawGetter.getStatus() == StatusCode.OK); + inputs.yawDeg = + BaseStatusSignal.getLatencyCompensatedValue(this.m_yawGetter, this.m_angularVelocity); + inputs.pitchDeg = + BaseStatusSignal.getLatencyCompensatedValue( + this.pitchStatusSignal, this.angularVelocityYStatusSignal); + inputs.rollDeg = + BaseStatusSignal.getLatencyCompensatedValue( + this.rollStatusSignal, this.angularVelocityXStatusSignal); + inputs.rollDegPerSec = this.angularVelocityXStatusSignal.getValue(); + inputs.pitchDegPerSec = this.angularVelocityYStatusSignal.getValue(); + inputs.yawDegPerSec = this.m_angularVelocity.getValue(); + } + + private void updateSwerveModuleInputs( + SwerveIOInputs inputs, SwerveModule module, SwerveModuleSignals signals) { + + double timestamp = Logger.getRealTimestamp(); + BaseStatusSignal.refreshAll( + signals.steerVelocityStatusSignal, + signals.steerPositionErrorStatusSignal, + signals.steerPositionReferenceStatusSignal, + signals.drivePositionStatusSignal, + signals.driveVelocityErrorStatusSignal, + signals.driveVelocityReferenceStatusSignal); + Logger.recordOutput("Drivetrain/swerve/refresh", Logger.getRealTimestamp() - timestamp); + + timestamp = Logger.getRealTimestamp(); + SwerveModulePosition position = module.getPosition(false); + SwerveModuleState state = module.getCurrentState(); + Logger.recordOutput("Drivetrain/swerve/position", Logger.getRealTimestamp() - timestamp); + + timestamp = Logger.getRealTimestamp(); + inputs.driveDistanceMeters = position.distanceMeters; + inputs.driveVelocityMetersPerSec = state.speedMetersPerSecond; + inputs.driveVelocityReferenceMetersPerSec = + Conversions.falconRPSToMechanismMPS( + signals.driveVelocityReferenceStatusSignal.getValue(), + SwerveConstants.MK4I_L2_WHEEL_CIRCUMFERENCE, + SwerveConstants.MK4I_L2_DRIVE_GEAR_RATIO); + inputs.driveVelocityErrorMetersPerSec = + Conversions.falconRPSToMechanismMPS( + signals.driveVelocityErrorStatusSignal.getValue(), + SwerveConstants.MK4I_L2_WHEEL_CIRCUMFERENCE, + SwerveConstants.MK4I_L2_DRIVE_GEAR_RATIO); + inputs.driveAppliedVolts = module.getDriveMotor().getMotorVoltage().getValue(); + inputs.driveStatorCurrentAmps = module.getDriveMotor().getStatorCurrent().getValue(); + inputs.driveSupplyCurrentAmps = module.getDriveMotor().getSupplyCurrent().getValue(); + inputs.driveTempCelsius = module.getDriveMotor().getDeviceTemp().getValue(); + Logger.recordOutput("Drivetrain/swerve/drive", Logger.getRealTimestamp() - timestamp); + + timestamp = Logger.getRealTimestamp(); + inputs.steerAbsolutePositionDeg = module.getCANcoder().getAbsolutePosition().getValue() * 360.0; + Logger.recordOutput("Drivetrain/swerve/CANcoder", Logger.getRealTimestamp() - timestamp); + + timestamp = Logger.getRealTimestamp(); + // since we are using the FusedCANcoder feature, the position and velocity signal for the angle + // motor accounts for the gear ratio; so, pass a gear ratio of 1 to just convert from rotations + // to degrees. + inputs.steerPositionDeg = position.angle.getDegrees(); + inputs.steerPositionReferenceDeg = + Conversions.falconRotationsToMechanismDegrees( + signals.steerPositionReferenceStatusSignal.getValue(), 1); + inputs.steerPositionErrorDeg = + Conversions.falconRotationsToMechanismDegrees( + signals.steerPositionErrorStatusSignal.getValue(), 1); + inputs.steerVelocityRevPerMin = + Conversions.falconRPSToMechanismRPM(signals.steerVelocityStatusSignal.getValue(), 1); + + inputs.steerAppliedVolts = module.getSteerMotor().getMotorVoltage().getValue(); + inputs.steerStatorCurrentAmps = module.getSteerMotor().getStatorCurrent().getValue(); + inputs.steerSupplyCurrentAmps = module.getSteerMotor().getSupplyCurrent().getValue(); + inputs.steerTempCelsius = module.getSteerMotor().getDeviceTemp().getValue(); + Logger.recordOutput("Drivetrain/swerve/steer", Logger.getRealTimestamp() - timestamp); + } + + @Override + public void holdXStance() { + this.targetChassisSpeeds = new ChassisSpeeds(0.0, 0.0, 0.0); + this.setControl(this.brakeRequest); + } + + @Override + public void driveFieldRelative( + double xVelocity, double yVelocity, double rotationalVelocity, boolean isOpenLoop) { + + this.targetChassisSpeeds = + ChassisSpeeds.discretize( + ChassisSpeeds.fromFieldRelativeSpeeds( + xVelocity, yVelocity, rotationalVelocity, this.getState().Pose.getRotation()), + Constants.LOOP_PERIOD_SECS); + + if (isOpenLoop) { + this.setControl( + this.driveFieldCentricRequest + .withDriveRequestType(SwerveModule.DriveRequestType.OpenLoopVoltage) + .withSteerRequestType(SwerveModule.SteerRequestType.MotionMagicExpo) + .withVelocityX(xVelocity) + .withVelocityY(yVelocity) + .withRotationalRate(rotationalVelocity)); + } else { + this.setControl( + this.driveFieldCentricRequest + .withDriveRequestType(SwerveModule.DriveRequestType.Velocity) + .withSteerRequestType(SwerveModule.SteerRequestType.MotionMagicExpo) + .withVelocityX(xVelocity) + .withVelocityY(yVelocity) + .withRotationalRate(rotationalVelocity)); + } + } + + @Override + public void driveFieldRelativeFacingAngle( + double xVelocity, double yVelocity, Rotation2d targetDirection, boolean isOpenLoop) { + this.targetChassisSpeeds = + ChassisSpeeds.discretize( + ChassisSpeeds.fromFieldRelativeSpeeds( + xVelocity, yVelocity, 0.0, getState().Pose.getRotation()), + Constants.LOOP_PERIOD_SECS); + + if (isOpenLoop) { + this.setControl( + this.driveFacingAngleRequest + .withDriveRequestType(SwerveModule.DriveRequestType.OpenLoopVoltage) + .withSteerRequestType(SwerveModule.SteerRequestType.MotionMagicExpo) + .withVelocityX(xVelocity) + .withVelocityY(yVelocity) + .withTargetDirection(targetDirection)); + } else { + this.setControl( + this.driveFacingAngleRequest + .withDriveRequestType(SwerveModule.DriveRequestType.Velocity) + .withSteerRequestType(SwerveModule.SteerRequestType.MotionMagicExpo) + .withVelocityX(xVelocity) + .withVelocityY(yVelocity) + .withTargetDirection(targetDirection)); + } + } + + @Override + public void pointWheelsAt(Rotation2d targetDirection) { + this.targetChassisSpeeds = new ChassisSpeeds(0.0, 0.0, 0.0); + + this.setControl(this.pointRequest.withModuleDirection(targetDirection)); + } + + @Override + public void driveRobotRelative( + double xVelocity, double yVelocity, double rotationalVelocity, boolean isOpenLoop) { + this.targetChassisSpeeds = + ChassisSpeeds.discretize( + new ChassisSpeeds(xVelocity, yVelocity, rotationalVelocity), + Constants.LOOP_PERIOD_SECS); + + if (isOpenLoop) { + this.setControl( + this.driveRobotCentricRequest + .withDriveRequestType(SwerveModule.DriveRequestType.OpenLoopVoltage) + .withSteerRequestType(SwerveModule.SteerRequestType.MotionMagicExpo) + .withVelocityX(xVelocity) + .withVelocityY(yVelocity) + .withRotationalRate(rotationalVelocity)); + } else { + this.setControl( + this.driveRobotCentricRequest + .withDriveRequestType(SwerveModule.DriveRequestType.Velocity) + .withSteerRequestType(SwerveModule.SteerRequestType.MotionMagicExpo) + .withVelocityX(xVelocity) + .withVelocityY(yVelocity) + .withRotationalRate(rotationalVelocity)); + } + } + + @Override + public void setChassisSpeeds(ChassisSpeeds speeds, boolean isOpenLoop) { + this.targetChassisSpeeds.omegaRadiansPerSecond = speeds.omegaRadiansPerSecond; + this.targetChassisSpeeds.vxMetersPerSecond = speeds.vxMetersPerSecond; + this.targetChassisSpeeds.vyMetersPerSecond = speeds.vyMetersPerSecond; + + if (isOpenLoop) { + this.setControl( + this.applyChassisSpeedsRequest + .withDriveRequestType(SwerveModule.DriveRequestType.OpenLoopVoltage) + .withSteerRequestType(SwerveModule.SteerRequestType.MotionMagicExpo) + .withSpeeds(speeds) + .withCenterOfRotation(this.centerOfRotation)); + } else { + this.setControl( + this.applyChassisSpeedsRequest + .withDriveRequestType(SwerveModule.DriveRequestType.Velocity) + .withSteerRequestType(SwerveModule.SteerRequestType.MotionMagicExpo) + .withSpeeds(speeds) + .withCenterOfRotation(this.centerOfRotation)); + } + } + + @Override + public void setGyroOffset(double expectedYaw) { + try { + m_stateLock.writeLock().lock(); + + // FIXME: check the math here not sure if we should add or subtract + m_fieldRelativeOffset = + getState().Pose.getRotation().plus(Rotation2d.fromDegrees(expectedYaw)); + } finally { + m_stateLock.writeLock().unlock(); + } + } + + @Override + public void setCenterOfRotation(Translation2d centerOfRotation) { + this.centerOfRotation = centerOfRotation; + } + + @Override + public void resetPose(Pose2d pose) { + this.seedFieldRelative(pose); + } + + @Override + public void setDriveMotorVoltage(double voltage) { + // FIXME: implement later with Idle Swerve Request object? + } + + @Override + public void setSteerMotorVoltage(double voltage) { + // FIXME: implement later with Idle Swerve Request object? + } + + @Override + public void setBrakeMode(boolean enable) { + for (SwerveModule swerveModule : this.Modules) { + MotorOutputConfigs config = new MotorOutputConfigs(); + swerveModule.getDriveMotor().getConfigurator().refresh(config); + config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; + swerveModule.getDriveMotor().getConfigurator().apply(config); + } + } + + /** + * Returns the average current of the swerve module drive motors in amps. + * + * @return the average current of the swerve module drive motors in amps + */ + private double getAverageDriveCurrent(DrivetrainIOInputsCollection inputs) { + double totalCurrent = 0.0; + for (SwerveIOInputs swerveInputs : inputs.swerve) { + totalCurrent += swerveInputs.driveStatorCurrentAmps; + } + return totalCurrent / inputs.swerve.length; + } + + public Pose2d getEstimatedPosition() { + return this.m_odometry.getEstimatedPosition(); + } + + public void resetPosition( + Rotation2d gyroAngle, SwerveModulePosition[] modulePositions, Pose2d poseMeters) { + this.m_odometry.resetPosition(gyroAngle, modulePositions, poseMeters); + } + + public Pose2d updateWithTime( + double currentTimeSeconds, Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) { + return this.m_odometry.updateWithTime(currentTimeSeconds, gyroAngle, modulePositions); + } +} diff --git a/src/main/java/frc/lib/team3061/drivetrain/DrivetrainIOGeneric.java b/src/main/java/frc/lib/team3061/drivetrain/DrivetrainIOGeneric.java new file mode 100644 index 00000000..c1bb5ff4 --- /dev/null +++ b/src/main/java/frc/lib/team3061/drivetrain/DrivetrainIOGeneric.java @@ -0,0 +1,376 @@ +package frc.lib.team3061.drivetrain; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +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 frc.lib.team3061.RobotConfig; +import frc.lib.team3061.drivetrain.swerve.SwerveModuleIO; +import frc.lib.team3061.gyro.GyroIO; +import frc.lib.team3061.util.RobotOdometry; +import frc.robot.Constants; +import java.util.ArrayList; +import java.util.List; +import org.littletonrobotics.junction.Logger; + +public class DrivetrainIOGeneric implements DrivetrainIO { + private final GyroIO gyroIO; + private double robotRotationDeg = 0.0; + + private final double trackwidthMeters = RobotConfig.getInstance().getTrackwidth(); + private final double wheelbaseMeters = RobotConfig.getInstance().getWheelbase(); + private final SwerveDriveKinematics kinematics = + RobotConfig.getInstance().getSwerveDriveKinematics(); + + private final SwerveModuleIO[] swerveModules = new SwerveModuleIO[4]; // FL, FR, BL, BR + + // some of this code is from the SDS example code + + private Translation2d centerOfRotation; + + private final SwerveModulePosition[] swerveModulePositions = + new SwerveModulePosition[] { + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition() + }; + private final SwerveModulePosition[] prevSwerveModulePositions = + new SwerveModulePosition[] { + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition() + }; + private final SwerveModuleState[] swerveModuleStates = + new SwerveModuleState[] { + new SwerveModuleState(), + new SwerveModuleState(), + new SwerveModuleState(), + new SwerveModuleState() + }; + private final SwerveModuleState[] prevSwerveModuleStates = + new SwerveModuleState[] { + new SwerveModuleState(), + new SwerveModuleState(), + new SwerveModuleState(), + new SwerveModuleState() + }; + private SwerveModuleState[] swerveReferenceStates = + new SwerveModuleState[] { + new SwerveModuleState(), + new SwerveModuleState(), + new SwerveModuleState(), + new SwerveModuleState() + }; + + private ChassisSpeeds targetChassisSpeeds; + + private double[] steerMotorsLastAngle = new double[4]; + + private final RobotOdometry odometry; + private Pose2d estimatedPoseWithoutGyro = new Pose2d(); + + private final List> odometrySignals = new ArrayList<>(); + + /** + * Creates a new Drivetrain subsystem. + * + * @param gyroIO the abstracted interface for the gyro for the drivetrain + * @param flModule the front left swerve module + * @param frModule the front right swerve module + * @param blModule the back left swerve module + * @param brModule the back right swerve module + */ + public DrivetrainIOGeneric( + GyroIO gyroIO, + SwerveModuleIO flModule, + SwerveModuleIO frModule, + SwerveModuleIO blModule, + SwerveModuleIO brModule) { + this.gyroIO = gyroIO; + this.swerveModules[0] = flModule; + this.swerveModules[1] = frModule; + this.swerveModules[2] = blModule; + this.swerveModules[3] = brModule; + + this.centerOfRotation = new Translation2d(); // default to (0,0) + + this.setGyroOffset(0.0); + + this.targetChassisSpeeds = new ChassisSpeeds(0.0, 0.0, 0.0); + + this.odometry = RobotOdometry.getInstance(); + + this.odometrySignals.addAll(this.gyroIO.getOdometryStatusSignals()); + for (SwerveModuleIO swerveModule : swerveModules) { + this.odometrySignals.addAll(swerveModule.getOdometryStatusSignals()); + } + } + + @Override + public void updateInputs(DrivetrainIOInputsCollection inputs) { + // synchronize all of the signals related to pose estimation + if (RobotConfig.getInstance().getPhoenix6Licensed()) { + // this is a licensed method + BaseStatusSignal.waitForAll( + Constants.LOOP_PERIOD_SECS, this.odometrySignals.toArray(new BaseStatusSignal[0])); + } + + // update and log gyro inputs + gyroIO.updateInputs(inputs.gyro); + + // update and log the swerve modules inputs + for (int i = 0; i < this.swerveModules.length; i++) { + this.swerveModules[i].updateInputs(inputs.swerve[i]); + } + + // update swerve module states and positions + for (int i = 0; i < this.swerveModuleStates.length; i++) { + prevSwerveModuleStates[i] = swerveModuleStates[i]; + swerveModuleStates[i] = + new SwerveModuleState( + inputs.swerve[i].driveVelocityMetersPerSec, + Rotation2d.fromDegrees(inputs.swerve[i].steerPositionDeg)); + prevSwerveModulePositions[i] = swerveModulePositions[i]; + swerveModulePositions[i] = + new SwerveModulePosition( + inputs.swerve[i].driveDistanceMeters, + Rotation2d.fromDegrees(inputs.swerve[i].steerPositionDeg)); + } + + // if the gyro is not connected, use the swerve module positions to estimate the robot's + // rotation + if (!inputs.gyro.connected || Constants.getMode() == Constants.Mode.SIM) { + SwerveModulePosition[] moduleDeltas = new SwerveModulePosition[4]; + for (int index = 0; index < moduleDeltas.length; index++) { + SwerveModulePosition current = swerveModulePositions[index]; + SwerveModulePosition previous = prevSwerveModulePositions[index]; + + moduleDeltas[index] = + new SwerveModulePosition( + current.distanceMeters - previous.distanceMeters, current.angle); + // FIXME: I don't think this assignment is needed... + previous.distanceMeters = current.distanceMeters; + } + + Twist2d twist = kinematics.toTwist2d(moduleDeltas); + this.gyroIO.addYaw(Math.toDegrees(twist.dtheta)); + + estimatedPoseWithoutGyro = estimatedPoseWithoutGyro.exp(twist); + this.robotRotationDeg = estimatedPoseWithoutGyro.getRotation().getDegrees(); + } else { + this.robotRotationDeg = inputs.gyro.yawDeg; + } + + inputs.drivetrain.swerveMeasuredStates = this.swerveModuleStates; + inputs.drivetrain.swerveReferenceStates = this.swerveReferenceStates; + + // update the pose estimator based on the gyro and swerve module positions + this.odometry.updateWithTime( + Logger.getRealTimestamp() / 1e6, + Rotation2d.fromDegrees(this.robotRotationDeg), + swerveModulePositions); + + // log poses, 3D geometry, and swerve module states, gyro offset + inputs.drivetrain.robotPoseWithoutGyro = estimatedPoseWithoutGyro; + inputs.drivetrain.robotPose = odometry.getEstimatedPosition(); + inputs.drivetrain.robotPose3D = new Pose3d(inputs.drivetrain.robotPose); + + inputs.drivetrain.targetVXMetersPerSec = this.targetChassisSpeeds.vxMetersPerSecond; + inputs.drivetrain.targetVYMetersPerSec = this.targetChassisSpeeds.vyMetersPerSecond; + inputs.drivetrain.targetAngularVelocityRadPerSec = + this.targetChassisSpeeds.omegaRadiansPerSecond; + + ChassisSpeeds measuredChassisSpeeds = kinematics.toChassisSpeeds(this.swerveModuleStates); + inputs.drivetrain.measuredVXMetersPerSec = measuredChassisSpeeds.vxMetersPerSecond; + inputs.drivetrain.measuredVYMetersPerSec = measuredChassisSpeeds.vyMetersPerSecond; + inputs.drivetrain.measuredAngularVelocityRadPerSec = + measuredChassisSpeeds.omegaRadiansPerSecond; + + inputs.drivetrain.averageDriveCurrent = this.getAverageDriveCurrent(inputs); + + inputs.drivetrain.rotation = Rotation2d.fromDegrees(this.robotRotationDeg); + } + + @Override + public void holdXStance() { + this.targetChassisSpeeds = new ChassisSpeeds(0.0, 0.0, 0.0); + this.swerveReferenceStates = + kinematics.toSwerveModuleStates(this.targetChassisSpeeds, this.centerOfRotation); + this.swerveReferenceStates[0].angle = + new Rotation2d(Math.PI / 2 - Math.atan(trackwidthMeters / wheelbaseMeters)); + this.swerveReferenceStates[1].angle = + new Rotation2d(Math.PI / 2 + Math.atan(trackwidthMeters / wheelbaseMeters)); + this.swerveReferenceStates[2].angle = + new Rotation2d(Math.PI / 2 + Math.atan(trackwidthMeters / wheelbaseMeters)); + this.swerveReferenceStates[3].angle = + new Rotation2d(3.0 / 2.0 * Math.PI - Math.atan(trackwidthMeters / wheelbaseMeters)); + setSwerveModuleStates(this.swerveReferenceStates, true, true); + } + + @Override + public void driveFieldRelative( + double xVelocity, double yVelocity, double rotationalVelocity, boolean isOpenLoop) { + + this.targetChassisSpeeds = + ChassisSpeeds.fromFieldRelativeSpeeds( + xVelocity, + yVelocity, + rotationalVelocity, + Rotation2d.fromDegrees(this.robotRotationDeg)); + + this.targetChassisSpeeds = + ChassisSpeeds.discretize(this.targetChassisSpeeds, Constants.LOOP_PERIOD_SECS); + + this.swerveReferenceStates = + kinematics.toSwerveModuleStates(this.targetChassisSpeeds, centerOfRotation); + setSwerveModuleStates(this.swerveReferenceStates, isOpenLoop, false); + } + + @Override + public void driveFieldRelativeFacingAngle( + double xVelocity, double yVelocity, Rotation2d targetDirection, boolean isOpenLoop) { + // FIXME: add support for holding a rotation angle + this.targetChassisSpeeds = + ChassisSpeeds.fromFieldRelativeSpeeds( + xVelocity, yVelocity, 0.0, Rotation2d.fromDegrees(this.robotRotationDeg)); + + this.targetChassisSpeeds = + ChassisSpeeds.discretize(this.targetChassisSpeeds, Constants.LOOP_PERIOD_SECS); + + this.swerveReferenceStates = + kinematics.toSwerveModuleStates(this.targetChassisSpeeds, centerOfRotation); + setSwerveModuleStates(this.swerveReferenceStates, isOpenLoop, false); + } + + @Override + public void pointWheelsAt(Rotation2d targetDirection) { + for (int i = 0; i < this.swerveReferenceStates.length; i++) { + this.swerveReferenceStates[i] = new SwerveModuleState(0.0, targetDirection); + } + + setSwerveModuleStates(this.swerveReferenceStates, false, true); + } + + @Override + public void driveRobotRelative( + double xVelocity, double yVelocity, double rotationalVelocity, boolean isOpenLoop) { + this.targetChassisSpeeds = new ChassisSpeeds(xVelocity, yVelocity, rotationalVelocity); + this.targetChassisSpeeds = + ChassisSpeeds.discretize(this.targetChassisSpeeds, Constants.LOOP_PERIOD_SECS); + + this.swerveReferenceStates = + kinematics.toSwerveModuleStates(this.targetChassisSpeeds, centerOfRotation); + setSwerveModuleStates(this.swerveReferenceStates, isOpenLoop, false); + } + + @Override + public void setChassisSpeeds(ChassisSpeeds speeds, boolean isOpenLoop) { + this.targetChassisSpeeds.omegaRadiansPerSecond = speeds.omegaRadiansPerSecond; + this.targetChassisSpeeds.vxMetersPerSecond = speeds.vxMetersPerSecond; + this.targetChassisSpeeds.vyMetersPerSecond = speeds.vyMetersPerSecond; + this.swerveReferenceStates = + kinematics.toSwerveModuleStates(this.targetChassisSpeeds, centerOfRotation); + setSwerveModuleStates(this.swerveReferenceStates, isOpenLoop, false); + } + + @Override + public void setGyroOffset(double expectedYaw) { + this.gyroIO.setYaw(expectedYaw); + this.estimatedPoseWithoutGyro = + new Pose2d( + estimatedPoseWithoutGyro.getX(), + estimatedPoseWithoutGyro.getY(), + Rotation2d.fromDegrees(expectedYaw)); + } + + @Override + public void setCenterOfRotation(Translation2d centerOfRotation) { + this.centerOfRotation = centerOfRotation; + } + + @Override + public void resetPose(Pose2d pose) { + setGyroOffset(pose.getRotation().getDegrees()); + this.estimatedPoseWithoutGyro = new Pose2d(pose.getTranslation(), pose.getRotation()); + this.odometry.resetPosition( + Rotation2d.fromDegrees(this.robotRotationDeg), swerveModulePositions, pose); + } + + @Override + public void setDriveMotorVoltage(double voltage) { + for (int i = 0; i < this.swerveModules.length; i++) { + this.swerveModules[i].setAnglePosition(0.0); + steerMotorsLastAngle[i] = 0.0; + this.swerveModules[i].setDriveMotorVoltage(voltage); + } + } + + @Override + public void setSteerMotorVoltage(double voltage) { + for (SwerveModuleIO swerveModule : this.swerveModules) { + swerveModule.setAngleMotorVoltage(voltage); + } + } + + @Override + public void setBrakeMode(boolean enable) { + for (SwerveModuleIO swerveModule : this.swerveModules) { + swerveModule.setAngleBrakeMode(enable); + swerveModule.setDriveBrakeMode(enable); + } + } + + /** + * Returns the average current of the swerve module drive motors in amps. + * + * @return the average current of the swerve module drive motors in amps + */ + private double getAverageDriveCurrent(DrivetrainIOInputsCollection inputs) { + double totalCurrent = 0.0; + for (SwerveIOInputs swerveInputs : inputs.swerve) { + totalCurrent += swerveInputs.driveStatorCurrentAmps; + } + return totalCurrent / inputs.swerve.length; + } + + private void setSwerveModuleStates( + SwerveModuleState[] states, boolean isOpenLoop, boolean forceAngle) { + double maxVelocity = RobotConfig.getInstance().getRobotMaxVelocity(); + + SwerveDriveKinematics.desaturateWheelSpeeds(states, maxVelocity); + + for (int i = 0; i < this.swerveModules.length; i++) { + states[i] = SwerveModuleState.optimize(states[i], swerveModuleStates[i].angle); + + if (isOpenLoop) { + this.swerveModules[i].setDriveMotorVoltage( + states[i].speedMetersPerSecond / maxVelocity * 12.0); + } else { + this.swerveModules[i].setDriveVelocity(states[i].speedMetersPerSecond); + } + + // Unless the angle is forced (e.g., X-stance), don't rotate the module if speed is less then + // 1%. This prevents jittering if the controller isn't tuned perfectly. Perhaps more + // importantly, it allows for smooth repeated movement as the wheel direction doesn't reset + // during pauses (e.g., multi-segmented auto paths). + double angle; + if (!forceAngle && Math.abs(states[i].speedMetersPerSecond) <= (maxVelocity * 0.01)) { + angle = this.steerMotorsLastAngle[i]; + } else { + angle = states[i].angle.getDegrees(); + } + + this.swerveModules[i].setAnglePosition(angle); + steerMotorsLastAngle[i] = angle; + } + } +} diff --git a/src/main/java/frc/lib/team3061/swerve/CTREModuleState.java b/src/main/java/frc/lib/team3061/drivetrain/swerve/CTREModuleState.java similarity index 97% rename from src/main/java/frc/lib/team3061/swerve/CTREModuleState.java rename to src/main/java/frc/lib/team3061/drivetrain/swerve/CTREModuleState.java index c5bc43d7..377f2947 100644 --- a/src/main/java/frc/lib/team3061/swerve/CTREModuleState.java +++ b/src/main/java/frc/lib/team3061/drivetrain/swerve/CTREModuleState.java @@ -2,7 +2,7 @@ * Initially from https://github.com/Team364/BaseFalconSwerve */ -package frc.lib.team3061.swerve; +package frc.lib.team3061.drivetrain.swerve; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModuleState; diff --git a/src/main/java/frc/lib/team3061/swerve/Conversions.java b/src/main/java/frc/lib/team3061/drivetrain/swerve/Conversions.java similarity index 98% rename from src/main/java/frc/lib/team3061/swerve/Conversions.java rename to src/main/java/frc/lib/team3061/drivetrain/swerve/Conversions.java index 7a54e8a5..17ec0273 100644 --- a/src/main/java/frc/lib/team3061/swerve/Conversions.java +++ b/src/main/java/frc/lib/team3061/drivetrain/swerve/Conversions.java @@ -2,7 +2,7 @@ * Initially from https://github.com/Team364/BaseFalconSwerve */ -package frc.lib.team3061.swerve; +package frc.lib.team3061.drivetrain.swerve; public class Conversions { diff --git a/src/main/java/frc/lib/team3061/swerve/SwerveModuleConstants.java b/src/main/java/frc/lib/team3061/drivetrain/swerve/SwerveConstants.java similarity index 95% rename from src/main/java/frc/lib/team3061/swerve/SwerveModuleConstants.java rename to src/main/java/frc/lib/team3061/drivetrain/swerve/SwerveConstants.java index 31daa2c7..46f77f6d 100644 --- a/src/main/java/frc/lib/team3061/swerve/SwerveModuleConstants.java +++ b/src/main/java/frc/lib/team3061/drivetrain/swerve/SwerveConstants.java @@ -2,15 +2,15 @@ * Initially from https://github.com/Team364/BaseFalconSwerve */ -package frc.lib.team3061.swerve; +package frc.lib.team3061.drivetrain.swerve; import com.ctre.phoenix6.signals.NeutralModeValue; -public final class SwerveModuleConstants { +public final class SwerveConstants { private static final String CONSTRUCTOR_EXCEPTION = "constant class"; - private SwerveModuleConstants() { + private SwerveConstants() { throw new IllegalStateException(CONSTRUCTOR_EXCEPTION); } @@ -24,7 +24,8 @@ public enum SwerveType { /* Wheel diameter is best determined empirically. Refer to this document for more information: !!! */ - public static final double MK4I_L2_WHEEL_DIAMETER_METERS = 0.096294; + public static final double MK4I_L2_WHEEL_DIAMETER_METERS = 0.09659072671; + public static final double MK4I_L2_WHEEL_CIRCUMFERENCE = MK4I_L2_WHEEL_DIAMETER_METERS * Math.PI; public static final double MK4I_L2_DRIVE_GEAR_RATIO = 1 / ((14.0 / 50.0) * (27.0 / 17.0) * (15.0 / 45.0)); diff --git a/src/main/java/frc/lib/team3061/swerve/SwerveModuleIO.java b/src/main/java/frc/lib/team3061/drivetrain/swerve/SwerveModuleIO.java similarity index 53% rename from src/main/java/frc/lib/team3061/swerve/SwerveModuleIO.java rename to src/main/java/frc/lib/team3061/drivetrain/swerve/SwerveModuleIO.java index 0e790649..9d3b3c4b 100644 --- a/src/main/java/frc/lib/team3061/swerve/SwerveModuleIO.java +++ b/src/main/java/frc/lib/team3061/drivetrain/swerve/SwerveModuleIO.java @@ -1,43 +1,21 @@ -package frc.lib.team3061.swerve; +package frc.lib.team3061.drivetrain.swerve; import com.ctre.phoenix6.StatusSignal; +import frc.lib.team3061.drivetrain.DrivetrainIO.SwerveIOInputs; import java.util.ArrayList; import java.util.List; -import org.littletonrobotics.junction.AutoLog; /** Swerve module hardware abstraction interface. */ public interface SwerveModuleIO { - @AutoLog - public static class SwerveModuleIOInputs { - double drivePositionDeg = 0.0; - double driveDistanceMeters = 0.0; - double driveVelocityMetersPerSec = 0.0; - double driveVelocityReference = 0.0; - double driveVelocityErrorMetersPerSec = 0.0; - double driveAppliedPercentage = 0.0; - double driveStatorCurrentAmps = 0.0; - double driveSupplyCurrentAmps = 0.0; - double driveTempCelsius = 0.0; - - double angleAbsolutePositionDeg = 0.0; - double anglePositionDeg = 0.0; - double anglePositionReference = 0.0; - double anglePositionErrorDeg = 0.0; - double angleVelocityRevPerMin = 0.0; - double angleAppliedPercentage = 0.0; - double angleStatorCurrentAmps = 0.0; - double angleSupplyCurrentAmps = 0.0; - double angleTempCelsius = 0.0; - } /** Updates the set of loggable inputs. */ - public default void updateInputs(SwerveModuleIOInputs inputs) {} + public default void updateInputs(SwerveIOInputs inputs) {} /** Run the drive motor at the specified percentage of full power. */ - public default void setDriveMotorPercentage(double percentage) {} + public default void setDriveMotorVoltage(double voltage) {} /** Run the angle motor at the specified percentage of full power. */ - public default void setAngleMotorPercentage(double percentage) {} + public default void setAngleMotorVoltage(double voltage) {} /** Run the drive motor at the specified velocity. */ public default void setDriveVelocity(double velocity) {} diff --git a/src/main/java/frc/lib/team3061/swerve/SwerveModuleIOTalonFXPhoenix6.java b/src/main/java/frc/lib/team3061/drivetrain/swerve/SwerveModuleIOTalonFXPhoenix6.java similarity index 89% rename from src/main/java/frc/lib/team3061/swerve/SwerveModuleIOTalonFXPhoenix6.java rename to src/main/java/frc/lib/team3061/drivetrain/swerve/SwerveModuleIOTalonFXPhoenix6.java index 846f294a..48ae03b0 100644 --- a/src/main/java/frc/lib/team3061/swerve/SwerveModuleIOTalonFXPhoenix6.java +++ b/src/main/java/frc/lib/team3061/drivetrain/swerve/SwerveModuleIOTalonFXPhoenix6.java @@ -1,6 +1,6 @@ -package frc.lib.team3061.swerve; +package frc.lib.team3061.drivetrain.swerve; -import static frc.lib.team3061.swerve.SwerveModuleConstants.*; +import static frc.lib.team3061.drivetrain.swerve.SwerveConstants.*; import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusCode; @@ -30,6 +30,8 @@ import edu.wpi.first.wpilibj.simulation.LinearSystemSim; import frc.lib.team3015.subsystem.FaultReporter; import frc.lib.team3061.RobotConfig; +import frc.lib.team3061.drivetrain.DrivetrainIO.SwerveIOInputs; +import frc.lib.team3061.drivetrain.swerve.SwerveConstants.SwerveType; import frc.lib.team6328.util.Alert; import frc.lib.team6328.util.Alert.AlertType; import frc.lib.team6328.util.TunableNumber; @@ -187,8 +189,11 @@ private void configAngleMotor(int angleMotorID, int canCoderID) { config.Slot0.kP = turnKp.get(); config.Slot0.kI = turnKi.get(); config.Slot0.kD = turnKd.get(); - config.Slot0.kS = RobotConfig.getInstance().getSwerveAngleKS() * 2 * Math.PI; - config.Slot0.kV = RobotConfig.getInstance().getSwerveAngleKV() * 2 * Math.PI; + config.Slot0.kS = RobotConfig.getInstance().getSwerveAngleKS(); + config.Slot0.kV = + RobotConfig.getInstance().getSwerveAngleKV() + * 2 + * Math.PI; // convert from V/(radians/s) to V/(rotations/s) config.ClosedLoopGeneral.ContinuousWrap = true; @@ -209,10 +214,10 @@ private void configAngleMotor(int angleMotorID, int canCoderID) { angleMotorConfigAlert.setText(status.toString()); } - this.anglePositionStatusSignal = this.angleMotor.getPosition(); - this.angleVelocityStatusSignal = this.angleMotor.getVelocity(); - this.anglePositionErrorStatusSignal = this.angleMotor.getClosedLoopError(); - this.anglePositionReferenceStatusSignal = this.angleMotor.getClosedLoopReference(); + this.anglePositionStatusSignal = this.angleMotor.getPosition().clone(); + this.angleVelocityStatusSignal = this.angleMotor.getVelocity().clone(); + this.anglePositionErrorStatusSignal = this.angleMotor.getClosedLoopError().clone(); + this.anglePositionReferenceStatusSignal = this.angleMotor.getClosedLoopReference().clone(); this.angleVoltageRequest = new VoltageOut(0.0); this.angleVoltageRequest.EnableFOC = RobotConfig.getInstance().getPhoenix6Licensed(); @@ -240,10 +245,7 @@ private void configDriveMotor(int driveMotorID) { config.Slot0.kP = driveKp.get(); config.Slot0.kI = driveKi.get(); config.Slot0.kD = driveKd.get(); - config.Slot0.kS = - RobotConfig.getInstance().getDriveKS() - / Conversions.mpsToFalconRPS(1.0, wheelCircumference, driveGearRatio); - ; + config.Slot0.kS = RobotConfig.getInstance().getDriveKS(); config.Slot0.kV = RobotConfig.getInstance().getDriveKV() / Conversions.mpsToFalconRPS(1.0, wheelCircumference, driveGearRatio); @@ -264,12 +266,12 @@ private void configDriveMotor(int driveMotorID) { driveMotorConfigAlert.setText(status.toString()); } - this.driveMotor.setRotorPosition(0.0); + this.driveMotor.setPosition(0.0); - this.drivePositionStatusSignal = this.driveMotor.getPosition(); - this.driveVelocityStatusSignal = this.driveMotor.getVelocity(); - this.driveVelocityErrorStatusSignal = this.driveMotor.getClosedLoopError(); - this.driveVelocityReferenceStatusSignal = this.driveMotor.getClosedLoopReference(); + this.drivePositionStatusSignal = this.driveMotor.getPosition().clone(); + this.driveVelocityStatusSignal = this.driveMotor.getVelocity().clone(); + this.driveVelocityErrorStatusSignal = this.driveMotor.getClosedLoopError().clone(); + this.driveVelocityReferenceStatusSignal = this.driveMotor.getClosedLoopReference().clone(); this.driveVoltageRequest = new VoltageOut(0.0); this.driveVoltageRequest.EnableFOC = RobotConfig.getInstance().getPhoenix6Licensed(); @@ -279,26 +281,29 @@ private void configDriveMotor(int driveMotorID) { /** Updates the set of loggable inputs. */ @Override - public void updateInputs(SwerveModuleIOInputs inputs) { + public void updateInputs(SwerveIOInputs inputs) { updateSim(); // only invoke refresh if Phoenix is not licensed (if licensed, these signals have already been // refreshed) if (!RobotConfig.getInstance().getPhoenix6Licensed()) { - anglePositionStatusSignal.refresh(); - angleVelocityStatusSignal.refresh(); - drivePositionStatusSignal.refresh(); - driveVelocityStatusSignal.refresh(); + BaseStatusSignal.refreshAll( + anglePositionStatusSignal, + angleVelocityStatusSignal, + drivePositionStatusSignal, + driveVelocityStatusSignal); + } else { + BaseStatusSignal.refreshAll( + anglePositionStatusSignal, + angleVelocityStatusSignal, + drivePositionStatusSignal, + driveVelocityStatusSignal, + anglePositionErrorStatusSignal, + anglePositionReferenceStatusSignal, + driveVelocityErrorStatusSignal, + driveVelocityReferenceStatusSignal); } - anglePositionErrorStatusSignal.refresh(); - anglePositionReferenceStatusSignal.refresh(); - driveVelocityErrorStatusSignal.refresh(); - driveVelocityReferenceStatusSignal.refresh(); - - inputs.drivePositionDeg = - Conversions.falconRotationsToMechanismDegrees( - drivePositionStatusSignal.getValue(), driveGearRatio); inputs.driveDistanceMeters = Conversions.falconRotationsToMechanismMeters( BaseStatusSignal.getLatencyCompensatedValue( @@ -308,38 +313,38 @@ public void updateInputs(SwerveModuleIOInputs inputs) { inputs.driveVelocityMetersPerSec = Conversions.falconRPSToMechanismMPS( driveVelocityStatusSignal.getValue(), wheelCircumference, driveGearRatio); - inputs.driveVelocityReference = + inputs.driveVelocityReferenceMetersPerSec = Conversions.falconRPSToMechanismMPS( driveVelocityReferenceStatusSignal.getValue(), wheelCircumference, driveGearRatio); inputs.driveVelocityErrorMetersPerSec = Conversions.falconRPSToMechanismMPS( driveVelocityErrorStatusSignal.getValue(), wheelCircumference, driveGearRatio); - inputs.driveAppliedPercentage = this.driveMotor.getDutyCycle().getValue(); + inputs.driveAppliedVolts = this.driveMotor.getMotorVoltage().getValue(); inputs.driveStatorCurrentAmps = this.driveMotor.getStatorCurrent().getValue(); inputs.driveSupplyCurrentAmps = this.driveMotor.getSupplyCurrent().getValue(); inputs.driveTempCelsius = this.driveMotor.getDeviceTemp().getValue(); - inputs.angleAbsolutePositionDeg = this.angleEncoder.getAbsolutePosition().getValue() * 360.0; + inputs.steerAbsolutePositionDeg = this.angleEncoder.getAbsolutePosition().getValue() * 360.0; // since we are using the FusedCANcoder feature, the position and velocity signal for the angle // motor accounts for the gear ratio; so, pass a gear ratio of 1 to just convert from rotations // to degrees. - inputs.anglePositionDeg = + inputs.steerPositionDeg = Conversions.falconRotationsToMechanismDegrees( BaseStatusSignal.getLatencyCompensatedValue( anglePositionStatusSignal, angleVelocityStatusSignal), 1); - inputs.anglePositionReference = + inputs.steerPositionReferenceDeg = Conversions.falconRotationsToMechanismDegrees( anglePositionReferenceStatusSignal.getValue(), 1); - inputs.anglePositionErrorDeg = + inputs.steerPositionErrorDeg = Conversions.falconRotationsToMechanismDegrees(anglePositionErrorStatusSignal.getValue(), 1); - inputs.angleVelocityRevPerMin = + inputs.steerVelocityRevPerMin = Conversions.falconRPSToMechanismRPM(angleVelocityStatusSignal.getValue(), 1); - inputs.angleAppliedPercentage = this.angleMotor.getDutyCycle().getValue(); - inputs.angleStatorCurrentAmps = this.angleMotor.getStatorCurrent().getValue(); - inputs.angleSupplyCurrentAmps = this.angleMotor.getSupplyCurrent().getValue(); - inputs.angleTempCelsius = this.angleMotor.getDeviceTemp().getValue(); + inputs.steerAppliedVolts = this.angleMotor.getMotorVoltage().getValue(); + inputs.steerStatorCurrentAmps = this.angleMotor.getStatorCurrent().getValue(); + inputs.steerSupplyCurrentAmps = this.angleMotor.getSupplyCurrent().getValue(); + inputs.steerTempCelsius = this.angleMotor.getDeviceTemp().getValue(); // update tunables if (driveKp.hasChanged() @@ -366,14 +371,14 @@ public void updateInputs(SwerveModuleIOInputs inputs) { /** Run the drive motor at the specified percentage of full power (12 V). */ @Override - public void setDriveMotorPercentage(double percentage) { - this.driveMotor.setControl(driveVoltageRequest.withOutput(percentage * 12.0)); + public void setDriveMotorVoltage(double voltage) { + this.driveMotor.setControl(driveVoltageRequest.withOutput(voltage)); } /** Run the angle motor at the specified percentage of full power. */ @Override - public void setAngleMotorPercentage(double percentage) { - this.angleMotor.setControl(angleVoltageRequest.withOutput(percentage * 12.0)); + public void setAngleMotorVoltage(double voltage) { + this.angleMotor.setControl(angleVoltageRequest.withOutput(voltage)); } /** Run the drive motor at the specified velocity. */ diff --git a/src/main/java/frc/lib/team3061/gyro/GyroIOPigeon2Phoenix6.java b/src/main/java/frc/lib/team3061/gyro/GyroIOPigeon2Phoenix6.java index b78d35c8..cfed82e6 100644 --- a/src/main/java/frc/lib/team3061/gyro/GyroIOPigeon2Phoenix6.java +++ b/src/main/java/frc/lib/team3061/gyro/GyroIOPigeon2Phoenix6.java @@ -28,17 +28,17 @@ public class GyroIOPigeon2Phoenix6 implements GyroIO { public GyroIOPigeon2Phoenix6(int id) { gyro = new Pigeon2(id, RobotConfig.getInstance().getCANBusName()); - this.yawStatusSignal = this.gyro.getYaw(); + this.yawStatusSignal = this.gyro.getYaw().clone(); this.yawStatusSignal.setUpdateFrequency(100); - this.pitchStatusSignal = this.gyro.getPitch(); + this.pitchStatusSignal = this.gyro.getPitch().clone(); this.pitchStatusSignal.setUpdateFrequency(100); - this.rollStatusSignal = this.gyro.getRoll(); + this.rollStatusSignal = this.gyro.getRoll().clone(); this.rollStatusSignal.setUpdateFrequency(100); - this.angularVelocityXStatusSignal = this.gyro.getAngularVelocityX(); + this.angularVelocityXStatusSignal = this.gyro.getAngularVelocityXWorld().clone(); this.angularVelocityXStatusSignal.setUpdateFrequency(100); - this.angularVelocityYStatusSignal = this.gyro.getAngularVelocityY(); + this.angularVelocityYStatusSignal = this.gyro.getAngularVelocityYWorld().clone(); this.angularVelocityYStatusSignal.setUpdateFrequency(100); - this.angularVelocityZStatusSignal = this.gyro.getAngularVelocityZ(); + this.angularVelocityZStatusSignal = this.gyro.getAngularVelocityZWorld().clone(); this.angularVelocityZStatusSignal.setUpdateFrequency(100); FaultReporter.getInstance().registerHardware("Drivetrain", "gyro", gyro); @@ -55,16 +55,18 @@ public void updateInputs(GyroIOInputs inputs) { // only invoke refresh if Phoenix is not licensed (if licensed, these signals have already been // refreshed) if (!RobotConfig.getInstance().getPhoenix6Licensed()) { - this.yawStatusSignal.refresh(); - this.angularVelocityZStatusSignal.refresh(); + BaseStatusSignal.refreshAll(this.yawStatusSignal, this.angularVelocityZStatusSignal); + } else { + BaseStatusSignal.refreshAll( + this.yawStatusSignal, + this.angularVelocityZStatusSignal, + this.pitchStatusSignal, + this.rollStatusSignal, + this.angularVelocityXStatusSignal, + this.angularVelocityYStatusSignal); } - this.pitchStatusSignal.refresh(); - this.rollStatusSignal.refresh(); - this.angularVelocityXStatusSignal.refresh(); - this.angularVelocityYStatusSignal.refresh(); - - inputs.connected = (this.yawStatusSignal.getError() == StatusCode.OK); + inputs.connected = (this.yawStatusSignal.getStatus() == StatusCode.OK); inputs.yawDeg = BaseStatusSignal.getLatencyCompensatedValue( this.yawStatusSignal, this.angularVelocityZStatusSignal); diff --git a/src/main/java/frc/lib/team3061/pneumatics/Pneumatics.java b/src/main/java/frc/lib/team3061/pneumatics/Pneumatics.java index 6720e84c..37587a53 100644 --- a/src/main/java/frc/lib/team3061/pneumatics/Pneumatics.java +++ b/src/main/java/frc/lib/team3061/pneumatics/Pneumatics.java @@ -60,12 +60,12 @@ public double getPressure() { @Override public void periodic() { io.updateInputs(inputs); - Logger.getInstance().processInputs("Pneumatics", inputs); + Logger.processInputs("Pneumatics", inputs); calculateAveragePressure(); // Log pressure - Logger.getInstance().recordOutput("PressurePsi", pressureSmoothedPsi); + Logger.recordOutput("PressurePsi", pressureSmoothedPsi); // Detect if dump value is open if (inputs.highPressurePSI > 3) { diff --git a/src/main/java/frc/lib/team3061/swerve/SwerveModule.java b/src/main/java/frc/lib/team3061/swerve/SwerveModule.java deleted file mode 100644 index 3d46bd33..00000000 --- a/src/main/java/frc/lib/team3061/swerve/SwerveModule.java +++ /dev/null @@ -1,275 +0,0 @@ -/* - * Initially from https://github.com/Team364/BaseFalconSwerve - */ - -package frc.lib.team3061.swerve; - -import static frc.robot.Constants.*; - -import com.ctre.phoenix6.StatusSignal; -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.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; -import edu.wpi.first.wpilibj2.command.CommandBase; -import edu.wpi.first.wpilibj2.command.Commands; -import frc.lib.team3015.subsystem.FaultReporter; -import java.util.List; -import org.littletonrobotics.junction.Logger; - -/** SwerveModule models a single swerve module. */ -public class SwerveModule { - private final SwerveModuleIO io; - private final SwerveModuleIOInputsAutoLogged inputs = new SwerveModuleIOInputsAutoLogged(); - - private int moduleNumber; - private String subsystemName; - private double lastAngle; - private double maxVelocity; - private double lastAngleMotorVelocity = 0.0; - private CommandBase wrappedSystemCheckCommand; - - private static final boolean DEBUGGING = false; - - /** - * Create a new swerve module. - * - * @param io the hardware-abstracted swerve module object - * @param moduleNumber the module number (0-3) - * @param maxVelocity the maximum drive velocity of the module in meters per second - */ - public SwerveModule(SwerveModuleIO io, int moduleNumber, double maxVelocity) { - this.io = io; - this.moduleNumber = moduleNumber; - this.maxVelocity = maxVelocity; - this.subsystemName = "SwerveModule" + moduleNumber; - - lastAngle = getState().angle.getDegrees(); - - /* set DEBUGGING to true to view values in Shuffleboard. This is useful when determining the steer offset constants. */ - if (DEBUGGING) { - ShuffleboardTab tab = Shuffleboard.getTab("Swerve"); - tab.addNumber( - "Mod " + this.moduleNumber + ": Cancoder", () -> inputs.angleAbsolutePositionDeg); - tab.addNumber("Mod " + this.moduleNumber + ": Integrated", () -> inputs.anglePositionDeg); - tab.addNumber( - "Mod " + this.moduleNumber + ": Velocity", () -> inputs.driveVelocityMetersPerSec); - } - - FaultReporter faultReporter = FaultReporter.getInstance(); - this.wrappedSystemCheckCommand = - faultReporter.registerSystemCheck(this.subsystemName, getSystemCheckCommand()); - } - - /** - * Set this swerve module to the specified speed and angle. - * - * @param desiredState the desired state of the module - * @param isOpenLoop if true, the drive motor will be set to the calculated fraction of the max - * velocity; if false, the drive motor will set to the specified velocity using a closed-loop - * controller (PID). - * @param forceAngle if true, the module will be forced to rotate to the specified angle; if - * false, the module will not rotate if the velocity is less than 1% of the max velocity. - */ - public void setDesiredState( - SwerveModuleState desiredState, boolean isOpenLoop, boolean forceAngle) { - - desiredState = SwerveModuleState.optimize(desiredState, getState().angle); - - if (isOpenLoop) { - double percentOutput = desiredState.speedMetersPerSecond / maxVelocity; - io.setDriveMotorPercentage(percentOutput); - } else { - io.setDriveVelocity(desiredState.speedMetersPerSecond); - } - - // Unless the angle is forced (e.g., X-stance), don't rotate the module if speed is less then - // 1%. This prevents jittering if the controller isn't tuned perfectly. Perhaps more - // importantly, it allows for smooth repeated movement as the wheel direction doesn't reset - // during pauses (e.g., multi-segmented auto paths). - double angle; - if (!forceAngle && Math.abs(desiredState.speedMetersPerSecond) <= (maxVelocity * 0.01)) { - angle = lastAngle; - } else { - angle = desiredState.angle.getDegrees(); - } - - io.setAnglePosition(angle); - lastAngle = angle; - } - - /** - * Set the drive motor to the specified voltage. This is only used for characterization via the - * FeedForwardCharacterization command. The module will be set to 0 degrees throughout the - * characterization; as a result, the wheels don't need to be clamped to hold them straight. - * - * @param voltage the specified voltage for the drive motor - */ - public void setVoltageForDriveCharacterization(double voltage) { - io.setAnglePosition(0.0); - lastAngle = 0.0; - io.setDriveMotorPercentage(voltage / 12.0); - } - - /** - * Set the angle motor to the specified voltage. This is only used for characterization via the - * FeedForwardCharacterization command. - * - * @param voltage the specified voltage for the angle motor - */ - public void setVoltageForRotateCharacterization(double voltage) { - io.setAngleMotorPercentage(voltage / 12.0); - } - - /** - * Get the current state of this swerve module. - * - * @return the current state of this swerve module - */ - public SwerveModuleState getState() { - double velocity = inputs.driveVelocityMetersPerSec; - Rotation2d angle = Rotation2d.fromDegrees(inputs.anglePositionDeg); - return new SwerveModuleState(velocity, angle); - } - - /** - * Get the current position of this swerve module. - * - * @return the current position of this swerve module - */ - public SwerveModulePosition getPosition() { - double distance = inputs.driveDistanceMeters; - Rotation2d angle = Rotation2d.fromDegrees(inputs.anglePositionDeg); - return new SwerveModulePosition(distance, angle); - } - - /** - * Get the number of this swerve module. - * - * @return the number of this swerve module - */ - public int getModuleNumber() { - return moduleNumber; - } - - /** - * Get the stator current of the drive motor of this swerve module. - * - * @return the stator current of the drive motor of this swerve module - */ - public double getDriveCurrent() { - return inputs.driveStatorCurrentAmps; - } - - /** - * Update this swerve module's inputs and log them. - * - *

This method must be invoked by the drivetrain subsystem's periodic method. - */ - public void updateAndProcessInputs() { - this.lastAngleMotorVelocity = inputs.angleVelocityRevPerMin; - io.updateInputs(inputs); - Logger.getInstance().processInputs("Mod" + moduleNumber, inputs); - } - - /** - * Set the brake mode of the drive motor. - * - * @param enable if true, the drive motor will be set to brake mode; if false, coast mode. - */ - public void setDriveBrakeMode(boolean enable) { - io.setDriveBrakeMode(enable); - } - - /** - * Set the brake mode of the angle motor. - * - * @param enable if true, the angle motor will be set to brake mode; if false, coast mode. - */ - public void setAngleBrakeMode(boolean enable) { - io.setAngleBrakeMode(enable); - } - - private CommandBase getSystemCheckCommand() { - return Commands.sequence( - Commands.run(() -> io.setAnglePosition(90.0)).withTimeout(1.0), - Commands.runOnce( - () -> { - double angle = inputs.anglePositionDeg % 360; - if (angle < 88 || angle > 92) { - FaultReporter.getInstance() - .addFault( - this.subsystemName, - "[System Check] Rotation Motor did not reach target position of 90 deg " - + angle, - false, - true); - } - }), - Commands.runOnce(() -> io.setDriveMotorPercentage(0.1)), - Commands.waitSeconds(0.5), - Commands.runOnce( - () -> { - if (inputs.driveVelocityMetersPerSec < 0.25) { - FaultReporter.getInstance() - .addFault( - this.subsystemName, - "[System Check] Drive motor encoder velocity too slow", - false, - true); - } - io.setDriveMotorPercentage(0.0); - }), - Commands.waitSeconds(0.25), - Commands.run(() -> io.setAnglePosition(0.0)).withTimeout(1.0), - Commands.runOnce( - () -> { - double angle = inputs.anglePositionDeg % 360; - if (angle < -2 || angle > 2) { - FaultReporter.getInstance() - .addFault( - this.subsystemName, - "[System Check] Rotation Motor did not reach target position of 0 deg " - + angle, - false, - true); - } - })) - .until(() -> !FaultReporter.getInstance().getFaults(this.subsystemName).isEmpty()) - .andThen(Commands.runOnce(() -> io.setDriveMotorPercentage(0.0))); - } - - public CommandBase getCheckCommand() { - return this.wrappedSystemCheckCommand; - } - - /** - * Get the velocity of the angle motor in radians per second. - * - * @return the velocity of the angle motor in radians per second - */ - public double getAngleMotorVelocity() { - return inputs.angleVelocityRevPerMin * 2.0 * Math.PI / 60.0; - } - - /** - * Get the acceleration of the angle motor in radians per second^2. - * - * @return the acceleration of the angle motor in radians per second^2 - */ - public double getAngleMotorAcceleration() { - return ((inputs.angleVelocityRevPerMin - this.lastAngleMotorVelocity) / 60.0 * 2.0 * Math.PI) - / LOOP_PERIOD_SECS; - } - - /** - * Returns a list of status signals for the swerve module related to odometry. This can be used to - * synchronize the gyro and swerve modules to improve the accuracy of pose estimation. - * - * @return the status signals for the swerve module - */ - public List> getOdometryStatusSignals() { - return io.getOdometryStatusSignals(); - } -} diff --git a/src/main/java/frc/lib/team3061/util/RobotOdometry.java b/src/main/java/frc/lib/team3061/util/RobotOdometry.java index 9938f352..4c470c6f 100644 --- a/src/main/java/frc/lib/team3061/util/RobotOdometry.java +++ b/src/main/java/frc/lib/team3061/util/RobotOdometry.java @@ -1,10 +1,16 @@ package frc.lib.team3061.util; +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.kinematics.SwerveModulePosition; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; import frc.lib.team3061.RobotConfig; +import frc.lib.team3061.drivetrain.DrivetrainIOCTRE; + +@java.lang.SuppressWarnings({"java:S6548"}) /** * Singleton class for SwerveDrivePoseEstimator that allows it to be shared by subsystems @@ -12,7 +18,8 @@ */ public class RobotOdometry { private static final RobotOdometry robotOdometry = new RobotOdometry(); - private SwerveDrivePoseEstimator estimator; + private SwerveDrivePoseEstimator estimator = null; + private DrivetrainIOCTRE customOdometry = null; private SwerveModulePosition[] defaultPositions = new SwerveModulePosition[] { new SwerveModulePosition(), @@ -30,11 +37,53 @@ private RobotOdometry() { new Pose2d()); } - public static RobotOdometry getInstance() { - return robotOdometry; + public Pose2d getEstimatedPosition() { + if (this.customOdometry == null) { + return this.estimator.getEstimatedPosition(); + } else { + return this.customOdometry.getEstimatedPosition(); + } + } + + public void resetPosition( + Rotation2d gyroAngle, SwerveModulePosition[] modulePositions, Pose2d poseMeters) { + if (this.customOdometry == null) { + this.estimator.resetPosition(gyroAngle, modulePositions, poseMeters); + + } else { + this.customOdometry.resetPosition(gyroAngle, modulePositions, poseMeters); + } + } + + public Pose2d updateWithTime( + double currentTimeSeconds, Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) { + if (this.customOdometry == null) { + return this.estimator.updateWithTime(currentTimeSeconds, gyroAngle, modulePositions); + + } else { + return this.customOdometry.updateWithTime(currentTimeSeconds, gyroAngle, modulePositions); + } + } + + public void addVisionMeasurement( + Pose2d visionRobotPoseMeters, + double timestampSeconds, + Matrix visionMeasurementStdDevs) { + if (this.customOdometry == null) { + this.estimator.addVisionMeasurement( + visionRobotPoseMeters, timestampSeconds, visionMeasurementStdDevs); + + } else { + this.customOdometry.addVisionMeasurement( + visionRobotPoseMeters, timestampSeconds, visionMeasurementStdDevs); + } } - public SwerveDrivePoseEstimator getPoseEstimator() { - return estimator; + public void setCustomOdometry(DrivetrainIOCTRE customOdometry) { + this.customOdometry = customOdometry; + } + + public static RobotOdometry getInstance() { + return robotOdometry; } } diff --git a/src/main/java/frc/lib/team3061/vision/Vision.java b/src/main/java/frc/lib/team3061/vision/Vision.java index 57ea7586..23a757d5 100644 --- a/src/main/java/frc/lib/team3061/vision/Vision.java +++ b/src/main/java/frc/lib/team3061/vision/Vision.java @@ -7,7 +7,6 @@ import edu.wpi.first.apriltag.AprilTagFieldLayout.OriginPosition; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Transform3d; @@ -51,7 +50,7 @@ public class Vision extends SubsystemBase { private boolean isEnabled = true; private boolean isVisionUpdating = false; - private SwerveDrivePoseEstimator poseEstimator; + private RobotOdometry odometry; private final TunableNumber poseDifferenceThreshold = new TunableNumber("Vision/VisionPoseThreshold", POSE_DIFFERENCE_THRESHOLD_METERS); private final TunableNumber stdDevSlope = new TunableNumber("Vision/stdDevSlope", 0.10); @@ -85,7 +84,7 @@ public Vision(VisionIO[] visionIOs) { } // retrieve a reference to the pose estimator singleton - this.poseEstimator = RobotOdometry.getInstance().getPoseEstimator(); + this.odometry = RobotOdometry.getInstance(); // add an indicator to the main Shuffleboard tab to indicate whether vision is updating in order // to alert the drive team if it is not. @@ -105,7 +104,7 @@ public Vision(VisionIO[] visionIOs) { } for (AprilTag tag : layout.getTags()) { - Logger.getInstance().recordOutput("Vision/AprilTags/" + tag.ID, tag.pose); + Logger.recordOutput("Vision/AprilTags/" + tag.ID, tag.pose); } } @@ -134,7 +133,7 @@ public void updateAlliance(DriverStation.Alliance newAlliance) { for (AprilTag tag : layout.getTags()) { layout .getTagPose(tag.ID) - .ifPresent(pose -> Logger.getInstance().recordOutput("Vision/AprilTags/" + tag.ID, pose)); + .ifPresent(pose -> Logger.recordOutput("Vision/AprilTags/" + tag.ID, pose)); } } @@ -148,7 +147,7 @@ public void periodic() { isVisionUpdating = false; for (int i = 0; i < visionIOs.length; i++) { visionIOs[i].updateInputs(ios[i]); - Logger.getInstance().processInputs("Vision" + i, ios[i]); + Logger.processInputs("Vision" + i, ios[i]); // only process the vision data if the timestamp is newer than the last one if (lastTimestamps[i] < ios[i].lastTimestamp) { @@ -160,11 +159,7 @@ public void periodic() { // only update the pose estimator if the pose from the vision data is close to the estimated // robot pose - if (poseEstimator - .getEstimatedPosition() - .minus(robotPose.toPose2d()) - .getTranslation() - .getNorm() + if (odometry.getEstimatedPosition().minus(robotPose.toPose2d()).getTranslation().getNorm() < MAX_POSE_DIFFERENCE_METERS) { // only update the pose estimator if the vision subsystem is enabled @@ -172,15 +167,15 @@ public void periodic() { // when updating the pose estimator, specify standard deviations based on the distance // from the robot to the AprilTag (the greater the distance, the less confident we are // in the measurement) - poseEstimator.addVisionMeasurement( + odometry.addVisionMeasurement( robotPose.toPose2d(), ios[i].lastTimestamp, getStandardDeviations(poseAndDistance.distanceToAprilTag)); isVisionUpdating = true; } - Logger.getInstance().recordOutput("Vision/RobotPose" + i, robotPose.toPose2d()); - Logger.getInstance().recordOutput("Vision/IsEnabled", isEnabled); + Logger.recordOutput("Vision/RobotPose" + i, robotPose.toPose2d()); + Logger.recordOutput("Vision/IsEnabled", isEnabled); } } } @@ -231,17 +226,13 @@ public boolean posesHaveConverged() { for (int i = 0; i < visionIOs.length; i++) { Pose3d robotPose = getRobotPose(i).robotPose; if (robotPose != null - && poseEstimator - .getEstimatedPosition() - .minus(robotPose.toPose2d()) - .getTranslation() - .getNorm() + && odometry.getEstimatedPosition().minus(robotPose.toPose2d()).getTranslation().getNorm() < poseDifferenceThreshold.get()) { - Logger.getInstance().recordOutput("Vision/posesInLine", true); + Logger.recordOutput("Vision/posesInLine", true); return true; } } - Logger.getInstance().recordOutput("Vision/posesInLine", false); + Logger.recordOutput("Vision/posesInLine", false); return false; } @@ -271,8 +262,8 @@ private RobotPoseFromAprilTag getRobotPose(int index) { // terms of logging, we are assuming that a given VisionIO object won't see more than 2 tags at // once for (int i = 0; i < 2; i++) { - Logger.getInstance().recordOutput("Vision/TagPose" + index + "_" + i, new Pose2d()); - Logger.getInstance().recordOutput("Vision/NVRobotPose" + index + "_" + i, new Pose2d()); + Logger.recordOutput("Vision/TagPose" + index + "_" + i, new Pose2d()); + Logger.recordOutput("Vision/NVRobotPose" + index + "_" + i, new Pose2d()); } for (PhotonTrackedTarget target : ios[index].lastResult.getTargets()) { @@ -284,10 +275,9 @@ private RobotPoseFromAprilTag getRobotPose(int index) { Pose3d cameraPose = tagPose.transformBy(cameraToTarget.inverse()); Pose3d robotPose = cameraPose.transformBy(camerasToRobots[index].inverse()); - Logger.getInstance() - .recordOutput("Vision/TagPose" + index + "_" + targetCount, tagPose.toPose2d()); - Logger.getInstance() - .recordOutput("Vision/NVRobotPose" + index + "_" + targetCount, robotPose.toPose2d()); + Logger.recordOutput("Vision/TagPose" + index + "_" + targetCount, tagPose.toPose2d()); + Logger.recordOutput( + "Vision/NVRobotPose" + index + "_" + targetCount, robotPose.toPose2d()); double targetDistance = target.getBestCameraToTarget().getTranslation().toTranslation2d().getNorm(); diff --git a/src/main/java/frc/lib/team3061/vision/VisionIO.java b/src/main/java/frc/lib/team3061/vision/VisionIO.java index 508abb3e..3da1534b 100644 --- a/src/main/java/frc/lib/team3061/vision/VisionIO.java +++ b/src/main/java/frc/lib/team3061/vision/VisionIO.java @@ -41,11 +41,11 @@ public void toLog(LogTable table) { } public void fromLog(LogTable table) { - byte[] photonPacketBytes = table.getRaw("photonPacketBytes", new byte[0]); + byte[] photonPacketBytes = table.get("photonPacketBytes", new byte[0]); lastResult = new PhotonPipelineResult(); lastResult.createFromPacket(new Packet(photonPacketBytes)); - lastTimestamp = table.getDouble("lastTimestamp", 0.0); + lastTimestamp = table.get("lastTimestamp", 0.0); } } diff --git a/src/main/java/frc/lib/team3061/vision/VisionIOSim.java b/src/main/java/frc/lib/team3061/vision/VisionIOSim.java index c2d96c06..61c948da 100644 --- a/src/main/java/frc/lib/team3061/vision/VisionIOSim.java +++ b/src/main/java/frc/lib/team3061/vision/VisionIOSim.java @@ -1,19 +1,19 @@ package frc.lib.team3061.vision; -import edu.wpi.first.apriltag.AprilTag; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFieldLayout.OriginPosition; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.DoubleArraySubscriber; import edu.wpi.first.networktables.NetworkTableEvent; import edu.wpi.first.networktables.NetworkTableInstance; import java.util.EnumSet; import java.util.function.Supplier; import org.photonvision.PhotonCamera; -import org.photonvision.SimVisionSystem; -import org.photonvision.SimVisionTarget; +import org.photonvision.simulation.PhotonCameraSim; +import org.photonvision.simulation.SimCameraProperties; +import org.photonvision.simulation.VisionSystemSim; import org.photonvision.targeting.PhotonPipelineResult; /** @@ -33,7 +33,8 @@ public class VisionIOSim implements VisionIO { private PhotonPipelineResult lastResult = new PhotonPipelineResult(); private Supplier poseSupplier; - private SimVisionSystem simVision; + private VisionSystemSim visionSim; + private PhotonCameraSim cameraSim; private AprilTagFieldLayout layout; /** @@ -48,18 +49,23 @@ public VisionIOSim( this.layout = layout; this.poseSupplier = poseSupplier; - this.simVision = - new SimVisionSystem( - CAMERA_NAME, DIAGONAL_FOV, robotToCamera, 9000, IMG_WIDTH, IMG_HEIGHT, 0); + this.visionSim = new VisionSystemSim(CAMERA_NAME); + this.visionSim.addAprilTags(layout); + SimCameraProperties cameraProp = new SimCameraProperties(); + cameraProp.setCalibration(IMG_WIDTH, IMG_HEIGHT, Rotation2d.fromDegrees(DIAGONAL_FOV)); + cameraProp.setCalibError(0.35, 0.10); + cameraProp.setFPS(15); + cameraProp.setAvgLatencyMs(50); + cameraProp.setLatencyStdDevMs(15); + + this.cameraSim = new PhotonCameraSim(camera, cameraProp); + + visionSim.addCamera(cameraSim, robotToCamera); + cameraSim.enableDrawWireframe(true); // default to the blue alliance; can be changed by invoking the setLayoutOrigin method layout.setOrigin(OriginPosition.kBlueAllianceWallRightSide); - for (AprilTag tag : layout.getTags()) { - this.simVision.addSimVisionTarget( - new SimVisionTarget(tag.pose, Units.inchesToMeters(6), Units.inchesToMeters(6), tag.ID)); - } - NetworkTableInstance inst = NetworkTableInstance.getDefault(); /* @@ -91,7 +97,7 @@ public VisionIOSim( */ @Override public synchronized void updateInputs(VisionIOInputs inputs) { - this.simVision.processFrame(poseSupplier.get()); + this.visionSim.update(poseSupplier.get()); inputs.lastTimestamp = this.lastTimestamp; inputs.lastResult = this.lastResult; } @@ -106,15 +112,7 @@ public synchronized void updateInputs(VisionIOInputs inputs) { public void setLayoutOrigin(OriginPosition origin) { layout.setOrigin(origin); - this.simVision.clearVisionTargets(); - for (AprilTag tag : layout.getTags()) { - layout - .getTagPose(tag.ID) - .ifPresent( - pose -> - this.simVision.addSimVisionTarget( - new SimVisionTarget( - pose, Units.inchesToMeters(6), Units.inchesToMeters(6), tag.ID))); - } + this.visionSim.clearVisionTargets(); + this.visionSim.addAprilTags(layout); } } diff --git a/src/main/java/frc/lib/team6328/util/FieldConstants.java b/src/main/java/frc/lib/team6328/util/FieldConstants.java index d322c484..091cdac4 100644 --- a/src/main/java/frc/lib/team6328/util/FieldConstants.java +++ b/src/main/java/frc/lib/team6328/util/FieldConstants.java @@ -17,6 +17,7 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import java.util.Map; +import java.util.Optional; @java.lang.SuppressWarnings({"java:S1118", "java:S115", "java:S2386"}) @@ -266,7 +267,8 @@ public static final class StagingLocations { * rightmost point on the BLUE ALLIANCE wall. */ public static Translation2d allianceFlip(Translation2d translation) { - if (DriverStation.getAlliance() == Alliance.Red) { + Optional alliance = DriverStation.getAlliance(); + if (alliance.isPresent() && alliance.get() == Alliance.Red) { return new Translation2d(fieldLength - translation.getX(), translation.getY()); } else { return translation; @@ -279,7 +281,8 @@ public static Translation2d allianceFlip(Translation2d translation) { * rightmost point on the BLUE ALLIANCE wall. */ public static Pose2d allianceFlip(Pose2d pose) { - if (DriverStation.getAlliance() == Alliance.Red) { + Optional alliance = DriverStation.getAlliance(); + if (alliance.isPresent() && alliance.get() == Alliance.Red) { return new Pose2d( fieldLength - pose.getX(), pose.getY(), diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 6ad4235a..a7fe4ce9 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -28,23 +28,26 @@ public final class Constants { // set to true in order to change all Tunable values via Shuffleboard public static final boolean TUNING_MODE = false; - private static final RobotType ROBOT = RobotType.ROBOT_2023_NOVA; + private static final RobotType ROBOT = RobotType.ROBOT_2023_NOVA_CTRE; private static final Alert invalidRobotAlert = new Alert("Invalid robot selected, using competition robot as default.", AlertType.ERROR); // FIXME: update for various robots public enum RobotType { + ROBOT_2023_NOVA_CTRE, ROBOT_2023_NOVA, ROBOT_2023_MK4I, ROBOT_DEFAULT, - ROBOT_SIMBOT + ROBOT_SIMBOT, + ROBOT_SIMBOT_CTRE } // FIXME: update for various robots public static RobotType getRobot() { if (RobotBase.isReal()) { - if (ROBOT == RobotType.ROBOT_SIMBOT) { // Invalid robot selected + if (ROBOT == RobotType.ROBOT_SIMBOT + || ROBOT == RobotType.ROBOT_SIMBOT_CTRE) { // Invalid robot selected invalidRobotAlert.set(true); return RobotType.ROBOT_DEFAULT; } else { @@ -59,11 +62,13 @@ public static RobotType getRobot() { public static Mode getMode() { switch (getRobot()) { case ROBOT_DEFAULT: + case ROBOT_2023_NOVA_CTRE: case ROBOT_2023_NOVA: case ROBOT_2023_MK4I: return RobotBase.isReal() ? Mode.REAL : Mode.REPLAY; case ROBOT_SIMBOT: + case ROBOT_SIMBOT_CTRE: return Mode.SIM; default: diff --git a/src/main/java/frc/robot/Field2d.java b/src/main/java/frc/robot/Field2d.java index b5a10eb1..dabc050d 100644 --- a/src/main/java/frc/robot/Field2d.java +++ b/src/main/java/frc/robot/Field2d.java @@ -1,16 +1,15 @@ package frc.robot; -import com.pathplanner.lib.PathConstraints; -import com.pathplanner.lib.PathPlanner; -import com.pathplanner.lib.PathPlannerTrajectory; -import com.pathplanner.lib.PathPoint; +import com.pathplanner.lib.path.GoalEndState; +import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.path.PathPlannerPath; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.DriverStation; import frc.lib.team3061.RobotConfig; +import frc.lib.team3061.drivetrain.Drivetrain; import frc.lib.team6328.util.FieldConstants; -import frc.robot.subsystems.drivetrain.Drivetrain; import java.util.ArrayList; import java.util.Arrays; import java.util.HashSet; @@ -32,7 +31,7 @@ public class Field2d { private static Field2d instance = null; private Region2d[] regions; - private DriverStation.Alliance alliance = DriverStation.Alliance.Invalid; + private DriverStation.Alliance alliance = DriverStation.Alliance.Red; /** * Get the singleton instance of the Field2d class. @@ -96,7 +95,7 @@ public Pose2d mapPoseToCurrentAlliance(Pose2d pose) { * @param subsystem the drivetrain subsystem * @return the path from the starting pose to the ending pose; null if no path exists */ - public PathPlannerTrajectory makePath( + public PathPlannerPath makePath( Pose2d start, Pose2d end, PathConstraints pathConstants, Drivetrain subsystem) { Region2d startRegion = null; Region2d endRegion = null; @@ -140,8 +139,13 @@ public PathPlannerTrajectory makePath( // add the ending point pointLocations.add(end.getTranslation()); - List pathPoints = createPathPoints(start, end, subsystem, pointLocations); - return PathPlanner.generatePath(pathConstants, pathPoints); + List pathPoses = createPathPoses(pointLocations); + List bezierPoints = PathPlannerPath.bezierFromPoses(pathPoses); + return new PathPlannerPath( + bezierPoints, + pathConstants, + new GoalEndState( + RobotConfig.getInstance().getMoveToPathFinalVelocity(), end.getRotation())); } /** @@ -158,53 +162,26 @@ public PathPlannerTrajectory makePath( * @param pointLocations the locations of the points in the path * @return the path points */ - private List createPathPoints( - Pose2d start, Pose2d end, Drivetrain subsystem, ArrayList pointLocations) { - List pathPoints = new ArrayList<>(); + private List createPathPoses(ArrayList pointLocations) { + List pathPoses = new ArrayList<>(); Rotation2d lastHeading = null; for (int i = 0; i < pointLocations.size() - 1; i++) { double deltaX = pointLocations.get(i + 1).getX() - pointLocations.get(i).getX(); double deltaY = pointLocations.get(i + 1).getY() - pointLocations.get(i).getY(); lastHeading = new Rotation2d(deltaX, deltaY); - if (i == 0) { - // if the robot is not currently moving, orient the heading towards the next point - if (subsystem.getVelocityX() == 0 && subsystem.getVelocityY() == 0) { - pathPoints.add( - new PathPoint( - pointLocations.get(i), - lastHeading, - start.getRotation(), - Math.sqrt( - Math.pow(subsystem.getVelocityX(), 2) - + Math.pow(subsystem.getVelocityY(), 2)))); - } - // if the robot is currently moving, maintain the current heading and velocity in order to - // have a smooth transition to the start of the path - else { - pathPoints.add( - new PathPoint( - pointLocations.get(i), - new Rotation2d(subsystem.getVelocityX(), subsystem.getVelocityY()), - start.getRotation(), - Math.sqrt( - Math.pow(subsystem.getVelocityX(), 2) - + Math.pow(subsystem.getVelocityY(), 2)))); - } - } else { - pathPoints.add(new PathPoint(pointLocations.get(i), lastHeading, end.getRotation())); - } + pathPoses.add( + new Pose2d(pointLocations.get(i).getX(), pointLocations.get(i).getY(), lastHeading)); } // the final path point will match the ending pose's rotation and the velocity as specified by // the robot's configuration class' getMoveToPathFinalVelocity method. - pathPoints.add( - new PathPoint( - pointLocations.get(pointLocations.size() - 1), - end.getRotation(), - end.getRotation(), - RobotConfig.getInstance().getMoveToPathFinalVelocity())); - - return pathPoints; + pathPoses.add( + new Pose2d( + pointLocations.get(pointLocations.size() - 1).getX(), + pointLocations.get(pointLocations.size() - 1).getY(), + lastHeading)); + + return pathPoses; } private List breadthFirstSearch(Region2d start, Region2d end) { @@ -220,10 +197,9 @@ private List breadthFirstSearch(Region2d start, Region2d end) { } todo.add( - new ArrayList<>( - Arrays.asList(start))); // add a path starting with startRegion to the todo list + new ArrayList<>(Arrays.asList(start))); // add a path starting with startRegion to the list - while (!todo.isEmpty()) { // while the todo list isn't empty, keep looking over the todo list. + while (!todo.isEmpty()) { // while the list isn't empty, keep looking over the list. ArrayList path = todo.poll(); Region2d region = path.get(path.size() - 1); // last region in the path diff --git a/src/main/java/frc/robot/LocalADStarAK.java b/src/main/java/frc/robot/LocalADStarAK.java new file mode 100644 index 00000000..d07f1792 --- /dev/null +++ b/src/main/java/frc/robot/LocalADStarAK.java @@ -0,0 +1,150 @@ +package frc.robot; + +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; + +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 { + private LocalADStar adStar = new LocalADStar(); + private boolean isNewPathAvailable = false; + private 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/Region2d.java b/src/main/java/frc/robot/Region2d.java index f038d3c6..96fa99b0 100644 --- a/src/main/java/frc/robot/Region2d.java +++ b/src/main/java/frc/robot/Region2d.java @@ -18,7 +18,7 @@ public class Region2d { private Path2D shape; private HashMap transitionMap; - private DriverStation.Alliance alliance = DriverStation.Alliance.Invalid; + private DriverStation.Alliance alliance = DriverStation.Alliance.Red; /** * Create a Region2d, a polygon, from an array of Translation2d specifying vertices of a polygon. @@ -45,32 +45,27 @@ public Region2d(Translation2d[] points) { public void logPoints() { // log the bounding rectangle of the region Rectangle2D rect = this.shape.getBounds2D(); - Logger.getInstance() - .recordOutput("Region2d/point0", new Pose2d(rect.getX(), rect.getY(), new Rotation2d())); - Logger.getInstance() - .recordOutput( - "Region2d/point1", - new Pose2d(rect.getX() + rect.getWidth(), rect.getY(), new Rotation2d())); - Logger.getInstance() - .recordOutput( - "Region2d/point2", - new Pose2d(rect.getX(), rect.getY() + rect.getHeight(), new Rotation2d())); - Logger.getInstance() - .recordOutput( - "Region2d/point3", - new Pose2d( - rect.getX() + rect.getWidth(), rect.getY() + rect.getHeight(), new Rotation2d())); + Logger.recordOutput("Region2d/point0", new Pose2d(rect.getX(), rect.getY(), new Rotation2d())); + Logger.recordOutput( + "Region2d/point1", + new Pose2d(rect.getX() + rect.getWidth(), rect.getY(), new Rotation2d())); + Logger.recordOutput( + "Region2d/point2", + new Pose2d(rect.getX(), rect.getY() + rect.getHeight(), new Rotation2d())); + Logger.recordOutput( + "Region2d/point3", + new Pose2d( + rect.getX() + rect.getWidth(), rect.getY() + rect.getHeight(), new Rotation2d())); // assume that there are at most 4 neighbors for (int i = 0; i < 4; i++) { - Logger.getInstance().recordOutput("Region2d/transition" + i, new Pose2d()); + Logger.recordOutput("Region2d/transition" + i, new Pose2d()); } int i = 0; for (Entry entry : transitionMap.entrySet()) { Translation2d point = entry.getValue(); - Logger.getInstance() - .recordOutput( - "Region2d/transition" + i, new Pose2d(point.getX(), point.getY(), new Rotation2d())); + Logger.recordOutput( + "Region2d/transition" + i, new Pose2d(point.getX(), point.getY(), new Rotation2d())); i++; } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index ab061d63..031f9d93 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,8 +4,8 @@ package frc.robot; -import com.pathplanner.lib.commands.PPSwerveControllerCommand; -import edu.wpi.first.math.util.Units; +import com.pathplanner.lib.pathfinding.Pathfinding; +import com.pathplanner.lib.util.PathPlannerLogging; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.lib.team6328.util.Alert; @@ -40,45 +40,45 @@ public Robot() { */ @Override public void robotInit() { + // DO THIS FIRST + Pathfinding.setPathfinder(new LocalADStarAK()); + final String GIT_DIRTY = "GitDirty"; // from AdvantageKit Robot Configuration docs // (https://github.com/Mechanical-Advantage/AdvantageKit/blob/main/docs/START-LOGGING.md#robot-configuration) - Logger logger = Logger.getInstance(); - // Set a metadata value - logger.recordMetadata("RuntimeType", getRuntimeType().toString()); - 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); + Logger.recordMetadata("RuntimeType", getRuntimeType().toString()); + 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(GIT_DIRTY, "All changes committed"); + Logger.recordMetadata(GIT_DIRTY, "All changes committed"); break; case 1: - logger.recordMetadata(GIT_DIRTY, "Uncommitted changes"); + Logger.recordMetadata(GIT_DIRTY, "Uncommitted changes"); break; default: - logger.recordMetadata(GIT_DIRTY, "Unknown"); + Logger.recordMetadata(GIT_DIRTY, "Unknown"); break; } switch (Constants.getMode()) { case REAL: - logger.addDataReceiver(new WPILOGWriter("/media/sda1")); + Logger.addDataReceiver(new WPILOGWriter("/media/sda1")); // Provide log data over the network, viewable in Advantage Scope. - logger.addDataReceiver(new NT4Publisher()); + Logger.addDataReceiver(new NT4Publisher()); LoggedPowerDistribution.getInstance(); break; case SIM: - logger.addDataReceiver(new WPILOGWriter("")); - logger.addDataReceiver(new NT4Publisher()); + Logger.addDataReceiver(new NT4Publisher()); break; case REPLAY: @@ -89,46 +89,38 @@ public void robotInit() { String path = LogFileUtil.findReplayLog(); // Read log file for replay - logger.setReplaySource(new WPILOGReader(path)); + Logger.setReplaySource(new WPILOGReader(path)); // Save replay results to a new log with the "_sim" suffix - logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(path, "_sim"))); + Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(path, "_sim"))); break; } // Start logging! No more data receivers, replay sources, or metadata values may be added. - logger.start(); + Logger.start(); // Alternative logging of scheduled commands CommandScheduler.getInstance() .onCommandInitialize( - command -> Logger.getInstance().recordOutput("Command initialized", command.getName())); + command -> Logger.recordOutput("Command initialized", command.getName())); CommandScheduler.getInstance() .onCommandInterrupt( - command -> Logger.getInstance().recordOutput("Command interrupted", command.getName())); + command -> Logger.recordOutput("Command interrupted", command.getName())); CommandScheduler.getInstance() - .onCommandFinish( - command -> Logger.getInstance().recordOutput("Command finished", command.getName())); + .onCommandFinish(command -> Logger.recordOutput("Command finished", command.getName())); // Logging of autonomous paths - PPSwerveControllerCommand.setLoggingCallbacks( - activeTrajectory -> - Logger.getInstance().recordOutput("PathFollowing/trajectory", activeTrajectory), - targetPose -> Logger.getInstance().recordOutput("PathFollowing/targetPose", targetPose), - setpoint -> { - Logger.getInstance().recordOutput("PathFollowing/targetXVel", setpoint.vxMetersPerSecond); - Logger.getInstance().recordOutput("PathFollowing/targetYVel", setpoint.vyMetersPerSecond); - Logger.getInstance() - .recordOutput( - "PathFollowing/targetAngularVel", - Units.radiansToDegrees(setpoint.omegaRadiansPerSecond)); - }, - (translationError, rotationError) -> { - Logger.getInstance().recordOutput("PathFollowing/xPosError", translationError.getX()); - Logger.getInstance().recordOutput("PathFollowing/yPosError", translationError.getY()); - Logger.getInstance() - .recordOutput("PathFollowing/rotationError", rotationError.getDegrees()); - }); + // Logging callback for current robot pose + PathPlannerLogging.setLogCurrentPoseCallback( + pose -> Logger.recordOutput("PathFollowing/currentPose", pose)); + + // Logging callback for target robot pose + PathPlannerLogging.setLogTargetPoseCallback( + pose -> Logger.recordOutput("PathFollowing/targetPose", pose)); + + // Logging callback for the active path, this is sent as a list of poses + PathPlannerLogging.setLogActivePathCallback( + poses -> Logger.recordOutput("PathFollowing/activePath", poses)); // Invoke the factory method to create the RobotContainer singleton. robotContainer = RobotContainer.getInstance(); @@ -151,7 +143,7 @@ public void robotPeriodic() { */ CommandScheduler.getInstance().run(); - logReceiverQueueAlert.set(Logger.getInstance().getReceiverQueueFault()); + logReceiverQueueAlert.set(Logger.getReceiverQueueFault()); } /** This method is invoked periodically when the robot is in the disabled state. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0cdba17f..59d77dec 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,13 +4,11 @@ package frc.robot; -import static frc.robot.Constants.*; import static frc.robot.FieldRegionConstants.*; -import com.pathplanner.lib.PathPlanner; -import com.pathplanner.lib.PathPlannerTrajectory; -import com.pathplanner.lib.commands.FollowPathWithEvents; -import com.pathplanner.lib.server.PathPlannerServer; +import com.pathplanner.lib.auto.NamedCommands; +import com.pathplanner.lib.commands.PathPlannerAuto; +import com.pathplanner.lib.path.PathPlannerPath; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; @@ -20,14 +18,16 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; import frc.lib.team3061.RobotConfig; +import frc.lib.team3061.drivetrain.Drivetrain; +import frc.lib.team3061.drivetrain.DrivetrainIO; +import frc.lib.team3061.drivetrain.DrivetrainIOCTRE; +import frc.lib.team3061.drivetrain.DrivetrainIOGeneric; +import frc.lib.team3061.drivetrain.swerve.SwerveModuleIO; +import frc.lib.team3061.drivetrain.swerve.SwerveModuleIOTalonFXPhoenix6; import frc.lib.team3061.gyro.GyroIO; import frc.lib.team3061.gyro.GyroIOPigeon2Phoenix6; import frc.lib.team3061.pneumatics.Pneumatics; -import frc.lib.team3061.pneumatics.PneumaticsIO; import frc.lib.team3061.pneumatics.PneumaticsIORev; -import frc.lib.team3061.swerve.SwerveModule; -import frc.lib.team3061.swerve.SwerveModuleIO; -import frc.lib.team3061.swerve.SwerveModuleIOTalonFXPhoenix6; import frc.lib.team3061.vision.Vision; import frc.lib.team3061.vision.VisionConstants; import frc.lib.team3061.vision.VisionIO; @@ -36,22 +36,19 @@ import frc.robot.Constants.Mode; import frc.robot.commands.FeedForwardCharacterization; import frc.robot.commands.FeedForwardCharacterization.FeedForwardCharacterizationData; -import frc.robot.commands.FollowPath; import frc.robot.commands.RotateToAngle; import frc.robot.commands.TeleopSwerve; import frc.robot.configs.DefaultRobotConfig; import frc.robot.configs.MK4IRobotConfig; +import frc.robot.configs.NovaCTRERobotConfig; import frc.robot.configs.NovaRobotConfig; import frc.robot.operator_interface.OISelector; import frc.robot.operator_interface.OperatorInterface; -import frc.robot.subsystems.drivetrain.Drivetrain; import frc.robot.subsystems.subsystem.Subsystem; import frc.robot.subsystems.subsystem.SubsystemIO; import java.io.IOException; import java.util.ArrayList; -import java.util.HashMap; -import java.util.List; -import java.util.Map; +import java.util.Optional; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; /** @@ -64,7 +61,7 @@ public class RobotContainer { private OperatorInterface oi = new OperatorInterface() {}; private RobotConfig config; private Drivetrain drivetrain; - private Alliance lastAlliance = DriverStation.Alliance.Invalid; + private Alliance lastAlliance = DriverStation.Alliance.Red; private Vision vision; private Subsystem subsystem; @@ -75,8 +72,6 @@ public class RobotContainer { // RobotContainer singleton private static RobotContainer robotContainer = new RobotContainer(); - private final Map autoEventMap = new HashMap<>(); - /** * Create the container for the robot. Contains subsystems, operator interface (OI) devices, and * commands. @@ -90,61 +85,12 @@ public RobotContainer() { // create real, simulated, or replay subsystems based on the mode and robot specified if (Constants.getMode() != Mode.REPLAY) { - int[] driveMotorCANIDs = config.getSwerveDriveMotorCANIDs(); - int[] steerMotorCANDIDs = config.getSwerveSteerMotorCANIDs(); - int[] steerEncoderCANDIDs = config.getSwerveSteerEncoderCANIDs(); - double[] steerOffsets = config.getSwerveSteerOffsets(); - SwerveModule flModule = - new SwerveModule( - new SwerveModuleIOTalonFXPhoenix6( - 0, - driveMotorCANIDs[0], - steerMotorCANDIDs[0], - steerEncoderCANDIDs[0], - steerOffsets[0]), - 0, - config.getRobotMaxVelocity()); - - SwerveModule frModule = - new SwerveModule( - new SwerveModuleIOTalonFXPhoenix6( - 1, - driveMotorCANIDs[1], - steerMotorCANDIDs[1], - steerEncoderCANDIDs[1], - steerOffsets[1]), - 1, - config.getRobotMaxVelocity()); - - SwerveModule blModule = - new SwerveModule( - new SwerveModuleIOTalonFXPhoenix6( - 2, - driveMotorCANIDs[2], - steerMotorCANDIDs[2], - steerEncoderCANDIDs[2], - steerOffsets[2]), - 2, - config.getRobotMaxVelocity()); - - SwerveModule brModule = - new SwerveModule( - new SwerveModuleIOTalonFXPhoenix6( - 3, - driveMotorCANIDs[3], - steerMotorCANDIDs[3], - steerEncoderCANDIDs[3], - steerOffsets[3]), - 3, - config.getRobotMaxVelocity()); + switch (Constants.getRobot()) { - case ROBOT_DEFAULT: - case ROBOT_2023_NOVA: - case ROBOT_2023_MK4I: + case ROBOT_2023_NOVA_CTRE: { - GyroIO gyro = new GyroIOPigeon2Phoenix6(config.getGyroCANID()); - - drivetrain = new Drivetrain(gyro, flModule, frModule, blModule, brModule); + DrivetrainIO drivetrainIO = new DrivetrainIOCTRE(); + drivetrain = new Drivetrain(drivetrainIO); String[] cameraNames = config.getCameraNames(); VisionIO[] visionIOs = new VisionIO[cameraNames.length]; @@ -152,20 +98,94 @@ public RobotContainer() { visionIOs[i] = new VisionIOPhotonVision(cameraNames[i]); } vision = new Vision(visionIOs); + // subsystem = new Subsystem(new SubsystemIOTalonFX()); + subsystem = new Subsystem(new SubsystemIO() {}); + break; + } + case ROBOT_DEFAULT: + case ROBOT_2023_NOVA: + case ROBOT_2023_MK4I: + case ROBOT_SIMBOT: + { + int[] driveMotorCANIDs = config.getSwerveDriveMotorCANIDs(); + int[] steerMotorCANDIDs = config.getSwerveSteerMotorCANIDs(); + int[] steerEncoderCANDIDs = config.getSwerveSteerEncoderCANIDs(); + double[] steerOffsets = config.getSwerveSteerOffsets(); + SwerveModuleIO flModule = + new SwerveModuleIOTalonFXPhoenix6( + 0, + driveMotorCANIDs[0], + steerMotorCANDIDs[0], + steerEncoderCANDIDs[0], + steerOffsets[0]); + + SwerveModuleIO frModule = + new SwerveModuleIOTalonFXPhoenix6( + 1, + driveMotorCANIDs[1], + steerMotorCANDIDs[1], + steerEncoderCANDIDs[1], + steerOffsets[1]); + + SwerveModuleIO blModule = + new SwerveModuleIOTalonFXPhoenix6( + 2, + driveMotorCANIDs[2], + steerMotorCANDIDs[2], + steerEncoderCANDIDs[2], + steerOffsets[2]); + + SwerveModuleIO brModule = + new SwerveModuleIOTalonFXPhoenix6( + 3, + driveMotorCANIDs[3], + steerMotorCANDIDs[3], + steerEncoderCANDIDs[3], + steerOffsets[3]); + + GyroIO gyro = new GyroIOPigeon2Phoenix6(config.getGyroCANID()); + DrivetrainIO drivetrainIO = + new DrivetrainIOGeneric(gyro, flModule, frModule, blModule, brModule); + drivetrain = new Drivetrain(drivetrainIO); + // subsystem = new Subsystem(new SubsystemIOTalonFX()); subsystem = new Subsystem(new SubsystemIO() {}); if (Constants.getRobot() == Constants.RobotType.ROBOT_DEFAULT) { new Pneumatics(new PneumaticsIORev()); } + + if (Constants.getRobot() == Constants.RobotType.ROBOT_SIMBOT) { + AprilTagFieldLayout layout; + try { + layout = new AprilTagFieldLayout(VisionConstants.APRILTAG_FIELD_LAYOUT_PATH); + } catch (IOException e) { + layout = new AprilTagFieldLayout(new ArrayList<>(), 16.4592, 8.2296); + } + vision = + new Vision( + new VisionIO[] { + new VisionIOSim( + layout, + drivetrain::getPose, + RobotConfig.getInstance().getRobotToCameraTransforms()[0]) + }); + } else { + String[] cameraNames = config.getCameraNames(); + VisionIO[] visionIOs = new VisionIO[cameraNames.length]; + for (int i = 0; i < visionIOs.length; i++) { + visionIOs[i] = new VisionIOPhotonVision(cameraNames[i]); + } + vision = new Vision(visionIOs); + } break; } - case ROBOT_SIMBOT: + case ROBOT_SIMBOT_CTRE: { - GyroIO gyro = new GyroIOPigeon2Phoenix6(config.getGyroCANID()); + DrivetrainIO drivetrainIO = new DrivetrainIOCTRE(); + drivetrain = new Drivetrain(drivetrainIO); - drivetrain = new Drivetrain(gyro, flModule, frModule, blModule, brModule); - new Pneumatics(new PneumaticsIO() {}); + // new Pneumatics(new PneumaticsIO() {}); AprilTagFieldLayout layout; try { layout = new AprilTagFieldLayout(VisionConstants.APRILTAG_FIELD_LAYOUT_PATH); @@ -180,7 +200,7 @@ public RobotContainer() { drivetrain::getPose, RobotConfig.getInstance().getRobotToCameraTransforms()[0]) }); - subsystem = new Subsystem(new SubsystemIO() {}); + // subsystem = new Subsystem(new SubsystemIO() {}); break; } @@ -189,18 +209,7 @@ public RobotContainer() { } } else { - SwerveModule flModule = - new SwerveModule(new SwerveModuleIO() {}, 0, config.getRobotMaxVelocity()); - - SwerveModule frModule = - new SwerveModule(new SwerveModuleIO() {}, 1, config.getRobotMaxVelocity()); - - SwerveModule blModule = - new SwerveModule(new SwerveModuleIO() {}, 2, config.getRobotMaxVelocity()); - - SwerveModule brModule = - new SwerveModule(new SwerveModuleIO() {}, 3, config.getRobotMaxVelocity()); - drivetrain = new Drivetrain(new GyroIO() {}, flModule, frModule, blModule, brModule); + drivetrain = new Drivetrain(new DrivetrainIO() {}); String[] cameraNames = config.getCameraNames(); VisionIO[] visionIOs = new VisionIO[cameraNames.length]; @@ -230,6 +239,10 @@ private void createRobotConfig() { case ROBOT_DEFAULT: config = new DefaultRobotConfig(); break; + case ROBOT_2023_NOVA_CTRE: + case ROBOT_SIMBOT_CTRE: + config = new NovaCTRERobotConfig(); + break; case ROBOT_2023_NOVA: case ROBOT_SIMBOT: config = new NovaRobotConfig(); @@ -329,8 +342,13 @@ private void configureButtonBindings() { /** Use this method to define your commands for autonomous mode. */ private void configureAutoCommands() { // Waypoints - autoEventMap.put("event1", Commands.print("passed marker 1")); - autoEventMap.put("event2", Commands.print("passed marker 2")); + NamedCommands.registerCommand("command1", Commands.print("passed marker 1")); + NamedCommands.registerCommand("command2", Commands.print("passed marker 2")); + NamedCommands.registerCommand( + "enableXStance", Commands.runOnce(drivetrain::enableXstance, drivetrain)); + NamedCommands.registerCommand( + "disableXStance", Commands.runOnce(drivetrain::disableXstance, drivetrain)); + NamedCommands.registerCommand("wait5Seconds", Commands.waitSeconds(5.0)); // build auto path commands @@ -342,23 +360,8 @@ private void configureAutoCommands() { * demonstration of PathPlanner path group with event markers * */ - List auto1Paths = - PathPlanner.loadPathGroup( - "TestPath", config.getAutoMaxSpeed(), config.getAutoMaxAcceleration()); - Command autoTest = - Commands.sequence( - new FollowPathWithEvents( - new FollowPath(auto1Paths.get(0), drivetrain, true, true), - auto1Paths.get(0).getMarkers(), - autoEventMap), - Commands.runOnce(drivetrain::enableXstance, drivetrain), - Commands.waitSeconds(5.0), - Commands.runOnce(drivetrain::disableXstance, drivetrain), - new FollowPathWithEvents( - new FollowPath(auto1Paths.get(1), drivetrain, false, true), - auto1Paths.get(1).getMarkers(), - autoEventMap)); - autoChooser.addOption("Test Path", autoTest); + Command autoTest = new PathPlannerAuto("TestAuto"); + autoChooser.addOption("Test Auto", autoTest); /************ Start Point ************ * @@ -366,12 +369,12 @@ private void configureAutoCommands() { * */ - PathPlannerTrajectory startPointPath = - PathPlanner.loadPath( - "StartPoint", config.getAutoMaxSpeed(), config.getAutoMaxAcceleration()); Command startPoint = Commands.runOnce( - () -> drivetrain.resetOdometry(startPointPath.getInitialState()), drivetrain); + () -> + drivetrain.resetPose( + PathPlannerPath.fromPathFile("StartPoint").getPreviewStartingHolonomicPose()), + drivetrain); autoChooser.addOption("Start Point", startPoint); /************ Drive Characterization ************ @@ -409,8 +412,7 @@ private void configureAutoCommands() { * used for empirically determining the wheel diameter * */ - PathPlannerTrajectory distanceTestPath = PathPlanner.loadPath("DistanceTest", 2, 2); - Command distanceTestPathCommand = new FollowPath(distanceTestPath, drivetrain, true, true); + Command distanceTestPathCommand = new PathPlannerAuto("DistanceTest"); autoChooser.addOption("Distance Path", distanceTestPathCommand); /************ Auto Tuning ************ @@ -418,8 +420,7 @@ private void configureAutoCommands() { * useful for tuning the autonomous PID controllers * */ - PathPlannerTrajectory tuningPath = PathPlanner.loadPath("Tuning", 2.0, 3.0); - Command tuningCommand = new FollowPath(tuningPath, drivetrain, true, true); + Command tuningCommand = new PathPlannerAuto("Tuning"); autoChooser.addOption("Auto Tuning", tuningCommand); /************ Drive Velocity Tuning ************ @@ -483,11 +484,6 @@ private void configureAutoCommands() { () -> drivetrain.drive(0.1, -0.1, 0.0, true, false), drivetrain))))); Shuffleboard.getTab("MAIN").add(autoChooser.getSendableChooser()); - - // enable the path planner server so we can update paths without redeploying code - if (TUNING_MODE) { - PathPlannerServer.startServer(3061); - } } private void configureDrivetrainCommands() { @@ -580,8 +576,9 @@ public Command getAutonomousCommand() { * singleton. */ public void checkAllianceColor() { - if (DriverStation.getAlliance() != lastAlliance) { - lastAlliance = DriverStation.getAlliance(); + Optional alliance = DriverStation.getAlliance(); + if (alliance.isPresent() && alliance.get() != lastAlliance) { + lastAlliance = alliance.get(); vision.updateAlliance(lastAlliance); Field2d.getInstance().updateAlliance(lastAlliance); } diff --git a/src/main/java/frc/robot/commands/DriveToPose.java b/src/main/java/frc/robot/commands/DriveToPose.java index 217f867d..fadb71dc 100644 --- a/src/main/java/frc/robot/commands/DriveToPose.java +++ b/src/main/java/frc/robot/commands/DriveToPose.java @@ -15,10 +15,10 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.Command; import frc.lib.team3061.RobotConfig; +import frc.lib.team3061.drivetrain.Drivetrain; import frc.lib.team6328.util.TunableNumber; -import frc.robot.subsystems.drivetrain.Drivetrain; import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; @@ -35,7 +35,7 @@ * *

At End: stops the drivetrain */ -public class DriveToPose extends CommandBase { +public class DriveToPose extends Command { private final Drivetrain drivetrain; private final Supplier poseSupplier; private Pose2d targetPose; @@ -125,7 +125,7 @@ public DriveToPose(Drivetrain drivetrain, Supplier poseSupplier) { */ @Override public void initialize() { - Logger.getInstance().recordOutput("ActiveCommands/DriveToPose", true); + Logger.recordOutput("ActiveCommands/DriveToPose", true); // Reset all controllers Pose2d currentPose = drivetrain.getPose(); @@ -137,7 +137,7 @@ public void initialize() { thetaController.setTolerance(thetaTolerance.get()); this.targetPose = poseSupplier.get(); - Logger.getInstance().recordOutput("DriveToPose/targetPose", targetPose); + Logger.recordOutput("DriveToPose/targetPose", targetPose); this.timer.restart(); } @@ -154,7 +154,6 @@ public void execute() { // the calculate method has not yet been invoked. running = true; - // Update from tunable numbers if (driveKp.hasChanged() || driveKd.hasChanged() @@ -212,9 +211,9 @@ public void execute() { */ @Override public boolean isFinished() { - Logger.getInstance().recordOutput("DriveToPose/xErr", xController.atGoal()); - Logger.getInstance().recordOutput("DriveToPose/yErr", yController.atGoal()); - Logger.getInstance().recordOutput("DriveToPose/tErr", thetaController.atGoal()); + Logger.recordOutput("DriveToPose/xErr", xController.atGoal()); + Logger.recordOutput("DriveToPose/yErr", yController.atGoal()); + Logger.recordOutput("DriveToPose/tErr", thetaController.atGoal()); // check that running is true (i.e., the calculate method has been invoked on the PID // controllers) and that each of the controllers is at their goal. This is important since these @@ -234,6 +233,6 @@ public boolean isFinished() { public void end(boolean interrupted) { drivetrain.stop(); running = false; - Logger.getInstance().recordOutput("ActiveCommands/DriveToPose", false); + Logger.recordOutput("ActiveCommands/DriveToPose", false); } } diff --git a/src/main/java/frc/robot/commands/FeedForwardCharacterization.java b/src/main/java/frc/robot/commands/FeedForwardCharacterization.java index 81632cf3..dc1758f1 100644 --- a/src/main/java/frc/robot/commands/FeedForwardCharacterization.java +++ b/src/main/java/frc/robot/commands/FeedForwardCharacterization.java @@ -9,7 +9,7 @@ package frc.robot.commands; import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Subsystem; import java.util.LinkedList; import java.util.List; @@ -19,7 +19,7 @@ import org.ejml.simple.SimpleMatrix; @java.lang.SuppressWarnings({"java:S106", "java:S107"}) -public class FeedForwardCharacterization extends CommandBase { +public class FeedForwardCharacterization extends Command { private static final double START_DELAY_SECS = 2.0; private static final double RAMP_RATE_VOLTS_PER_SECOND = 2.0; diff --git a/src/main/java/frc/robot/commands/FollowPath.java b/src/main/java/frc/robot/commands/FollowPath.java deleted file mode 100644 index ced7bdd6..00000000 --- a/src/main/java/frc/robot/commands/FollowPath.java +++ /dev/null @@ -1,108 +0,0 @@ -package frc.robot.commands; - -import com.pathplanner.lib.PathPlannerTrajectory; -import com.pathplanner.lib.PathPlannerTrajectory.PathPlannerState; -import com.pathplanner.lib.commands.PPSwerveControllerCommand; -import edu.wpi.first.wpilibj.DriverStation; -import frc.lib.team3061.RobotConfig; -import frc.robot.subsystems.drivetrain.Drivetrain; -import org.littletonrobotics.junction.Logger; - -/** - * This command, when executed, instructs the drivetrain subsystem to follow the specified - * trajectory, presumably during the autonomous period. The superclass' execute method invokes the - * drivetrain subsystem's setSwerveModuleStates method to follow the trajectory. - * - *

Requires: the Drivetrain subsystem (handled by superclass) - * - *

Finished When: the time of the specified path has elapsed (handled by superclass) - * - *

At End: stops the drivetrain - */ -public class FollowPath extends PPSwerveControllerCommand { - private Drivetrain drivetrain; - private PathPlannerTrajectory trajectory; - private boolean initialPath; - private boolean useAllianceColor; - - /** - * Constructs a new FollowPath object. - * - * @param trajectory the specified trajectory created by PathPlanner - * @param subsystem the drivetrain subsystem required by this command - * @param initialPath true, if this trajectory is the first in a sequence of trajectories or the - * only trajectory, in which case the gyro and odometry will be initialized to match the start - * of trajectory; false, if this trajectory is a subsequent trajectory in which case the gyro - * and odometry will not be re-initialized in order to ensure a smooth transition between - * trajectories - * @param useAllianceColor if true, the path states will be automatically transformed based on - * alliance color. In order for this to work properly, you MUST create your path on the blue - * side of the field. - */ - public FollowPath( - PathPlannerTrajectory trajectory, - Drivetrain subsystem, - boolean initialPath, - boolean useAllianceColor) { - super( - trajectory, - subsystem::getPose, - RobotConfig.getInstance().getSwerveDriveKinematics(), - subsystem.getAutoXController(), - subsystem.getAutoYController(), - subsystem.getAutoThetaController(), - subsystem::setSwerveModuleStates, - useAllianceColor, - subsystem); - - this.drivetrain = subsystem; - this.trajectory = trajectory; - this.initialPath = initialPath; - this.useAllianceColor = useAllianceColor; - } - - /** - * This method is invoked once when this command is scheduled. If the trajectory is the first in a - * sequence of trajectories or the only trajectory, initialize the gyro and odometry to match the - * start of trajectory. PathPlanner sets the origin of the field to the lower left corner (i.e., - * the corner of the field to the driver's right). Zero degrees is away from the driver and - * increases in the CCW direction. It is critical that this initialization occurs in this method - * and not the constructor as this object is constructed well before the command is scheduled. - */ - @Override - public void initialize() { - Logger.getInstance().recordOutput("ActiveCommands/FollowPath", true); - - super.initialize(); - - // reset odometry to the starting pose of the trajectory - if (initialPath) { - PathPlannerState initialState = this.trajectory.getInitialState(); - if (this.useAllianceColor) { - initialState = - PathPlannerTrajectory.transformStateForAlliance( - initialState, DriverStation.getAlliance()); - } - this.drivetrain.resetOdometry(initialState); - } - - // reset the theta controller such that old accumulated ID values aren't used with the new path - // this doesn't matter if only the P value is non-zero, which is the current behavior - this.drivetrain.getAutoXController().reset(); - this.drivetrain.getAutoYController().reset(); - this.drivetrain.getAutoThetaController().reset(); - } - - /** - * This method will be invoked when this command finishes or is interrupted. It stops the motion - * of the drivetrain. - * - * @param interrupted true if the command was interrupted by another command being scheduled - */ - @Override - public void end(boolean interrupted) { - this.drivetrain.stop(); - super.end(interrupted); - Logger.getInstance().recordOutput("ActiveCommands/FollowPath", false); - } -} diff --git a/src/main/java/frc/robot/commands/MoveToPose.java b/src/main/java/frc/robot/commands/MoveToPose.java deleted file mode 100644 index abf6533e..00000000 --- a/src/main/java/frc/robot/commands/MoveToPose.java +++ /dev/null @@ -1,217 +0,0 @@ -package frc.robot.commands; - -import com.pathplanner.lib.PathConstraints; -import com.pathplanner.lib.PathPlannerTrajectory; -import com.pathplanner.lib.commands.PPSwerveControllerCommand; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.lib.team3061.RobotConfig; -import frc.lib.team6328.util.Alert; -import frc.lib.team6328.util.Alert.AlertType; -import frc.lib.team6328.util.TunableNumber; -import frc.robot.Field2d; -import frc.robot.subsystems.drivetrain.Drivetrain; -import java.util.function.Supplier; -import org.littletonrobotics.junction.Logger; - -/** - * This command, when executed, generates a trajectory from the current pose (projected into the - * future to account for the average latency introduced by generating the trajectory) of the robot - * to a specified pose on the field and instructs the drivetrain subsystem to follow the specified - * trajectory using a PPSwerveControllerCommand object. The specified pose will be translated based - * on the alliance color. The initial speed will match the current speed and the final speed will - * match the speed returned by the RobotConfig's getStallAgainstElementVelocity method. The - * PPSwerveControllerCommand contained by this command will never be scheduled; this command invoke - * each of the command methods at the appropriate time. - * - *

For following a predetermined trajectory, refer to the FollowPath Command class. For driving - * in a straight line to a pose, refer to the DriveToPose Command class. - * - *

Requires: the Drivetrain subsystem - * - *

Finished When: trajectory is null or time of specified trajectory has elapsed (from - * PPSwerveControllerCommand object) - * - *

At End: stops the drivetrain, sets trajectory to null - */ -public class MoveToPose extends CommandBase { - private Supplier endPoseSupplier; - private Drivetrain drivetrain; - private double minTrajectoryTraversalTime; - private PathPlannerTrajectory trajectory; - private PPSwerveControllerCommand ppSwerveControllerCommand; - private Alert noTrajectoryAlert = - new Alert("No trajectory between start and end pose", AlertType.WARNING); - private final TunableNumber timeOffset = new TunableNumber("MoveToPose/TIME_OFFSET", 0.1); - - /** - * Constructs a new MoveToPose command object. A pose supplier is specified instead of a pose - * since the target pose may not be known when this command is created. - * - * @param subsystem the drivetrain subsystem required by this command - * @param endPoseSupplier a supplier of the end pose of the trajectory - */ - public MoveToPose(Drivetrain subsystem, Supplier endPoseSupplier) { - this(subsystem, endPoseSupplier, 0); - } - - /** - * Constructs a new MoveToPose command object. A pose supplier is specified instead of a pose - * since the target pose may not be known when this command is created. The generated trajectory - * will be constrained such that the time to traverse the trajectory is at least the specified - * minimum time. This is useful for coordinating this command with the motion of a mechanism. - * - * @param subsystem - * @param endPoseSupplier - * @param minTrajectoryTraversalTime - */ - public MoveToPose( - Drivetrain subsystem, Supplier endPoseSupplier, double minTrajectoryTraversalTime) { - this.endPoseSupplier = endPoseSupplier; - this.drivetrain = subsystem; - this.minTrajectoryTraversalTime = minTrajectoryTraversalTime; - - addRequirements(subsystem); - } - - @Override - public void initialize() { - Logger.getInstance().recordOutput("ActiveCommands/MoveToPose", true); - - // reset the theta controller such that old accumulated ID values aren't used with the new - // trajectory - // this doesn't matter if only the P value is non-zero, which is the current behavior - this.drivetrain.getAutoXController().reset(); - this.drivetrain.getAutoYController().reset(); - this.drivetrain.getAutoThetaController().reset(); - - double beforeCalc = Logger.getInstance().getRealTimestamp(); - - Pose2d endPose = endPose(); - - // calculate the starting pose by projecting the current pose forward in time (assuming the - // robot is not rotating) - Pose2d startingPose = - new Pose2d( - this.drivetrain.getPose().getX() + this.drivetrain.getVelocityX() * timeOffset.get(), - this.drivetrain.getPose().getY() + this.drivetrain.getVelocityY() * timeOffset.get(), - this.drivetrain.getPose().getRotation()); - - // set the maximum velocity to accommodate the minimum trajectory traversal time - double distance = endPose.minus(this.drivetrain.getPose()).getTranslation().getNorm(); - double maxVelocity = RobotConfig.getInstance().getAutoMaxSpeed(); - if (minTrajectoryTraversalTime != 0) { - maxVelocity = distance / minTrajectoryTraversalTime; - } - maxVelocity = Math.min(maxVelocity, RobotConfig.getInstance().getAutoMaxSpeed()); - - // the Field2d singleton generates the trajectory from the poses and constraints - this.trajectory = - Field2d.getInstance() - .makePath( - startingPose, - endPose, - new PathConstraints( - maxVelocity, RobotConfig.getInstance().getAutoMaxAcceleration()), - this.drivetrain); - - noTrajectoryAlert.set(this.trajectory == null); - - if (this.trajectory != null) { - // create the wrapped PPSwerveControllerCommand object and invoke its initialize method since - // it will never be scheduled - this.ppSwerveControllerCommand = - new PPSwerveControllerCommand( - this.trajectory, - this.drivetrain::getPose, - RobotConfig.getInstance().getSwerveDriveKinematics(), - this.drivetrain.getAutoXController(), - this.drivetrain.getAutoYController(), - this.drivetrain.getAutoThetaController(), - this.drivetrain::setSwerveModuleStates); - this.ppSwerveControllerCommand.initialize(); - - double afterCalc = Logger.getInstance().getRealTimestamp(); - Logger.getInstance().recordOutput("Odometry/trajectoryCalcTime", afterCalc - beforeCalc); - - Logger.getInstance().recordOutput("Odometry/trajectory", trajectory); - } - } - - /** - * This method is invoked periodically while this command is scheduled. It executes the contained - * PPSwerveControllerCommand object. - */ - @Override - public void execute() { - if (this.ppSwerveControllerCommand != null) { - this.ppSwerveControllerCommand.execute(); - } - } - - /** - * This method returns true if the command has finished. It is invoked periodically while this - * command is scheduled (after execute is invoked). This command is considered finished if the - * move-to-pose feature is disabled on the drivetrain subsystem, if a trajectory failed to be - * generated, or if the contained PPSwerveControllerCommand command is finished. - * - * @return true if the command has finished - */ - @Override - public boolean isFinished() { - return !drivetrain.isMoveToPoseEnabled() - || this.trajectory == null - || ppSwerveControllerCommand.isFinished(); - } - - /** - * This method will be invoked when this command finishes or is interrupted. It stops the motion - * of the drivetrain if a trajectory was successfully created. - * - * @param interrupted true if the command was interrupted by another command being scheduled - */ - @Override - public void end(boolean interrupted) { - if (this.ppSwerveControllerCommand != null) { - this.ppSwerveControllerCommand.end(interrupted); - } - - if (this.trajectory != null) { - this.drivetrain.stop(); - } - this.trajectory = null; - this.ppSwerveControllerCommand = null; - - Logger.getInstance().recordOutput("ActiveCommands/MoveToPose", false); - } - - /** - * Returns the total time in seconds of the trajectory. - * - * @return the total time in seconds of the trajectory - */ - public double getTotalTimeSeconds() { - if (this.trajectory != null) { - return this.trajectory.getTotalTimeSeconds(); - } else { - return 0; - } - } - - /** - * Calculates the end pose based on the pose from the pose supplier. The end pose is mapped based - * on the alliance color. - * - *

This method can be overridden to translate the end pose for a specific scenario (e.g., - * account for the robot width). - */ - protected Pose2d endPose() { - Pose2d endPose = endPoseSupplier.get(); - - Pose2d translatedEndPose = Field2d.getInstance().mapPoseToCurrentAlliance(endPose); - - Logger.getInstance().recordOutput("MoveToPose/endPose", translatedEndPose); - - return translatedEndPose; - } -} diff --git a/src/main/java/frc/robot/commands/RotateToAngle.java b/src/main/java/frc/robot/commands/RotateToAngle.java index 9925e7e3..30cd638a 100644 --- a/src/main/java/frc/robot/commands/RotateToAngle.java +++ b/src/main/java/frc/robot/commands/RotateToAngle.java @@ -6,10 +6,10 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.Command; import frc.lib.team3061.RobotConfig; +import frc.lib.team3061.drivetrain.Drivetrain; import frc.lib.team6328.util.TunableNumber; -import frc.robot.subsystems.drivetrain.Drivetrain; import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.Logger; @@ -24,7 +24,7 @@ * *

At End: nothing (the drivetrain is left in whatever state it was in when the command finished) */ -public class RotateToAngle extends CommandBase { +public class RotateToAngle extends Command { private final Drivetrain drivetrain; private final DoubleSupplier targetAngleSupplier; private final DoubleSupplier translationXSupplier; @@ -94,7 +94,7 @@ public RotateToAngle( */ @Override public void initialize() { - Logger.getInstance().recordOutput("ActiveCommands/RotateToAngle", true); + Logger.recordOutput("ActiveCommands/RotateToAngle", true); Pose2d currentPose = drivetrain.getPose(); thetaController.reset(currentPose.getRotation().getRadians()); @@ -105,7 +105,7 @@ public void initialize() { Units.degreesToRadians(this.targetAngleSupplier.getAsDouble()) - Math.PI, Units.degreesToRadians(this.targetAngleSupplier.getAsDouble()) + Math.PI); - Logger.getInstance().recordOutput("RotateToAngle/AngleDeg", targetAngleSupplier.getAsDouble()); + Logger.recordOutput("RotateToAngle/AngleDeg", targetAngleSupplier.getAsDouble()); } /** @@ -144,7 +144,7 @@ public void execute() { double xVelocity = xPercentage * RobotConfig.getInstance().getRobotMaxVelocity(); double yVelocity = yPercentage * RobotConfig.getInstance().getRobotMaxVelocity(); - drivetrain.drive(xVelocity, yVelocity, thetaVelocity, true, false); + drivetrain.drive(xVelocity, yVelocity, thetaVelocity, true, drivetrain.getFieldRelative()); } /** @@ -167,6 +167,6 @@ public boolean isFinished() { */ @Override public void end(boolean interrupted) { - Logger.getInstance().recordOutput("ActiveCommands/RotateToAngle", false); + Logger.recordOutput("ActiveCommands/RotateToAngle", false); } } diff --git a/src/main/java/frc/robot/commands/TeleopSwerve.java b/src/main/java/frc/robot/commands/TeleopSwerve.java index f5941d7e..a361f80a 100644 --- a/src/main/java/frc/robot/commands/TeleopSwerve.java +++ b/src/main/java/frc/robot/commands/TeleopSwerve.java @@ -1,10 +1,10 @@ package frc.robot.commands; -import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.Command; import frc.lib.team3061.RobotConfig; +import frc.lib.team3061.drivetrain.Drivetrain; import frc.lib.team6328.util.TunableNumber; import frc.robot.Constants; -import frc.robot.subsystems.drivetrain.Drivetrain; import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.Logger; @@ -21,7 +21,7 @@ * *

At End: stops the drivetrain */ -public class TeleopSwerve extends CommandBase { +public class TeleopSwerve extends Command { private final Drivetrain drivetrain; private final DoubleSupplier translationXSupplier; @@ -73,7 +73,7 @@ public TeleopSwerve( @Override public void initialize() { - Logger.getInstance().recordOutput("ActiveCommands/TeleopSwerve", true); + Logger.recordOutput("ActiveCommands/TeleopSwerve", true); } /** @@ -94,9 +94,9 @@ public void execute() { double yVelocity = yPercentage * maxVelocityMetersPerSecond; double rotationalVelocity = rotationPercentage * maxAngularVelocityRadiansPerSecond; - Logger.getInstance().recordOutput("TeleopSwerve/xVelocity", xVelocity); - Logger.getInstance().recordOutput("TeleopSwerve/yVelocity", yVelocity); - Logger.getInstance().recordOutput("TeleopSwerve/rotationalVelocity", rotationalVelocity); + Logger.recordOutput("TeleopSwerve/xVelocity", xVelocity); + Logger.recordOutput("TeleopSwerve/yVelocity", yVelocity); + Logger.recordOutput("TeleopSwerve/rotationalVelocity", rotationalVelocity); // if the robot is not in turbo mode, limit the acceleration if (!drivetrain.getTurbo()) { @@ -130,7 +130,7 @@ public void execute() { lastYVelocity = yVelocity; lastAngularVelocity = rotationalVelocity; - drivetrain.drive(xVelocity, yVelocity, rotationalVelocity, true, false); + drivetrain.drive(xVelocity, yVelocity, rotationalVelocity, true, drivetrain.getFieldRelative()); } /** @@ -141,7 +141,7 @@ public void execute() { */ @Override public void end(boolean interrupted) { - Logger.getInstance().recordOutput("ActiveCommands/TeleopSwerve", false); + Logger.recordOutput("ActiveCommands/TeleopSwerve", false); } /** diff --git a/src/main/java/frc/robot/configs/DefaultRobotConfig.java b/src/main/java/frc/robot/configs/DefaultRobotConfig.java index b15f9939..52806b8d 100644 --- a/src/main/java/frc/robot/configs/DefaultRobotConfig.java +++ b/src/main/java/frc/robot/configs/DefaultRobotConfig.java @@ -4,7 +4,7 @@ import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation3d; import frc.lib.team3061.RobotConfig; -import frc.lib.team3061.swerve.SwerveModuleConstants.SwerveType; +import frc.lib.team3061.drivetrain.swerve.SwerveConstants.SwerveType; /* * Refer to the README for how to represent your robot's configuration. For more information on diff --git a/src/main/java/frc/robot/configs/MK4IRobotConfig.java b/src/main/java/frc/robot/configs/MK4IRobotConfig.java index 86c82875..f998bbac 100644 --- a/src/main/java/frc/robot/configs/MK4IRobotConfig.java +++ b/src/main/java/frc/robot/configs/MK4IRobotConfig.java @@ -4,7 +4,7 @@ import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation3d; import frc.lib.team3061.RobotConfig; -import frc.lib.team3061.swerve.SwerveModuleConstants.SwerveType; +import frc.lib.team3061.drivetrain.swerve.SwerveConstants.SwerveType; /* * Refer to the README for how to represent your robot's configuration. For more information on diff --git a/src/main/java/frc/robot/configs/NovaCTRERobotConfig.java b/src/main/java/frc/robot/configs/NovaCTRERobotConfig.java new file mode 100644 index 00000000..323f0510 --- /dev/null +++ b/src/main/java/frc/robot/configs/NovaCTRERobotConfig.java @@ -0,0 +1,393 @@ +package frc.robot.configs; + +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; +import frc.lib.team3061.RobotConfig; +import frc.lib.team3061.drivetrain.swerve.SwerveConstants.SwerveType; + +/* + * Refer to the README for how to represent your robot's configuration. For more information on + * these methods, refer to the documentation in the RobotConfig class. + */ +public class NovaCTRERobotConfig extends RobotConfig { + + private static final int FRONT_LEFT_MODULE_DRIVE_MOTOR = 13; + private static final int FRONT_LEFT_MODULE_STEER_MOTOR = 12; + private static final int FRONT_LEFT_MODULE_STEER_ENCODER = 14; + private static final double FRONT_LEFT_MODULE_STEER_OFFSET_ROT = -0.604492; + + private static final int FRONT_RIGHT_MODULE_DRIVE_MOTOR = 16; + private static final int FRONT_RIGHT_MODULE_STEER_MOTOR = 15; + private static final int FRONT_RIGHT_MODULE_STEER_ENCODER = 17; + private static final double FRONT_RIGHT_MODULE_STEER_OFFSET_ROT = -0.277832; + + private static final int BACK_LEFT_MODULE_DRIVE_MOTOR = 7; + private static final int BACK_LEFT_MODULE_STEER_MOTOR = 6; + private static final int BACK_LEFT_MODULE_STEER_ENCODER = 8; + private static final double BACK_LEFT_MODULE_STEER_OFFSET_ROT = -0.249512; + + private static final int BACK_RIGHT_MODULE_DRIVE_MOTOR = 10; + private static final int BACK_RIGHT_MODULE_STEER_MOTOR = 9; + private static final int BACK_RIGHT_MODULE_STEER_ENCODER = 11; + private static final double BACK_RIGHT_MODULE_STEER_OFFSET_ROT = -0.024414; + + private static final int GYRO_ID = 3; + + private static final double TRACKWIDTH_METERS = 0.523875; // 20.625 + private static final double WHEELBASE_METERS = 0.52705; // 20.75 + private static final double ROBOT_WIDTH_WITH_BUMPERS = 0.8382; // meters //33 in + private static final double ROBOT_LENGTH_WITH_BUMPERS = 0.8382; // meters // 33 in + + /* Angle Motor PID Values */ + private static final double ANGLE_KP = 100.0; + private static final double ANGLE_KI = 0.0; + private static final double ANGLE_KD = 0.05; + + private static final double ANGLE_KS = 0.1891233333; + private static final double ANGLE_KV = 0.4399866667; + private static final double ANGLE_KA = 0.001663333333; + + /* Drive Motor PID Values */ + private static final double DRIVE_KP = 0.2; + private static final double DRIVE_KI = 0.0; + private static final double DRIVE_KD = 0.0; + + private static final double DRIVE_KS = 0.4004375; + private static final double DRIVE_KV = 2.7637325; + private static final double DRIVE_KA = 0.0139575; + + private static final SwerveType SWERVE_TYPE = SwerveType.MK4I; + + private static final double MAX_VELOCITY_METERS_PER_SECOND = 4.78; + private static final double MAX_COAST_VELOCITY_METERS_PER_SECOND = 0.05; + private static final double SLOW_MODE_MULTIPLIER = 0.75; + + private static final double MAX_DRIVE_ACCELERATION_METERS_PER_SECOND_SQUARED = 10.0; + private static final double MAX_TURN_ACCELERATION_RADIANS_PER_SECOND_SQUARED = 4.0 * Math.PI; + + private static final String CAN_BUS_NAME = "canbus1"; + + private static final String CAMERA_NAME_0 = "OV9281"; + + private static final String CAMERA_NAME_1 = "OV9281R"; + + private static final Transform3d ROBOT_TO_CAMERA_0 = + new Transform3d( + new Translation3d( + Units.inchesToMeters(-10.406), + Units.inchesToMeters(6.603), + Units.inchesToMeters(49.240)), + new Rotation3d(0, Units.degreesToRadians(25), Units.degreesToRadians(30))); + + private static final Transform3d ROBOT_TO_CAMERA_1 = + new Transform3d( + new Translation3d( + Units.inchesToMeters(-10.406), + Units.inchesToMeters(-6.603), + Units.inchesToMeters(49.240)), + new Rotation3d(0, Units.degreesToRadians(25), Units.degreesToRadians(-30))); + + private static final double AUTO_MAX_SPEED_METERS_PER_SECOND = 2.0; + private static final double AUTO_MAX_ACCELERATION_METERS_PER_SECOND_SQUARED = 2.0; + private static final double AUTO_DRIVE_P_CONTROLLER = 5.0; + private static final double AUTO_DRIVE_I_CONTROLLER = 0.0; + private static final double AUTO_DRIVE_D_CONTROLLER = 0.0; + private static final double AUTO_TURN_P_CONTROLLER = 5.0; + private static final double AUTO_TURN_I_CONTROLLER = 0.0; + private static final double AUTO_TURN_D_CONTROLLER = 0.0; + + // Drive to Pose constants + private static final double DRIVE_TO_POSE_DRIVE_KP = 2.5; + private static final double DRIVE_TO_POSE_DRIVE_KD = 0.0; + private static final double DRIVE_TO_POSE_THETA_KP = 18.0; + private static final double DRIVE_TO_POSE_THETA_KI = 10.0; + private static final double DRIVE_TO_POSE_THETA_KD = 0.0; + private static final double DRIVE_TO_POSE_DRIVE_TOLERANCE_METERS = 0.08; + private static final double DRIVE_TO_POSE_THETA_TOLERANCE_RADIANS = 0.008; + + private static final double SQUARING_SPEED_METERS_PER_SECOND = 1.0; + + @Override + public boolean getPhoenix6Licensed() { + return true; + } + + @Override + public double getSwerveAngleKP() { + return ANGLE_KP; + } + + @Override + public double getSwerveAngleKI() { + return ANGLE_KI; + } + + @Override + public double getSwerveAngleKD() { + return ANGLE_KD; + } + + @Override + public double getSwerveAngleKS() { + return ANGLE_KS; + } + + @Override + public double getSwerveAngleKV() { + return ANGLE_KV; + } + + @Override + public double getSwerveAngleKA() { + return ANGLE_KA; + } + + @Override + public double getSwerveDriveKP() { + return DRIVE_KP; + } + + @Override + public double getSwerveDriveKI() { + return DRIVE_KI; + } + + @Override + public double getSwerveDriveKD() { + return DRIVE_KD; + } + + @Override + public double getDriveKS() { + return DRIVE_KS; + } + + @Override + public double getDriveKV() { + return DRIVE_KV; + } + + @Override + public double getDriveKA() { + return DRIVE_KA; + } + + @Override + public SwerveType getSwerveType() { + return SWERVE_TYPE; + } + + @Override + public int[] getSwerveDriveMotorCANIDs() { + return new int[] { + FRONT_LEFT_MODULE_DRIVE_MOTOR, + FRONT_RIGHT_MODULE_DRIVE_MOTOR, + BACK_LEFT_MODULE_DRIVE_MOTOR, + BACK_RIGHT_MODULE_DRIVE_MOTOR + }; + } + + @Override + public int[] getSwerveSteerMotorCANIDs() { + return new int[] { + FRONT_LEFT_MODULE_STEER_MOTOR, + FRONT_RIGHT_MODULE_STEER_MOTOR, + BACK_LEFT_MODULE_STEER_MOTOR, + BACK_RIGHT_MODULE_STEER_MOTOR + }; + } + + @Override + public int[] getSwerveSteerEncoderCANIDs() { + return new int[] { + FRONT_LEFT_MODULE_STEER_ENCODER, + FRONT_RIGHT_MODULE_STEER_ENCODER, + BACK_LEFT_MODULE_STEER_ENCODER, + BACK_RIGHT_MODULE_STEER_ENCODER + }; + } + + @Override + public double[] getSwerveSteerOffsets() { + return new double[] { + FRONT_LEFT_MODULE_STEER_OFFSET_ROT, + FRONT_RIGHT_MODULE_STEER_OFFSET_ROT, + BACK_LEFT_MODULE_STEER_OFFSET_ROT, + BACK_RIGHT_MODULE_STEER_OFFSET_ROT + }; + } + + @Override + public int getGyroCANID() { + return GYRO_ID; + } + + @Override + public double getTrackwidth() { + return TRACKWIDTH_METERS; + } + + @Override + public double getWheelbase() { + return WHEELBASE_METERS; + } + + @Override + public double getRobotWidthWithBumpers() { + return ROBOT_WIDTH_WITH_BUMPERS; + } + + @Override + public double getRobotLengthWithBumpers() { + return ROBOT_LENGTH_WITH_BUMPERS; + } + + @Override + public Transform3d[] getRobotToCameraTransforms() { + return new Transform3d[] {ROBOT_TO_CAMERA_0, ROBOT_TO_CAMERA_1}; + } + + @Override + public double getRobotMaxVelocity() { + return MAX_VELOCITY_METERS_PER_SECOND; + } + + @Override + public double getRobotMaxDriveAcceleration() { + return MAX_DRIVE_ACCELERATION_METERS_PER_SECOND_SQUARED; + } + + @Override + public double getRobotMaxTurnAcceleration() { + return MAX_TURN_ACCELERATION_RADIANS_PER_SECOND_SQUARED; + } + + @Override + public double getRobotSlowModeMultiplier() { + return SLOW_MODE_MULTIPLIER; + } + + @Override + public double getRobotMaxCoastVelocity() { + return MAX_COAST_VELOCITY_METERS_PER_SECOND; + } + + @Override + public double getAutoMaxSpeed() { + return AUTO_MAX_SPEED_METERS_PER_SECOND; + } + + @Override + public double getAutoMaxAcceleration() { + return AUTO_MAX_ACCELERATION_METERS_PER_SECOND_SQUARED; + } + + @Override + public double getAutoDriveKP() { + return AUTO_DRIVE_P_CONTROLLER; + } + + @Override + public double getAutoDriveKI() { + return AUTO_DRIVE_I_CONTROLLER; + } + + @Override + public double getAutoDriveKD() { + return AUTO_DRIVE_D_CONTROLLER; + } + + @Override + public double getAutoTurnKP() { + return AUTO_TURN_P_CONTROLLER; + } + + @Override + public double getAutoTurnKI() { + return AUTO_TURN_I_CONTROLLER; + } + + @Override + public double getAutoTurnKD() { + return AUTO_TURN_D_CONTROLLER; + } + + @Override + public String getCANBusName() { + return CAN_BUS_NAME; + } + + @Override + public String[] getCameraNames() { + return new String[] {CAMERA_NAME_0, CAMERA_NAME_1}; + } + + @Override + public double getDriveToPoseDriveKP() { + return DRIVE_TO_POSE_DRIVE_KP; + } + + @Override + public double getDriveToPoseDriveKD() { + return DRIVE_TO_POSE_DRIVE_KD; + } + + @Override + public double getDriveToPoseThetaKI() { + return DRIVE_TO_POSE_THETA_KI; + } + + @Override + public double getDriveToPoseThetaKP() { + return DRIVE_TO_POSE_THETA_KP; + } + + @Override + public double getDriveToPoseThetaKD() { + return DRIVE_TO_POSE_THETA_KD; + } + + @Override + public double getDriveToPoseDriveMaxVelocity() { + return 0.5; + } + + @Override + public double getDriveToPoseDriveMaxAcceleration() { + return getAutoMaxAcceleration(); + } + + @Override + public double getDriveToPoseTurnMaxVelocity() { + return getDriveToPoseDriveMaxVelocity() + / Math.hypot(getTrackwidth() / 2.0, getWheelbase() / 2.0); + } + + @Override + public double getDriveToPoseTurnMaxAcceleration() { + return getDriveToPoseDriveMaxAcceleration() + / Math.hypot(getTrackwidth() / 2.0, getWheelbase() / 2.0); + } + + @Override + public double getDriveToPoseDriveTolerance() { + return DRIVE_TO_POSE_DRIVE_TOLERANCE_METERS; + } + + @Override + public double getDriveToPoseThetaTolerance() { + return DRIVE_TO_POSE_THETA_TOLERANCE_RADIANS; + } + + @Override + public int getPneumaticsHubCANID() { + return 0; + } + + @Override + public double getMoveToPathFinalVelocity() { + return SQUARING_SPEED_METERS_PER_SECOND; + } +} diff --git a/src/main/java/frc/robot/configs/NovaRobotConfig.java b/src/main/java/frc/robot/configs/NovaRobotConfig.java index 305a52ab..4ab0bb2e 100644 --- a/src/main/java/frc/robot/configs/NovaRobotConfig.java +++ b/src/main/java/frc/robot/configs/NovaRobotConfig.java @@ -5,7 +5,7 @@ import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; import frc.lib.team3061.RobotConfig; -import frc.lib.team3061.swerve.SwerveModuleConstants.SwerveType; +import frc.lib.team3061.drivetrain.swerve.SwerveConstants.SwerveType; /* * Refer to the README for how to represent your robot's configuration. For more information on @@ -16,22 +16,22 @@ public class NovaRobotConfig extends RobotConfig { private static final int FRONT_LEFT_MODULE_DRIVE_MOTOR = 13; private static final int FRONT_LEFT_MODULE_STEER_MOTOR = 12; private static final int FRONT_LEFT_MODULE_STEER_ENCODER = 14; - private static final double FRONT_LEFT_MODULE_STEER_OFFSET_ROT = -0.104492; + private static final double FRONT_LEFT_MODULE_STEER_OFFSET_ROT = -0.104492; // -0.604492; private static final int FRONT_RIGHT_MODULE_DRIVE_MOTOR = 16; private static final int FRONT_RIGHT_MODULE_STEER_MOTOR = 15; private static final int FRONT_RIGHT_MODULE_STEER_ENCODER = 17; - private static final double FRONT_RIGHT_MODULE_STEER_OFFSET_ROT = -0.277832; + private static final double FRONT_RIGHT_MODULE_STEER_OFFSET_ROT = -0.277832; // -0.277832; private static final int BACK_LEFT_MODULE_DRIVE_MOTOR = 7; private static final int BACK_LEFT_MODULE_STEER_MOTOR = 6; private static final int BACK_LEFT_MODULE_STEER_ENCODER = 8; - private static final double BACK_LEFT_MODULE_STEER_OFFSET_ROT = -0.749512; + private static final double BACK_LEFT_MODULE_STEER_OFFSET_ROT = -0.749512; // -0.249512; private static final int BACK_RIGHT_MODULE_DRIVE_MOTOR = 10; private static final int BACK_RIGHT_MODULE_STEER_MOTOR = 9; private static final int BACK_RIGHT_MODULE_STEER_ENCODER = 11; - private static final double BACK_RIGHT_MODULE_STEER_OFFSET_ROT = -0.024414; + private static final double BACK_RIGHT_MODULE_STEER_OFFSET_ROT = -0.024414; // -0.024414; private static final int GYRO_ID = 3; @@ -89,12 +89,13 @@ public class NovaRobotConfig extends RobotConfig { Units.inchesToMeters(49.240)), new Rotation3d(0, Units.degreesToRadians(25), Units.degreesToRadians(-30))); - private static final double AUTO_MAX_SPEED_METERS_PER_SECOND = 2.0; - private static final double AUTO_MAX_ACCELERATION_METERS_PER_SECOND_SQUARED = 2.0; - private static final double AUTO_DRIVE_P_CONTROLLER = 8.0; + private static final double AUTO_MAX_SPEED_METERS_PER_SECOND = MAX_VELOCITY_METERS_PER_SECOND; + private static final double AUTO_MAX_ACCELERATION_METERS_PER_SECOND_SQUARED = + MAX_DRIVE_ACCELERATION_METERS_PER_SECOND_SQUARED; + private static final double AUTO_DRIVE_P_CONTROLLER = 5.0; private static final double AUTO_DRIVE_I_CONTROLLER = 0.0; private static final double AUTO_DRIVE_D_CONTROLLER = 0.0; - private static final double AUTO_TURN_P_CONTROLLER = 0.2; + private static final double AUTO_TURN_P_CONTROLLER = 5.0; private static final double AUTO_TURN_I_CONTROLLER = 0.0; private static final double AUTO_TURN_D_CONTROLLER = 0.0; diff --git a/src/main/java/frc/robot/configs/SierraRobotConfig.java b/src/main/java/frc/robot/configs/SierraRobotConfig.java index 4d7edf32..8d044010 100644 --- a/src/main/java/frc/robot/configs/SierraRobotConfig.java +++ b/src/main/java/frc/robot/configs/SierraRobotConfig.java @@ -4,7 +4,7 @@ import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation3d; import frc.lib.team3061.RobotConfig; -import frc.lib.team3061.swerve.SwerveModuleConstants.SwerveType; +import frc.lib.team3061.drivetrain.swerve.SwerveConstants.SwerveType; /* * Refer to the README for how to represent your robot's configuration. For more information on diff --git a/src/main/java/frc/robot/subsystems/subsystem/Subsystem.java b/src/main/java/frc/robot/subsystems/subsystem/Subsystem.java index 4110f868..6bdcc64b 100644 --- a/src/main/java/frc/robot/subsystems/subsystem/Subsystem.java +++ b/src/main/java/frc/robot/subsystems/subsystem/Subsystem.java @@ -4,7 +4,7 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; -import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.lib.team3015.subsystem.FaultReporter; @@ -53,7 +53,7 @@ public Subsystem(SubsystemIO io) { @Override public void periodic() { io.updateInputs(inputs); - Logger.getInstance().processInputs("Subsystem", inputs); + Logger.processInputs("Subsystem", inputs); // when testing, set the motor power, current, or position based on the Tunables (if non-zero) if (TESTING) { @@ -98,7 +98,7 @@ public void setMotorPosition(double position) { io.setMotorPosition(position, POSITION_FEEDFORWARD); } - private CommandBase getSystemCheckCommand() { + private Command getSystemCheckCommand() { return Commands.sequence( Commands.run(() -> io.setMotorPower(0.3)).withTimeout(1.0), Commands.runOnce( diff --git a/src/main/java/frc/robot/subsystems/subsystem/SubsystemIOTalonFX.java b/src/main/java/frc/robot/subsystems/subsystem/SubsystemIOTalonFX.java index c3621382..8a0ea97b 100644 --- a/src/main/java/frc/robot/subsystems/subsystem/SubsystemIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/subsystem/SubsystemIOTalonFX.java @@ -13,7 +13,7 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import frc.lib.team3015.subsystem.FaultReporter; import frc.lib.team3061.RobotConfig; -import frc.lib.team3061.swerve.Conversions; +import frc.lib.team3061.drivetrain.swerve.Conversions; import frc.lib.team6328.util.Alert; import frc.lib.team6328.util.Alert.AlertType; import frc.lib.team6328.util.TunableNumber; @@ -143,7 +143,7 @@ private void configMotor(int motorID) { configAlert.setText(status.toString()); } - this.motor.setRotorPosition(0); + this.motor.setPosition(0); this.voltageRequest = new VoltageOut(0.0); this.voltageRequest.EnableFOC = RobotConfig.getInstance().getPhoenix6Licensed(); diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json index fa08705a..114ee222 100644 --- a/vendordeps/AdvantageKit.json +++ b/vendordeps/AdvantageKit.json @@ -1,32 +1,33 @@ { "fileName": "AdvantageKit.json", "name": "AdvantageKit", - "version": "2.2.4", + "version": "3.0.0-beta-6", "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", + "frcYear": "2024", "mavenUrls": [], "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", "javaDependencies": [ { "groupId": "org.littletonrobotics.akit.junction", "artifactId": "wpilib-shim", - "version": "2.2.4" + "version": "3.0.0-beta-6" }, { "groupId": "org.littletonrobotics.akit.junction", "artifactId": "junction-core", - "version": "2.2.4" + "version": "3.0.0-beta-6" }, { "groupId": "org.littletonrobotics.akit.conduit", "artifactId": "conduit-api", - "version": "2.2.4" + "version": "3.0.0-beta-6" } ], "jniDependencies": [ { "groupId": "org.littletonrobotics.akit.conduit", "artifactId": "conduit-wpilibio", - "version": "2.2.4", + "version": "3.0.0-beta-6", "skipInvalidPlatforms": false, "isJar": false, "validPlatforms": [ diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index 8e61586b..eb271997 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,8 +1,9 @@ { "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2023.4.4", + "version": "2024.0.0-beta-6", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", + "frcYear": "2024", "mavenUrls": [ "https://3015rangerrobotics.github.io/pathplannerlib/repo" ], @@ -11,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2023.4.4" + "version": "2024.0.0-beta-6" } ], "jniDependencies": [], @@ -19,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2023.4.4", + "version": "2024.0.0-beta-6", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -28,7 +29,9 @@ "windowsx86-64", "linuxx86-64", "osxuniversal", - "linuxathena" + "linuxathena", + "linuxarm32", + "linuxarm64" ] } ] diff --git a/vendordeps/Phoenix6And5.json b/vendordeps/Phoenix6.json similarity index 65% rename from vendordeps/Phoenix6And5.json rename to vendordeps/Phoenix6.json index 0de310fc..7ea073a5 100644 --- a/vendordeps/Phoenix6And5.json +++ b/vendordeps/Phoenix6.json @@ -1,61 +1,32 @@ { - "fileName": "Phoenix6And5.json", - "name": "CTRE-Phoenix (v6 And v5)", - "version": "23.1.0", - "frcYear": 2023, - "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a", + "fileName": "Phoenix6.json", + "name": "CTRE-Phoenix (v6)", + "version": "24.0.0-beta-5", + "frcYear": 2024, + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ "https://maven.ctr-electronics.com/release/" ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6And5-frc2023-latest.json", - "javaDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "api-java", - "version": "5.31.0" - }, + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-beta-latest.json", + "conflictsWith": [ { - "groupId": "com.ctre.phoenix", - "artifactId": "wpiapi-java", - "version": "5.31.0" - }, + "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a", + "errorMessage": "The combined Phoenix-6-And-5 vendordep is no longer supported. Please remove the vendordep and instead add both the latest Phoenix 6 vendordep and Phoenix 5 vendordep.", + "offlineFileName": "Phoenix6And5.json" + } + ], + "javaDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "23.1.0" + "version": "24.0.0-beta-5" } ], "jniDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "cci", - "version": "5.31.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "cci-sim", - "version": "5.31.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "23.1.0", + "version": "24.0.0-beta-5", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +39,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "23.1.0", + "version": "24.0.0-beta-5", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -81,7 +52,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "23.1.0", + "version": "24.0.0-beta-5", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -94,7 +65,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "23.1.0", + "version": "24.0.0-beta-5", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -107,7 +78,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "23.1.0", + "version": "24.0.0-beta-5", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -120,7 +91,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "23.1.0", + "version": "24.0.0-beta-5", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -133,7 +104,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "23.1.0", + "version": "24.0.0-beta-5", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -146,7 +117,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "23.1.0", + "version": "24.0.0-beta-5", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -159,7 +130,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "23.1.0", + "version": "24.0.0-beta-5", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -172,7 +143,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "23.1.0", + "version": "24.0.0-beta-5", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -185,40 +156,10 @@ ], "cppDependencies": [ { - "groupId": "com.ctre.phoenix", + "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "5.31.0", - "libName": "CTRE_Phoenix_WPI", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "api-cpp", - "version": "5.31.0", - "libName": "CTRE_Phoenix", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "cci", - "version": "5.31.0", - "libName": "CTRE_PhoenixCCI", + "version": "24.0.0-beta-5", + "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, @@ -232,7 +173,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "23.1.0", + "version": "24.0.0-beta-5", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -245,40 +186,10 @@ "simMode": "hwsim" }, { - "groupId": "com.ctre.phoenix.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "5.31.0", - "libName": "CTRE_Phoenix_WPISim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "api-cpp-sim", - "version": "5.31.0", - "libName": "CTRE_PhoenixSim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "cci-sim", - "version": "5.31.0", - "libName": "CTRE_PhoenixCCISim", + "version": "24.0.0-beta-5", + "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, @@ -292,7 +203,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "23.1.0", + "version": "24.0.0-beta-5", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -307,7 +218,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "23.1.0", + "version": "24.0.0-beta-5", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -322,7 +233,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "23.1.0", + "version": "24.0.0-beta-5", "libName": "CTRE_SimTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -337,7 +248,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "23.1.0", + "version": "24.0.0-beta-5", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -352,7 +263,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "23.1.0", + "version": "24.0.0-beta-5", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -367,7 +278,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "23.1.0", + "version": "24.0.0-beta-5", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -382,7 +293,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "23.1.0", + "version": "24.0.0-beta-5", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -397,7 +308,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "23.1.0", + "version": "24.0.0-beta-5", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -412,7 +323,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "23.1.0", + "version": "24.0.0-beta-5", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -423,36 +334,6 @@ "osxuniversal" ], "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6", - "artifactId": "wpiapi-cpp", - "version": "23.1.0", - "libName": "CTRE_Phoenix6_WPI", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "wpiapi-cpp-sim", - "version": "23.1.0", - "libName": "CTRE_Phoenix6_WPISim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" } ] } \ No newline at end of file diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index f2d0b7df..7eb37558 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,24 +1,25 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2023.1.3", + "version": "2024.0.0", + "frcYear": "2024", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ "https://maven.revrobotics.com/" ], - "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2023.json", + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2024.json", "javaDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2023.1.3" + "version": "2024.0.0" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2023.1.3", + "version": "2024.0.0", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -36,7 +37,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2023.1.3", + "version": "2024.0.0", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -54,7 +55,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2023.1.3", + "version": "2024.0.0", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/WPILibNewCommands.json b/vendordeps/WPILibNewCommands.json index da4bc529..67bf3898 100644 --- a/vendordeps/WPILibNewCommands.json +++ b/vendordeps/WPILibNewCommands.json @@ -1,37 +1,38 @@ -{ - "fileName": "WPILibNewCommands.json", - "name": "WPILib-New-Commands", - "version": "1.0.0", - "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", - "mavenUrls": [], - "jsonUrl": "", - "javaDependencies": [ - { - "groupId": "edu.wpi.first.wpilibNewCommands", - "artifactId": "wpilibNewCommands-java", - "version": "wpilib" - } - ], - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "edu.wpi.first.wpilibNewCommands", - "artifactId": "wpilibNewCommands-cpp", - "version": "wpilib", - "libName": "wpilibNewCommands", - "headerClassifier": "headers", - "sourcesClassifier": "sources", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "linuxarm32", - "linuxarm64", - "windowsx86-64", - "windowsx86", - "linuxx86-64", - "osxuniversal" - ] - } - ] -} +{ + "fileName": "WPILibNewCommands.json", + "name": "WPILib-New-Commands", + "version": "1.0.0", + "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", + "frcYear": "2024", + "mavenUrls": [], + "jsonUrl": "", + "javaDependencies": [ + { + "groupId": "edu.wpi.first.wpilibNewCommands", + "artifactId": "wpilibNewCommands-java", + "version": "wpilib" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "edu.wpi.first.wpilibNewCommands", + "artifactId": "wpilibNewCommands-cpp", + "version": "wpilib", + "libName": "wpilibNewCommands", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "windowsx86-64", + "windowsx86", + "linuxx86-64", + "osxuniversal" + ] + } + ] +} diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index dad31057..dfb7f3fe 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,8 +1,9 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2023.4.2", - "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004 ", + "version": "dev-v2024.1.1-beta-3-2-g524b1351", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", + "frcYear": "2024", "mavenUrls": [ "https://maven.photonvision.org/repository/internal", "https://maven.photonvision.org/repository/snapshots" @@ -13,7 +14,7 @@ { "groupId": "org.photonvision", "artifactId": "PhotonLib-cpp", - "version": "v2023.4.2", + "version": "dev-v2024.1.1-beta-3-2-g524b1351", "libName": "Photon", "headerClassifier": "headers", "sharedLibrary": true, @@ -30,12 +31,12 @@ { "groupId": "org.photonvision", "artifactId": "PhotonLib-java", - "version": "v2023.4.2" + "version": "dev-v2024.1.1-beta-3-2-g524b1351" }, { "groupId": "org.photonvision", "artifactId": "PhotonTargeting-java", - "version": "v2023.4.2" + "version": "dev-v2024.1.1-beta-3-2-g524b1351" } ] } \ No newline at end of file