diff --git a/.gitignore b/.gitignore index 7fa9fd5e..07cc290c 100644 --- a/.gitignore +++ b/.gitignore @@ -159,7 +159,7 @@ gradle-app.setting bin/ # IntelliJ -# .idea folder has its own internal .gitignore so trust it for what to ignore. +.idea/* .run # Fleet diff --git a/.idea/compiler.xml b/.idea/compiler.xml index b589d56e..c38434a4 100644 --- a/.idea/compiler.xml +++ b/.idea/compiler.xml @@ -1,6 +1,16 @@ + + + + + + + + + + \ No newline at end of file diff --git a/.idea/modules.xml b/.idea/modules.xml index 19771160..7602cd4f 100644 --- a/.idea/modules.xml +++ b/.idea/modules.xml @@ -2,6 +2,7 @@ + diff --git a/.idea/modules/2024_Robot.main.iml b/.idea/modules/2024_Robot.main.iml index ef3acc9a..0434e9d2 100644 --- a/.idea/modules/2024_Robot.main.iml +++ b/.idea/modules/2024_Robot.main.iml @@ -1,6 +1,9 @@ + + + diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index 3ed4c4bb..f9bfd47f 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -1,6 +1,6 @@ { "enableCppIntellisense": false, "currentLanguage": "java", - "projectYear": "2024", + "projectYear": "2025", "teamNumber": 1310 } \ No newline at end of file diff --git a/WPILib-License.md b/WPILib-License.md index 43b62ec2..e7cd597b 100644 --- a/WPILib-License.md +++ b/WPILib-License.md @@ -1,24 +1,24 @@ -Copyright (c) 2009-2023 FIRST and other WPILib contributors -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - * Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - * Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in the - documentation and/or other materials provided with the distribution. - * Neither the name of FIRST, WPILib, nor the names of other WPILib - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND -ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED -WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR -PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR -ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND -ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS -SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +Copyright (c) 2009-2024 FIRST and other WPILib contributors +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of FIRST, WPILib, nor the names of other WPILib + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR +PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/build.gradle b/build.gradle index dbfd1afd..504d0228 100644 --- a/build.gradle +++ b/build.gradle @@ -1,101 +1,117 @@ -plugins { - id "java" - id "edu.wpi.first.GradleRIO" version "2024.3.2" -} - -java { - sourceCompatibility = JavaVersion.VERSION_17 - targetCompatibility = JavaVersion.VERSION_17 -} - -def ROBOT_MAIN_CLASS = "frc.robot.Main" - -// Define my targets (RoboRIO) and artifacts (deployable files) -// This is added by GradleRIO's backing project DeployUtils. -deploy { - targets { - roborio(getTargetTypeClass('RoboRIO')) { - // Team number is loaded either from the .wpilib/wpilib_preferences.json - // or from command line. If not found an exception will be thrown. - // You can use getTeamOrDefault(team) instead of getTeamNumber if you - // want to store a team number in this file. - team = project.frc.getTeamNumber() - debug = project.frc.getDebugOrDefault(false) - - artifacts { - // First part is artifact name, 2nd is artifact type - // getTargetTypeClass is a shortcut to get the class type using a string - - frcJava(getArtifactTypeClass('FRCJavaArtifact')) { - } - - // Static files artifact - frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { - files = project.fileTree('src/main/deploy') - directory = '/home/lvuser/deploy' - } - } - } - } -} - -def deployArtifact = deploy.targets.roborio.artifacts.frcJava - -// Set to true to use debug for JNI. -wpi.java.debugJni = false - -// Set this to true to enable desktop support. -def includeDesktopSupport = true - -// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. -// Also defines JUnit 5. -dependencies { - implementation wpi.java.deps.wpilib() - implementation wpi.java.vendor.java() - - roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio) - roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio) - - roborioRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.roborio) - roborioRelease wpi.java.vendor.jniRelease(wpi.platforms.roborio) - - nativeDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.desktop) - nativeDebug wpi.java.vendor.jniDebug(wpi.platforms.desktop) - simulationDebug wpi.sim.enableDebug() - - nativeRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.desktop) - nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop) - simulationRelease wpi.sim.enableRelease() - - testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' - testRuntimeOnly 'org.junit.platform:junit-platform-launcher' -} - -test { - useJUnitPlatform() - systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true' -} - -// Simulation configuration (e.g. environment variables). -wpi.sim.addGui().defaultEnabled = true -wpi.sim.addDriverstation() - -// Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar') -// in order to make them all available at runtime. Also adding the manifest so WPILib -// 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 -} - -// Configure jar and deploy tasks -deployArtifact.jarTask = jar -wpi.java.configureExecutableTasks(jar) -wpi.java.configureTestTasks(test) - -// Configure string concat to always inline compile -tasks.withType(JavaCompile) { - options.compilerArgs.add '-XDstringConcat=inline' -} +plugins { + id "java" + id "edu.wpi.first.GradleRIO" version "2025.2.1" +} + +java { + sourceCompatibility = JavaVersion.VERSION_17 + targetCompatibility = JavaVersion.VERSION_17 +} + +def ROBOT_MAIN_CLASS = "frc.robot.Main" + +// Define my targets (RoboRIO) and artifacts (deployable files) +// This is added by GradleRIO's backing project DeployUtils. +deploy { + targets { + roborio(getTargetTypeClass('RoboRIO')) { + // Team number is loaded either from the .wpilib/wpilib_preferences.json + // or from command line. If not found an exception will be thrown. + // You can use getTeamOrDefault(team) instead of getTeamNumber if you + // want to store a team number in this file. + team = project.frc.getTeamNumber() + debug = project.frc.getDebugOrDefault(false) + + artifacts { + // First part is artifact name, 2nd is artifact type + // getTargetTypeClass is a shortcut to get the class type using a string + + frcJava(getArtifactTypeClass('FRCJavaArtifact')) { + // Enable VisualVM connection + jvmArgs.add("-Dcom.sun.management.jmxremote=true") + jvmArgs.add("-Dcom.sun.management.jmxremote.port=1198") + jvmArgs.add("-Dcom.sun.management.jmxremote.local.only=false") + jvmArgs.add("-Dcom.sun.management.jmxremote.ssl=false") + jvmArgs.add("-Dcom.sun.management.jmxremote.authenticate=false") + jvmArgs.add("-Djava.rmi.server.hostname=10.13.10.2") + } + + // Static files artifact + frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { + files = project.fileTree('src/main/deploy') + directory = '/home/lvuser/deploy' + deleteOldFiles = false // Change to true to delete files on roboRIO that no + // longer exist in deploy directory of this project + } + } + } + } +} + +def deployArtifact = deploy.targets.roborio.artifacts.frcJava + +// Set to true to use debug for JNI. +wpi.java.debugJni = false + +// Set this to true to enable desktop support. +def includeDesktopSupport = false + +// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. +// Also defines JUnit 5. +dependencies { + annotationProcessor wpi.java.deps.wpilibAnnotations() + implementation wpi.java.deps.wpilib() + implementation wpi.java.vendor.java() + + roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio) + roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio) + + roborioRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.roborio) + roborioRelease wpi.java.vendor.jniRelease(wpi.platforms.roborio) + + nativeDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.desktop) + nativeDebug wpi.java.vendor.jniDebug(wpi.platforms.desktop) + simulationDebug wpi.sim.enableDebug() + + nativeRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.desktop) + nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop) + simulationRelease wpi.sim.enableRelease() + + implementation 'ca.team1310:swerve:3.2.1' + + testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' + testRuntimeOnly 'org.junit.platform:junit-platform-launcher' +} + +test { + useJUnitPlatform() + systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true' +} + +// Simulation configuration (e.g. environment variables). +wpi.sim.addGui().defaultEnabled = true +wpi.sim.addDriverstation() + +// Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar') +// in order to make them all available at runtime. Also adding the manifest so WPILib +// 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 +} + +// Configure jar and deploy tasks +deployArtifact.jarTask = jar +wpi.java.configureExecutableTasks(jar) +wpi.java.configureTestTasks(test) + +// Configure string concat to always inline compile +tasks.withType(JavaCompile) { + options.compilerArgs.add '-XDstringConcat=inline' +} + +repositories { + mavenLocal() +} diff --git a/gradle/wrapper/gradle-wrapper.jar b/gradle/wrapper/gradle-wrapper.jar index d64cd491..a4b76b95 100644 Binary files a/gradle/wrapper/gradle-wrapper.jar and b/gradle/wrapper/gradle-wrapper.jar differ diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties index 5e82d67b..8e975a5f 100644 --- a/gradle/wrapper/gradle-wrapper.properties +++ b/gradle/wrapper/gradle-wrapper.properties @@ -1,7 +1,7 @@ -distributionBase=GRADLE_USER_HOME -distributionPath=permwrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-8.5-bin.zip -networkTimeout=10000 -validateDistributionUrl=true -zipStoreBase=GRADLE_USER_HOME -zipStorePath=permwrapper/dists +distributionBase=GRADLE_USER_HOME +distributionPath=permwrapper/dists +distributionUrl=https\://services.gradle.org/distributions/gradle-8.11-bin.zip +networkTimeout=10000 +validateDistributionUrl=true +zipStoreBase=GRADLE_USER_HOME +zipStorePath=permwrapper/dists diff --git a/gradlew b/gradlew index 1aa94a42..f5feea6d 100755 --- a/gradlew +++ b/gradlew @@ -15,6 +15,8 @@ # See the License for the specific language governing permissions and # limitations under the License. # +# SPDX-License-Identifier: Apache-2.0 +# ############################################################################## # @@ -55,7 +57,7 @@ # Darwin, MinGW, and NonStop. # # (3) This script is generated from the Groovy template -# https://github.com/gradle/gradle/blob/HEAD/subprojects/plugins/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt +# https://github.com/gradle/gradle/blob/HEAD/platforms/jvm/plugins-application/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt # within the Gradle project. # # You can find Gradle at https://github.com/gradle/gradle/. @@ -84,7 +86,8 @@ done # shellcheck disable=SC2034 APP_BASE_NAME=${0##*/} # Discard cd standard output in case $CDPATH is set (https://github.com/gradle/gradle/issues/25036) -APP_HOME=$( cd "${APP_HOME:-./}" > /dev/null && pwd -P ) || exit +APP_HOME=$( cd -P "${APP_HOME:-./}" > /dev/null && printf '%s +' "$PWD" ) || exit # Use the maximum available, or set MAX_FD != -1 to use that value. MAX_FD=maximum diff --git a/gradlew.bat b/gradlew.bat index 93e3f59f..9b42019c 100644 --- a/gradlew.bat +++ b/gradlew.bat @@ -1,92 +1,94 @@ -@rem -@rem Copyright 2015 the original author or authors. -@rem -@rem Licensed under the Apache License, Version 2.0 (the "License"); -@rem you may not use this file except in compliance with the License. -@rem You may obtain a copy of the License at -@rem -@rem https://www.apache.org/licenses/LICENSE-2.0 -@rem -@rem Unless required by applicable law or agreed to in writing, software -@rem distributed under the License is distributed on an "AS IS" BASIS, -@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -@rem See the License for the specific language governing permissions and -@rem limitations under the License. -@rem - -@if "%DEBUG%"=="" @echo off -@rem ########################################################################## -@rem -@rem Gradle startup script for Windows -@rem -@rem ########################################################################## - -@rem Set local scope for the variables with windows NT shell -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% - -@rem Resolve any "." and ".." in APP_HOME to make it shorter. -for %%i in ("%APP_HOME%") do set APP_HOME=%%~fi - -@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. -set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m" - -@rem Find java.exe -if defined JAVA_HOME goto findJavaFromJavaHome - -set JAVA_EXE=java.exe -%JAVA_EXE% -version >NUL 2>&1 -if %ERRORLEVEL% equ 0 goto execute - -echo. -echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. -echo. -echo Please set the JAVA_HOME variable in your environment to match the -echo location of your Java installation. - -goto fail - -:findJavaFromJavaHome -set JAVA_HOME=%JAVA_HOME:"=% -set JAVA_EXE=%JAVA_HOME%/bin/java.exe - -if exist "%JAVA_EXE%" goto execute - -echo. -echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% -echo. -echo Please set the JAVA_HOME variable in your environment to match the -echo location of your Java installation. - -goto fail - -:execute -@rem Setup the command line - -set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar - - -@rem Execute Gradle -"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %* - -:end -@rem End local scope for the variables with windows NT shell -if %ERRORLEVEL% equ 0 goto mainEnd - -:fail -rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of -rem the _cmd.exe /c_ return code! -set EXIT_CODE=%ERRORLEVEL% -if %EXIT_CODE% equ 0 set EXIT_CODE=1 -if not ""=="%GRADLE_EXIT_CONSOLE%" exit %EXIT_CODE% -exit /b %EXIT_CODE% - -:mainEnd -if "%OS%"=="Windows_NT" endlocal - -:omega +@rem +@rem Copyright 2015 the original author or authors. +@rem +@rem Licensed under the Apache License, Version 2.0 (the "License"); +@rem you may not use this file except in compliance with the License. +@rem You may obtain a copy of the License at +@rem +@rem https://www.apache.org/licenses/LICENSE-2.0 +@rem +@rem Unless required by applicable law or agreed to in writing, software +@rem distributed under the License is distributed on an "AS IS" BASIS, +@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +@rem See the License for the specific language governing permissions and +@rem limitations under the License. +@rem +@rem SPDX-License-Identifier: Apache-2.0 +@rem + +@if "%DEBUG%"=="" @echo off +@rem ########################################################################## +@rem +@rem Gradle startup script for Windows +@rem +@rem ########################################################################## + +@rem Set local scope for the variables with windows NT shell +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% + +@rem Resolve any "." and ".." in APP_HOME to make it shorter. +for %%i in ("%APP_HOME%") do set APP_HOME=%%~fi + +@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. +set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m" + +@rem Find java.exe +if defined JAVA_HOME goto findJavaFromJavaHome + +set JAVA_EXE=java.exe +%JAVA_EXE% -version >NUL 2>&1 +if %ERRORLEVEL% equ 0 goto execute + +echo. 1>&2 +echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. 1>&2 +echo. 1>&2 +echo Please set the JAVA_HOME variable in your environment to match the 1>&2 +echo location of your Java installation. 1>&2 + +goto fail + +:findJavaFromJavaHome +set JAVA_HOME=%JAVA_HOME:"=% +set JAVA_EXE=%JAVA_HOME%/bin/java.exe + +if exist "%JAVA_EXE%" goto execute + +echo. 1>&2 +echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% 1>&2 +echo. 1>&2 +echo Please set the JAVA_HOME variable in your environment to match the 1>&2 +echo location of your Java installation. 1>&2 + +goto fail + +:execute +@rem Setup the command line + +set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar + + +@rem Execute Gradle +"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %* + +:end +@rem End local scope for the variables with windows NT shell +if %ERRORLEVEL% equ 0 goto mainEnd + +:fail +rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of +rem the _cmd.exe /c_ return code! +set EXIT_CODE=%ERRORLEVEL% +if %EXIT_CODE% equ 0 set EXIT_CODE=1 +if not ""=="%GRADLE_EXIT_CONSOLE%" exit %EXIT_CODE% +exit /b %EXIT_CODE% + +:mainEnd +if "%OS%"=="Windows_NT" endlocal + +:omega diff --git a/settings.gradle b/settings.gradle index d94f73c6..7cab49b5 100644 --- a/settings.gradle +++ b/settings.gradle @@ -1,30 +1,30 @@ -import org.gradle.internal.os.OperatingSystem - -pluginManagement { - repositories { - mavenLocal() - gradlePluginPortal() - String frcYear = '2024' - File frcHome - if (OperatingSystem.current().isWindows()) { - String publicFolder = System.getenv('PUBLIC') - if (publicFolder == null) { - publicFolder = "C:\\Users\\Public" - } - def homeRoot = new File(publicFolder, "wpilib") - frcHome = new File(homeRoot, frcYear) - } else { - def userFolder = System.getProperty("user.home") - def homeRoot = new File(userFolder, "wpilib") - frcHome = new File(homeRoot, frcYear) - } - def frcHomeMaven = new File(frcHome, 'maven') - maven { - name 'frcHome' - url frcHomeMaven - } - } -} - -Properties props = System.getProperties(); -props.setProperty("org.gradle.internal.native.headers.unresolved.dependencies.ignore", "true"); +import org.gradle.internal.os.OperatingSystem + +pluginManagement { + repositories { + mavenLocal() + gradlePluginPortal() + String frcYear = '2025' + File frcHome + if (OperatingSystem.current().isWindows()) { + String publicFolder = System.getenv('PUBLIC') + if (publicFolder == null) { + publicFolder = "C:\\Users\\Public" + } + def homeRoot = new File(publicFolder, "wpilib") + frcHome = new File(homeRoot, frcYear) + } else { + def userFolder = System.getProperty("user.home") + def homeRoot = new File(userFolder, "wpilib") + frcHome = new File(homeRoot, frcYear) + } + def frcHomeMaven = new File(frcHome, 'maven') + maven { + name 'frcHome' + url frcHomeMaven + } + } +} + +Properties props = System.getProperties(); +props.setProperty("org.gradle.internal.native.headers.unresolved.dependencies.ignore", "true"); diff --git a/src/main/deploy/swerve/neo/controllerproperties.json b/src/main/deploy/swerve/neo/controllerproperties.json deleted file mode 100644 index 6f2dd133..00000000 --- a/src/main/deploy/swerve/neo/controllerproperties.json +++ /dev/null @@ -1,8 +0,0 @@ -{ - "angleJoystickRadiusDeadband": 0, - "heading": { - "p": 0, - "i": 0, - "d": 0 - } -} \ No newline at end of file diff --git a/src/main/deploy/swerve/neo/modules/backleft.json b/src/main/deploy/swerve/neo/modules/backleft.json deleted file mode 100644 index a147632f..00000000 --- a/src/main/deploy/swerve/neo/modules/backleft.json +++ /dev/null @@ -1,26 +0,0 @@ -{ - "drive": { - "type": "sparkmax", - "id": 35, - "canbus": null - }, - "angle": { - "type": "sparkmax", - "id": 36, - "canbus": null - }, - "encoder": { - "type": "cancoder", - "id": 37, - "canbus": null - }, - "inverted": { - "drive": false, - "angle": true - }, - "absoluteEncoderOffset": 334.24812, - "location": { - "front": -12.375, - "left": 11.375 - } -} \ No newline at end of file diff --git a/src/main/deploy/swerve/neo/modules/backright.json b/src/main/deploy/swerve/neo/modules/backright.json deleted file mode 100644 index 6402efe2..00000000 --- a/src/main/deploy/swerve/neo/modules/backright.json +++ /dev/null @@ -1,26 +0,0 @@ -{ - "drive": { - "type": "sparkmax", - "id": 30, - "canbus": null - }, - "angle": { - "type": "sparkmax", - "id": 31, - "canbus": null - }, - "encoder": { - "type": "cancoder", - "id": 32, - "canbus": null - }, - "inverted": { - "drive": false, - "angle": true - }, - "absoluteEncoderOffset": 102.8322, - "location": { - "front": -12.375, - "left": -11.375 - } -} \ No newline at end of file diff --git a/src/main/deploy/swerve/neo/modules/frontleft.json b/src/main/deploy/swerve/neo/modules/frontleft.json deleted file mode 100644 index 5da62f9a..00000000 --- a/src/main/deploy/swerve/neo/modules/frontleft.json +++ /dev/null @@ -1,26 +0,0 @@ -{ - "drive": { - "type": "sparkmax", - "id": 10, - "canbus": null - }, - "angle": { - "type": "sparkmax", - "id": 11, - "canbus": null - }, - "encoder": { - "type": "cancoder", - "id": 12, - "canbus": null - }, - "inverted": { - "drive": false, - "angle": true - }, - "absoluteEncoderOffset": 222.9786, - "location": { - "front": 12.375, - "left": 11.375 - } -} \ No newline at end of file diff --git a/src/main/deploy/swerve/neo/modules/frontright.json b/src/main/deploy/swerve/neo/modules/frontright.json deleted file mode 100644 index 52e625a4..00000000 --- a/src/main/deploy/swerve/neo/modules/frontright.json +++ /dev/null @@ -1,26 +0,0 @@ -{ - "drive": { - "type": "sparkmax", - "id": 20, - "canbus": null - }, - "angle": { - "type": "sparkmax", - "id": 21, - "canbus": null - }, - "encoder": { - "type": "cancoder", - "id": 22, - "canbus": null - }, - "inverted": { - "drive": false, - "angle": true - }, - "absoluteEncoderOffset": 12.12876, - "location": { - "front": 12.375, - "left": -11.375 - } -} \ No newline at end of file diff --git a/src/main/deploy/swerve/neo/modules/physicalproperties.json b/src/main/deploy/swerve/neo/modules/physicalproperties.json deleted file mode 100644 index bb1b1dd0..00000000 --- a/src/main/deploy/swerve/neo/modules/physicalproperties.json +++ /dev/null @@ -1,16 +0,0 @@ -{ - "conversionFactor": { - "drive": 0.047286787200699704, - "angle": 16.8 - }, - "currentLimit": { - "drive": 40, - "angle": 20 - }, - "rampRate": { - "drive": 0.25, - "angle": 0.25 - }, - "wheelGripCoefficientOfFriction": 1.19, - "optimalVoltage": 12 -} \ No newline at end of file diff --git a/src/main/deploy/swerve/neo/modules/pidfproperties.json b/src/main/deploy/swerve/neo/modules/pidfproperties.json deleted file mode 100644 index 99664bbe..00000000 --- a/src/main/deploy/swerve/neo/modules/pidfproperties.json +++ /dev/null @@ -1,16 +0,0 @@ -{ - "drive": { - "p": 0.0020645, - "i": 0, - "d": 0, - "f": 0, - "iz": 0 - }, - "angle": { - "p": 0.01, - "i": 0, - "d": 0, - "f": 0, - "iz": 0 - } -} \ No newline at end of file diff --git a/src/main/deploy/swerve/neo/swervedrive.json b/src/main/deploy/swerve/neo/swervedrive.json deleted file mode 100644 index a3e36527..00000000 --- a/src/main/deploy/swerve/neo/swervedrive.json +++ /dev/null @@ -1,14 +0,0 @@ -{ - "imu": { - "type": "navx", - "id": -1, - "canbus": "null" - }, - "invertedIMU": true, - "modules": [ - "frontleft.json", - "frontright.json", - "backleft.json", - "backright.json" - ] -} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 471b7606..ed4ed664 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -7,8 +7,6 @@ import static edu.wpi.first.math.util.Units.inchesToMeters; import static frc.robot.Constants.BotTarget.BLUE_NOTE_WOLVERINE; import static frc.robot.Constants.BotTarget.RED_NOTE_WOLVERINE; -import static frc.robot.Constants.Swerve.Chassis.TRACK_WIDTH_METRES; -import static frc.robot.Constants.Swerve.Chassis.WHEEL_BASE_METRES; import static frc.robot.Constants.UsefulHeadings.FACING_CHAIN_BLUE_CENTER; import static frc.robot.Constants.UsefulHeadings.FACING_CHAIN_BLUE_LEFT; import static frc.robot.Constants.UsefulHeadings.FACING_CHAIN_BLUE_RIGHT; @@ -16,12 +14,20 @@ import static frc.robot.Constants.UsefulHeadings.FACING_CHAIN_RED_LEFT; import static frc.robot.Constants.UsefulHeadings.FACING_CHAIN_RED_RIGHT; +import ca.team1310.swerve.SwerveTelemetry; +import ca.team1310.swerve.core.config.*; +import ca.team1310.swerve.utils.Coordinates; +import frc.robot.subsystems.vision.VisionConfig; 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.math.geometry.Translation3d; import edu.wpi.first.wpilibj.util.Color; import frc.robot.subsystems.lighting.LightstripRegion; +import frc.robot.subsystems.swerve.SwerveDriveSubsystemConfig; +import frc.robot.subsystems.swerve.SwerveRotationConfig; +import frc.robot.subsystems.swerve.SwerveTranslationConfig; +import frc.robot.subsystems.vision.VisionTelemetryLevel; /** * The Constants class provides a convenient place for teams to hold robot-wide @@ -50,6 +56,162 @@ public static final class Swerve { */ public static final boolean DISABLED = false; + /** + * Front to back from the middle of the wheels + */ + public static final double WHEEL_BASE_METRES = inchesToMeters(24.75); + /** + * Side to side from the middle of the wheels + */ + public static final double TRACK_WIDTH_METRES = inchesToMeters(22.75); + + public static final double SDS_MK4I_WHEEL_RADIUS_M = 0.051; + + public static final SwerveTranslationConfig TRANSLATION_CONFIG = new SwerveTranslationConfig( + 0.02, + 1.0, + 20, + 20, + 42.0, + 1.2, + 0, + 0 + ); + + public static final SwerveRotationConfig ROTATION_CONFIG = new SwerveRotationConfig( + /* min rot vel radPS */Rotation2d.fromDegrees(12).getRadians(), + /* max rot vel radPS */Rotation2d.fromRotations(1).getRadians(), + /* max rotation jump speed */Rotation2d.fromDegrees(120).getRadians(), + /* slow zone */Rotation2d.fromDegrees(55).getRadians(), + /* max rotation accel */Rotation2d.fromRotations(1310).getRadians(), + /* rotation tolerance */Rotation2d.fromDegrees(2).getRadians(), + 0.4, + 0, + 0 + ); + + private static final MotorConfig ANGLE_MOTOR_CONFIG = new MotorConfig( + MotorType.NEO_SPARK_MAX, + true, + 20, + 12, + 0.25, + 150.0 / 7/* SDS MK4i 150/7:1 */, + 0.0125, + 0, + 0, + 0, + 0 + ); + + private static final MotorConfig DRIVE_MOTOR_CONFIG = new MotorConfig( + MotorType.NEO_SPARK_FLEX, + true, + 40, + 12, + 0.25, + 6.75/* SDS MK4i L2 --> 6.75:1 */, + 0.11, + 0, + 0, + 0, + 0 + ); + + private static final EncoderConfig ANGLE_ENCODER_CONFIG = new EncoderConfig(false, 0.005, 5); + + public static final ModuleConfig FRONT_LEFT = new ModuleConfig( + "frontleft", + new Coordinates(TRACK_WIDTH_METRES / 2, WHEEL_BASE_METRES / 2), + SDS_MK4I_WHEEL_RADIUS_M, + 10, + DRIVE_MOTOR_CONFIG, + 11, + ANGLE_MOTOR_CONFIG, + 12, + 222.9786, + ANGLE_ENCODER_CONFIG + ); + + public static final ModuleConfig FRONT_RIGHT = new ModuleConfig( + "frontright", + new Coordinates(TRACK_WIDTH_METRES / 2, -WHEEL_BASE_METRES / 2), + SDS_MK4I_WHEEL_RADIUS_M, + 20, + DRIVE_MOTOR_CONFIG, + 21, + ANGLE_MOTOR_CONFIG, + 22, + 12.12876, + ANGLE_ENCODER_CONFIG + ); + + public static final ModuleConfig BACK_LEFT = new ModuleConfig( + "backleft", + new Coordinates(-TRACK_WIDTH_METRES / 2, WHEEL_BASE_METRES / 2), + SDS_MK4I_WHEEL_RADIUS_M, + 35, + DRIVE_MOTOR_CONFIG, + 36, + ANGLE_MOTOR_CONFIG, + 37, + 334.24812, + ANGLE_ENCODER_CONFIG + ); + + public static final ModuleConfig BACK_RIGHT = new ModuleConfig( + "backright", + new Coordinates(-TRACK_WIDTH_METRES / 2, -WHEEL_BASE_METRES / 2), + SDS_MK4I_WHEEL_RADIUS_M, + 30, + DRIVE_MOTOR_CONFIG, + 31, + ANGLE_MOTOR_CONFIG, + 32, + 102.8322, + ANGLE_ENCODER_CONFIG + ); + + public static final SwerveTelemetry TELEMETRY = new SwerveTelemetry(4); + + static { + TELEMETRY.level = TelemetryLevel.VERBOSE; + } + + public static final CoreSwerveConfig CORE_SWERVE_CONFIG = new CoreSwerveConfig( + WHEEL_BASE_METRES, + TRACK_WIDTH_METRES, + SDS_MK4I_WHEEL_RADIUS_M, + Robot.kDefaultPeriod, + TRANSLATION_CONFIG.maxModuleSpeedMPS(), + TRANSLATION_CONFIG.maxSpeedMPS(), + ROTATION_CONFIG.maxRotVelocityRadPS(), + FRONT_LEFT, + FRONT_RIGHT, + BACK_LEFT, + BACK_RIGHT, + TELEMETRY.level + ); + + public static final VisionConfig VISION_CONFIG = new VisionConfig( + 0, + 0, + 0.7, + 0.1, + .5, + true, + VisionTelemetryLevel.VERBOSE + ); + + + public static final SwerveDriveSubsystemConfig SUBSYSTEM_CONFIG = new SwerveDriveSubsystemConfig( + true, + CORE_SWERVE_CONFIG, + VISION_CONFIG, + TRANSLATION_CONFIG, + ROTATION_CONFIG + ); + public static final class Chassis { /** @@ -83,7 +245,7 @@ public static final class Chassis { public static final Rotation2d ROTATION_SLOW_ZONE = Rotation2d.fromDegrees(35); public static final Rotation2d MIN_ROTATIONAL_VELOCITY_PER_SEC = Rotation2d.fromDegrees(45); public static final Rotation2d MAX_ROTATIONAL_VELOCITY_PER_SEC = Rotation2d.fromDegrees(360); - public static final Rotation2d MAX_ROTATIONAL_JUMP_VELOCITY_PER_SEC = Rotation2d.fromDegrees(205); + public static final Rotation2d MAX_ROTATIONAL_JUMP_VELOCITY_PER_SEC = Rotation2d.fromDegrees(180); public static final double MAX_ROTATION_ACCELERATION_RAD_PER_SEC2 = Rotation2d.fromRotations(1310) .getRadians(); public static final Rotation2d ROTATION_DECELERATION_DISTANCE = Rotation2d.fromDegrees(5); @@ -128,7 +290,7 @@ public static final class HeadingPIDConfig { * Standard drive speed factor. Regular teleop drive will use this factor of the max * translational speed. */ - public static final double GENERAL_SPEED_FACTOR = .6; + public static final double GENERAL_SPEED_FACTOR = .4; /** * Maximum drive speed factor. When boosting, this factor will be multiplied against the @@ -650,7 +812,7 @@ public static final class ArmConstants { public static final class ClimbConstants { - public static final boolean DISABLED = false; + public static final boolean DISABLED = true; public static final int RIGHT_CLIMB_MOTOR_CAN_ADDRESS = 60; public static final int LEFT_CLIMB_MOTOR_CAN_ADDRESS = 61; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 69b87c96..07840dac 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -17,7 +17,6 @@ import frc.robot.subsystems.ClimbSubsystem; import frc.robot.subsystems.lighting.LightingSubsystem; import frc.robot.subsystems.swerve.SwerveSubsystem; -import frc.robot.subsystems.swerve.yagsl.YagslSubsystem; import frc.robot.subsystems.vision.JackmanVisionSubsystem; import static frc.robot.Constants.LightingConstants.*; @@ -42,21 +41,20 @@ public class RobotContainer { private final ArmSubsystem arm = new ArmSubsystem(); private final ClimbSubsystem climb = new ClimbSubsystem(lighting); private final File yagslConfig = new File(Filesystem.getDeployDirectory(), "swerve/neo"); - private final SwerveSubsystem drive = new YagslSubsystem(lighting, yagslConfig); -// private final SwerveSubsystem drive = new RunnymedeSwerveSubsystem(lighting); + private final SwerveSubsystem swerveDriveSubsystem = new SwerveSubsystem(Constants.Swerve.SUBSYSTEM_CONFIG); private final OperatorInput operatorInput = new OperatorInput( OiConstants.DRIVER_CONTROLLER_PORT, OiConstants.OPERATOR_CONTROLLER_PORT, - drive, arm, climb, jackman, lighting); + swerveDriveSubsystem, arm, climb, jackman, lighting); /** * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { - drive.setDefaultCommand(new TeleopDriveCommand(drive, lighting, operatorInput)); + swerveDriveSubsystem.setDefaultCommand(new TeleopDriveCommand(swerveDriveSubsystem, lighting, operatorInput)); arm.setDefaultCommand(new DefaultArmCommand(operatorInput, arm)); - climb.setDefaultCommand(new DefaultClimbCommand(operatorInput, climb, drive)); + climb.setDefaultCommand(new DefaultClimbCommand(operatorInput, climb, swerveDriveSubsystem)); operatorInput.configureTriggerBindings(); operatorInput.initAutoSelectors(); diff --git a/src/main/java/frc/robot/commands/arm/ArmBaseCommand.java b/src/main/java/frc/robot/commands/arm/ArmBaseCommand.java index 4fd9031a..e9aa8fc0 100644 --- a/src/main/java/frc/robot/commands/arm/ArmBaseCommand.java +++ b/src/main/java/frc/robot/commands/arm/ArmBaseCommand.java @@ -400,7 +400,8 @@ public boolean driveToArmPosition(double targetLinkAngle, double targetAimAngle, // Determine if the arm is within the requested range if (Math.abs(linkAngleError) <= linkTolerance && Math.abs(aimAngleError) <= aimTolerance) { - + armSubsystem.setLinkPivotSpeed(0); + armSubsystem.setAimPivotSpeed(0); return true; } diff --git a/src/main/java/frc/robot/commands/auto/ScoreLoadedBarnumValjean.java b/src/main/java/frc/robot/commands/auto/ScoreLoadedBarnumValjean.java index f6fd9da2..09aba45f 100644 --- a/src/main/java/frc/robot/commands/auto/ScoreLoadedBarnumValjean.java +++ b/src/main/java/frc/robot/commands/auto/ScoreLoadedBarnumValjean.java @@ -30,9 +30,9 @@ public ScoreLoadedBarnumValjean(SwerveSubsystem swerve, ArmSubsystem armSubsyste // barnum addCommands(new WaitCommand(.8) - .deadlineWith(new StartIntakeCommand(armSubsystem, lighting))); + .raceWith(new StartIntakeCommand(armSubsystem, lighting))); addCommands(new StartIntakeCommand(armSubsystem, lighting) - .deadlineWith(new DriveToPositionCommand(swerve, BLUE_BARNUM, RED_BARNUM))); + .raceWith(new DriveToPositionCommand(swerve, BLUE_BARNUM, RED_BARNUM))); addCommands(new DriveToPositionCommand(swerve, BLUE_BARNUM_SHOT, RED_BARNUM_SHOT)); addCommands(RotateToTargetCommand.createRotateToSpeakerCommand(swerve) .alongWith(new CompactFromIntakeCommand(armSubsystem, false) @@ -44,7 +44,7 @@ public ScoreLoadedBarnumValjean(SwerveSubsystem swerve, ArmSubsystem armSubsyste .andThen(new StartIntakeCommand(armSubsystem, lighting)); Command drive = new RotateToLocationCommand(swerve, BLUE_VALJEAN, RED_VALJEAN) .andThen(new DriveToPositionFacingCommand(swerve, BLUE_VALJEAN, RED_VALJEAN, 1.5)); - addCommands(arm.deadlineWith(drive)); + addCommands(arm.raceWith(drive)); addCommands(RotateToTargetCommand.createRotateToSpeakerCommand(swerve) diff --git a/src/main/java/frc/robot/commands/auto/ScoreLoadedWolverineBarnumValjean.java b/src/main/java/frc/robot/commands/auto/ScoreLoadedWolverineBarnumValjean.java index 8fa31702..954f92d4 100644 --- a/src/main/java/frc/robot/commands/auto/ScoreLoadedWolverineBarnumValjean.java +++ b/src/main/java/frc/robot/commands/auto/ScoreLoadedWolverineBarnumValjean.java @@ -33,7 +33,7 @@ public ScoreLoadedWolverineBarnumValjean(SwerveSubsystem swerve, ArmSubsystem ar addCommands(new DriveToPositionCommand(swerve, IN_FRONT_OF_WOLVERINE_BLUE, IN_FRONT_OF_WOLVERINE_RED)); addCommands( new StartIntakeCommand(armSubsystem, lighting) - .deadlineWith(new DriveToPositionCommand(swerve, WOLVERINE_PICKUP_BLUE, WOLVERINE_PICKUP_RED, 1.5))); + .raceWith(new DriveToPositionCommand(swerve, WOLVERINE_PICKUP_BLUE, WOLVERINE_PICKUP_RED, 1.5))); // todo: maybe remove this reverse code addCommands(new SimpleDriveRobotOrientedCommand(swerve, -1.0, 0, 0, 0.4)); addCommands(RotateToTargetCommand.createRotateToSpeakerCommand(swerve)); @@ -47,7 +47,7 @@ public ScoreLoadedWolverineBarnumValjean(SwerveSubsystem swerve, ArmSubsystem ar .andThen(new StartIntakeCommand(armSubsystem, lighting)); Command drive = new RotateToLocationCommand(swerve, BLUE_BARNUM, RED_BARNUM) .andThen(new DriveToPositionFacingCommand(swerve, BLUE_BARNUM, RED_BARNUM, 1.5)); - addCommands(arm.deadlineWith(drive)); + addCommands(arm.raceWith(drive)); addCommands(new DriveToPositionCommand(swerve, BLUE_BARNUM_SHOT, RED_BARNUM_SHOT)); @@ -65,7 +65,7 @@ public ScoreLoadedWolverineBarnumValjean(SwerveSubsystem swerve, ArmSubsystem ar .andThen(new StartIntakeCommand(armSubsystem, lighting)); Command drive = new RotateToLocationCommand(swerve, BLUE_VALJEAN, RED_VALJEAN) .andThen(new DriveToPositionFacingCommand(swerve, BLUE_VALJEAN, RED_VALJEAN, 1.5)); - addCommands(arm.deadlineWith(drive)); + addCommands(arm.raceWith(drive)); addCommands(RotateToTargetCommand.createRotateToSpeakerCommand(swerve) diff --git a/src/main/java/frc/robot/commands/auto/TheDoubleDown.java b/src/main/java/frc/robot/commands/auto/TheDoubleDown.java index 0eb684d3..8ec5af09 100644 --- a/src/main/java/frc/robot/commands/auto/TheDoubleDown.java +++ b/src/main/java/frc/robot/commands/auto/TheDoubleDown.java @@ -26,7 +26,7 @@ public TheDoubleDown(SwerveSubsystem swerve, ArmSubsystem armSubsystem, Lighting // barnum addCommands(new StartIntakeCommand(armSubsystem, lighting) - .deadlineWith(new WaitCommand(2).andThen(new SimpleDriveRobotOrientedCommand(swerve, 1, 0, 0, 3)))); + .raceWith(new WaitCommand(2).andThen(new SimpleDriveRobotOrientedCommand(swerve, 1, 0, 0, 3)))); addCommands(new CompactCommand(armSubsystem).alongWith(new ReverseNoteCommand(armSubsystem))); addCommands(new SimpleDriveRobotOrientedCommand(swerve, -1, 0, TEMP_DRIFT_COMPENSATION.getRadians(), 3)); addCommands(new ShootCommand(armSubsystem, lighting)); diff --git a/src/main/java/frc/robot/commands/climb/BaseClimbCommand.java b/src/main/java/frc/robot/commands/climb/BaseClimbCommand.java index 2698c642..db3212f2 100644 --- a/src/main/java/frc/robot/commands/climb/BaseClimbCommand.java +++ b/src/main/java/frc/robot/commands/climb/BaseClimbCommand.java @@ -39,7 +39,7 @@ protected void climbFlat(double speed) { } else { // figure out how crooken we are - double rollRadians = driveSubsystem.getGyroRotation3d().getZ(); + double rollRadians = driveSubsystem.getPose().getRotation().getRadians(); double absRollRadians = Math.abs(rollRadians); if (absRollRadians > Constants.ClimbConstants.LEVEL_CLIMB_TOLERANCE.getRadians()) { diff --git a/src/main/java/frc/robot/commands/swervedrive/SwerveUtils.java b/src/main/java/frc/robot/commands/swervedrive/SwerveUtils.java index 047c9f53..771f784e 100644 --- a/src/main/java/frc/robot/commands/swervedrive/SwerveUtils.java +++ b/src/main/java/frc/robot/commands/swervedrive/SwerveUtils.java @@ -2,10 +2,10 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; -import frc.robot.Constants; +import frc.robot.Constants.Swerve.Chassis.VelocityPIDConfig; -import static frc.robot.Constants.Swerve.Chassis.*; -import static frc.robot.Constants.Swerve.Chassis.MIN_TRANSLATION_SPEED_MPS; +import static frc.robot.Constants.Swerve.TRANSLATION_CONFIG; +import static frc.robot.Constants.Swerve.ROTATION_CONFIG; public class SwerveUtils { private SwerveUtils() { @@ -45,11 +45,11 @@ public static Rotation2d getHeadingToFieldPosition(Translation2d currentRobotLoc public static boolean isCloseEnough(Rotation2d currentHeading, Rotation2d desiredHeading, Rotation2d tolerance) { if (tolerance == null) { - tolerance = ROTATION_TOLERANCE; + tolerance = Rotation2d.fromRadians(ROTATION_CONFIG.toleranceRadians()); } - else if (tolerance.getRadians() < ROTATION_TOLERANCE.getRadians()) { + else if (tolerance.getRadians() < ROTATION_CONFIG.toleranceRadians()) { // tolerance can't be below the minimum the robot can achieve - tolerance = ROTATION_TOLERANCE; + tolerance = Rotation2d.fromRadians(ROTATION_CONFIG.toleranceRadians()); } Rotation2d delta = desiredHeading.minus(currentHeading); @@ -64,7 +64,7 @@ else if (tolerance.getRadians() < ROTATION_TOLERANCE.getRadians()) { */ public static boolean isCloseEnough(Translation2d currentLocation, Translation2d desiredLocation) { Translation2d delta = desiredLocation.minus(currentLocation); - return Math.abs(delta.getNorm()) <= TRANSLATION_TOLERANCE_METRES; + return Math.abs(delta.getNorm()) <= TRANSLATION_CONFIG.toleranceMetres(); } /** @@ -82,17 +82,17 @@ static Translation2d computeVelocity(Translation2d translationToTravel, double m double distanceMetres = translationToTravel.getNorm(); // don't worry about tiny translations - if (distanceMetres < TRANSLATION_TOLERANCE_METRES) { + if (distanceMetres < TRANSLATION_CONFIG.toleranceMetres()) { return new Translation2d(); } // safety code - if (maxSpeed > MAX_TRANSLATION_SPEED_MPS) { - maxSpeed = MAX_TRANSLATION_SPEED_MPS; + if (maxSpeed > TRANSLATION_CONFIG.maxSpeedMPS()) { + maxSpeed = TRANSLATION_CONFIG.maxSpeedMPS(); } // ensure that we have enough room to decelerate - double decelDistance = DECEL_FROM_MAX_TO_STOP_DIST_METRES; + double decelDistance = 1.9; double decelDistRatio = distanceMetres / decelDistance; if (decelDistRatio < 1) { maxSpeed *= decelDistRatio; @@ -111,11 +111,10 @@ static Translation2d computeVelocity(Translation2d translationToTravel, double m } // Confirm speed is not too slow to move - if (speed < MIN_TRANSLATION_SPEED_MPS) { - speed = MIN_TRANSLATION_SPEED_MPS; + if (speed < TRANSLATION_CONFIG.minSpeedMPS()) { + speed = TRANSLATION_CONFIG.minSpeedMPS(); } - Rotation2d angle = translationToTravel.getAngle(); double xSign = Math.signum(translationToTravel.getX()); @@ -143,14 +142,14 @@ static Rotation2d computeOmega(Rotation2d target, Rotation2d current) { double errSignum = Math.signum(errorRad); final double omegaRad; - if (absErrRad < ROTATION_TOLERANCE.getRadians()) { + if (absErrRad < ROTATION_CONFIG.toleranceRadians()) { omegaRad = 0; } - else if (absErrRad < ROTATION_SLOW_ZONE.getRadians()) { - omegaRad = errSignum * MIN_ROTATIONAL_VELOCITY_PER_SEC.getRadians(); + else if (absErrRad < ROTATION_CONFIG.slowZoneRadians()) { + omegaRad = errSignum * ROTATION_CONFIG.minRotVelocityRadPS(); } else { - omegaRad = errSignum * MAX_ROTATIONAL_JUMP_VELOCITY_PER_SEC.getRadians(); + omegaRad = errSignum * ROTATION_CONFIG.maxJumpSpeedRadPS(); } // log(String.format("omega: %.2f", omegaRad)); diff --git a/src/main/java/frc/robot/commands/swervedrive/TeleopDriveCommand.java b/src/main/java/frc/robot/commands/swervedrive/TeleopDriveCommand.java index 6011e5d7..17c6b2b8 100644 --- a/src/main/java/frc/robot/commands/swervedrive/TeleopDriveCommand.java +++ b/src/main/java/frc/robot/commands/swervedrive/TeleopDriveCommand.java @@ -78,12 +78,12 @@ public void execute() { // its y value, but that should convert into positive x movement on the field. The // Runnymede Controller inverts stick y-axis values, so "forward" on stick is positive. // Thus, positive y stick axis maps to positive x translation on the field. - final double vX = oi.getDriverControllerAxis(LEFT, Y); + final double vX = -oi.getDriverControllerAxis(LEFT, Y); // Left and right movement on the left stick (the stick's x-axis) maps to the y-axis on the // field. Left on the stick (negative x) maps to positive y on the field, and vice versa. // Thus, negative x stick axis maps to positive y translation on the field. - final double vY = -oi.getDriverControllerAxis(LEFT, X); + final double vY = oi.getDriverControllerAxis(LEFT, X); // Left and right on the right stick will change the direction the robot is facing - its // heading. Positive x values on the stick translate to clockwise motion, and vice versa. @@ -123,7 +123,7 @@ public void execute() { modeForDebug = "Steering"; // Compute omega lockOnSpeaker = false; - double w = Math.pow(correctedCcwRotAngularVelPct, 3) * MAX_ROTATIONAL_VELOCITY_PER_SEC.getRadians(); + double w = Math.pow(correctedCcwRotAngularVelPct, 3) * MAX_ROTATIONAL_VELOCITY_PER_SEC.getRadians()/2; omega = Rotation2d.fromRadians(w); // Save previous heading for when we are finished steering. headingSetpoint = swerve.getPose().getRotation(); @@ -239,7 +239,7 @@ private static Translation2d calculateTeleopVelocity(double vX, double vY, doubl magnitude = Math.pow(magnitude, 3); // convert from % to mps - magnitude = magnitude * boostFactor * MAX_TRANSLATION_SPEED_MPS; + magnitude = magnitude * boostFactor * MAX_TRANSLATION_SPEED_MPS * 4; // convert to vector return new Translation2d(magnitude, angle); diff --git a/src/main/java/frc/robot/commands/test/SystemTestCommand.java b/src/main/java/frc/robot/commands/test/SystemTestCommand.java index 743ae2af..c8c7b2b6 100644 --- a/src/main/java/frc/robot/commands/test/SystemTestCommand.java +++ b/src/main/java/frc/robot/commands/test/SystemTestCommand.java @@ -205,51 +205,51 @@ private void applyMotorSpeed() { case NONE: break; case FRONT_LEFT_DRIVE: { - double mps = motorSpeed * Constants.Swerve.Chassis.MAX_MODULE_SPEED_MPS; + double mps = motorSpeed * Constants.Swerve.TRANSLATION_CONFIG.maxModuleSpeedMPS(); angle = Rotation2d.fromDegrees(0); - drive.setModuleStateForTestMode(Constants.Swerve.Module.FRONT_LEFT, new SwerveModuleState(mps, angle)); + drive.setModuleState(Constants.Swerve.FRONT_LEFT.name(), new SwerveModuleState(mps, angle)); break; } case FRONT_LEFT_TURN: { motorSpeed = 0; angle = new Rotation2d(controller.getLeftX(), controller.getLeftY()); - drive.setModuleStateForTestMode(Constants.Swerve.Module.FRONT_LEFT, new SwerveModuleState(0, angle)); + drive.setModuleState(Constants.Swerve.FRONT_LEFT.name(), new SwerveModuleState(0, angle)); break; } case BACK_LEFT_DRIVE: { - double mps = motorSpeed * Constants.Swerve.Chassis.MAX_MODULE_SPEED_MPS; + double mps = motorSpeed * Constants.Swerve.TRANSLATION_CONFIG.maxModuleSpeedMPS(); angle = Rotation2d.fromDegrees(0); - drive.setModuleStateForTestMode(Constants.Swerve.Module.BACK_LEFT, new SwerveModuleState(mps, angle)); + drive.setModuleState(Constants.Swerve.BACK_LEFT.name(), new SwerveModuleState(mps, angle)); break; } case BACK_LEFT_TURN: { motorSpeed = 0; angle = new Rotation2d(controller.getLeftX(), controller.getLeftY()); - drive.setModuleStateForTestMode(Constants.Swerve.Module.BACK_LEFT, new SwerveModuleState(0, angle)); + drive.setModuleState(Constants.Swerve.BACK_LEFT.name(), new SwerveModuleState(0, angle)); break; } case BACK_RIGHT_DRIVE: { - double mps = motorSpeed * Constants.Swerve.Chassis.MAX_MODULE_SPEED_MPS; + double mps = motorSpeed * Constants.Swerve.TRANSLATION_CONFIG.maxModuleSpeedMPS(); angle = Rotation2d.fromDegrees(0); - drive.setModuleStateForTestMode(Constants.Swerve.Module.BACK_RIGHT, new SwerveModuleState(mps, angle)); + drive.setModuleState(Constants.Swerve.BACK_RIGHT.name(), new SwerveModuleState(mps, angle)); break; } case BACK_RIGHT_TURN: { motorSpeed = 0; angle = new Rotation2d(controller.getLeftX(), controller.getLeftY()); - drive.setModuleStateForTestMode(Constants.Swerve.Module.BACK_RIGHT, new SwerveModuleState(0, angle)); + drive.setModuleState(Constants.Swerve.BACK_RIGHT.name(), new SwerveModuleState(0, angle)); break; } case FRONT_RIGHT_DRIVE: { - double mps = motorSpeed * Constants.Swerve.Chassis.MAX_MODULE_SPEED_MPS; + double mps = motorSpeed * Constants.Swerve.TRANSLATION_CONFIG.maxModuleSpeedMPS(); angle = Rotation2d.fromDegrees(0); - drive.setModuleStateForTestMode(Constants.Swerve.Module.FRONT_RIGHT, new SwerveModuleState(mps, angle)); + drive.setModuleState(Constants.Swerve.FRONT_RIGHT.name(), new SwerveModuleState(mps, angle)); break; } case FRONT_RIGHT_TURN: { motorSpeed = 0; angle = new Rotation2d(controller.getLeftX(), controller.getLeftY()); - drive.setModuleStateForTestMode(Constants.Swerve.Module.FRONT_RIGHT, new SwerveModuleState(0, angle)); + drive.setModuleState(Constants.Swerve.FRONT_RIGHT.name(), new SwerveModuleState(0, angle)); break; } case LINK: { diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index 236bad86..c194d7ea 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -8,8 +8,8 @@ import static frc.robot.Constants.ArmConstants.SLOW_AIM_SPEED; import static frc.robot.Constants.ArmConstants.SLOW_LINK_SPEED; -import com.revrobotics.CANSparkLowLevel.MotorType; -import com.revrobotics.CANSparkMax; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.AnalogInput; @@ -20,15 +20,15 @@ public class ArmSubsystem extends RunnymedeSubsystemBase { - private final CANSparkMax linkMotor = new CANSparkMax(ArmConstants.LINK_MOTOR_CAN_ADDRESS, + private final SparkMax linkMotor = new SparkMax(ArmConstants.LINK_MOTOR_CAN_ADDRESS, MotorType.kBrushless); - private final CANSparkMax aimMotor = new CANSparkMax(ArmConstants.AIM_MOTOR_CAN_ADDRESS, + private final SparkMax aimMotor = new SparkMax(ArmConstants.AIM_MOTOR_CAN_ADDRESS, MotorType.kBrushless); - private final CANSparkMax intakeMotor = new CANSparkMax(ArmConstants.INTAKE_MOTOR_CAN_ADDRESS, + private final SparkMax intakeMotor = new SparkMax(ArmConstants.INTAKE_MOTOR_CAN_ADDRESS, MotorType.kBrushless); - private final CANSparkMax shooterBottomMotor = new CANSparkMax(ArmConstants.SHOOTER_MOTOR_CAN_ADDRESS, + private final SparkMax shooterBottomMotor = new SparkMax(ArmConstants.SHOOTER_MOTOR_CAN_ADDRESS, MotorType.kBrushless); - private final CANSparkMax shooterTopMotor = new CANSparkMax(ArmConstants.SHOOTER_MOTOR_CAN_ADDRESS + 1, + private final SparkMax shooterTopMotor = new SparkMax(ArmConstants.SHOOTER_MOTOR_CAN_ADDRESS + 1, MotorType.kBrushless); private final DigitalInput linkLowerLimitSwitch = new DigitalInput(ArmConstants.LINK_LOWER_LIMIT_SWITCH_DIO_PORT); private final DigitalInput noteDetector = new DigitalInput(ArmConstants.INTAKE_NOTE_DETECTOR_DIO_PORT); diff --git a/src/main/java/frc/robot/subsystems/ClimbSubsystem.java b/src/main/java/frc/robot/subsystems/ClimbSubsystem.java index 3c715aa7..03af7ab2 100644 --- a/src/main/java/frc/robot/subsystems/ClimbSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ClimbSubsystem.java @@ -2,8 +2,9 @@ import static frc.robot.Constants.ClimbConstants.*; -import com.revrobotics.CANSparkLowLevel.MotorType; -import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; import edu.wpi.first.wpilibj.DigitalInput; import frc.robot.Constants.ClimbConstants; @@ -16,11 +17,11 @@ public class ClimbSubsystem extends RunnymedeSubsystemBase { // Lights Subsystem private final LightingSubsystem lighting; - private final CANSparkMax leftClimbMotor = new CANSparkMax( + private final SparkMax leftClimbMotor = new SparkMax( ClimbConstants.LEFT_CLIMB_MOTOR_CAN_ADDRESS, MotorType.kBrushless); - private final CANSparkMax rightClimbMotor = new CANSparkMax( + private final SparkMax rightClimbMotor = new SparkMax( ClimbConstants.RIGHT_CLIMB_MOTOR_CAN_ADDRESS, MotorType.kBrushless); private final DigitalInput rightClimbLimitSwitch = new DigitalInput( @@ -169,7 +170,7 @@ public void initEncoders() { if (!rightEncoderInitialized) { if (rightAllTheWayDown()) { rightClimbMotor.getEncoder().setPosition(0); - rightClimbMotor.burnFlash(); +// rightClimbMotor.burnFlash(); rightEncoderInitialized = true; } else { @@ -180,7 +181,7 @@ public void initEncoders() { if (!leftEncoderInitialized) { if (leftAllTheWayDown()) { leftClimbMotor.getEncoder().setPosition(0); - leftClimbMotor.burnFlash(); +// leftClimbMotor.burnFlash(); leftEncoderInitialized = true; } else { diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDriveSubsystemConfig.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDriveSubsystemConfig.java new file mode 100644 index 00000000..9413bf14 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveDriveSubsystemConfig.java @@ -0,0 +1,12 @@ +package frc.robot.subsystems.swerve; + +import ca.team1310.swerve.core.config.CoreSwerveConfig; +import frc.robot.subsystems.vision.VisionConfig; + +public record SwerveDriveSubsystemConfig( + boolean enabled, + CoreSwerveConfig coreConfig, + VisionConfig visionConfig, + SwerveTranslationConfig translationConfig, + SwerveRotationConfig rotationConfig +) {} diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveRotationConfig.java b/src/main/java/frc/robot/subsystems/swerve/SwerveRotationConfig.java new file mode 100644 index 00000000..5b1e2d80 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveRotationConfig.java @@ -0,0 +1,13 @@ +package frc.robot.subsystems.swerve; + +public record SwerveRotationConfig( + double minRotVelocityRadPS, + double maxRotVelocityRadPS, + double maxJumpSpeedRadPS, + double slowZoneRadians, + double maxAccelerationRadPS2, + double toleranceRadians, + double headingP, + double headingI, + double headingD +) {} diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 39267a78..259c5baf 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -1,43 +1,77 @@ package frc.robot.subsystems.swerve; -import static frc.robot.Constants.Swerve.Chassis.MAX_ROTATION_ACCELERATION_RAD_PER_SEC2; -import static frc.robot.Constants.Swerve.Chassis.MAX_TRANSLATION_ACCELERATION_MPS2; -import static frc.robot.RunnymedeUtils.format; -import static frc.robot.utils.vision.PoseConfidence.NONE; - -import edu.wpi.first.math.Matrix; +import static ca.team1310.swerve.utils.SwerveUtils.difference; +import static ca.team1310.swerve.utils.SwerveUtils.normalizeDegrees; +import static frc.robot.Constants.FieldConstants.FIELD_EXTENT_METRES_X; +import static frc.robot.Constants.FieldConstants.FIELD_EXTENT_METRES_Y; + +import ca.team1310.swerve.RunnymedeSwerveDrive; +import ca.team1310.swerve.SwerveTelemetry; +import ca.team1310.swerve.gyro.GyroAwareSwerveDrive; +import ca.team1310.swerve.utils.SwerveUtils; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.filter.SlewRateLimiter; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.*; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import frc.robot.Constants; -import frc.robot.subsystems.RunnymedeSubsystemBase; -import frc.robot.subsystems.lighting.LightingSubsystem; -import frc.robot.subsystems.lighting.pattern.VisionConfidenceHigh; -import frc.robot.subsystems.lighting.pattern.VisionConfidenceLow; -import frc.robot.subsystems.lighting.pattern.VisionConfidenceMedium; -import frc.robot.subsystems.lighting.pattern.VisionConfidenceNone; -import frc.robot.utils.vision.PoseConfidence; -import frc.robot.utils.vision.InvalidVisionDataException; -import frc.robot.utils.vision.VisionPositionInfo; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.telemetry.Telemetry; -import frc.robot.utils.vision.HughVision; -public abstract class SwerveSubsystem extends RunnymedeSubsystemBase { +public class SwerveSubsystem extends SubsystemBase { + + private final RunnymedeSwerveDrive drive; + private final SwerveDriveSubsystemConfig config; + private final double maxTranslationSpeedMPS; + private final SlewRateLimiter xLimiter; + private final SlewRateLimiter yLimiter; + private final SlewRateLimiter omegaLimiter; + private final PIDController headingPIDController; + private final PIDController velocityPIDController; + + public SwerveSubsystem(SwerveDriveSubsystemConfig config) { + this.drive = new GyroAwareSwerveDrive(config.coreConfig()); + this.config = config; + this.maxTranslationSpeedMPS = config.coreConfig().maxAttainableTranslationSpeedMetresPerSecond(); + this.xLimiter = new SlewRateLimiter(this.config.translationConfig().maxAccelMPS2()); + this.yLimiter = new SlewRateLimiter(this.config.translationConfig().maxAccelMPS2()); + this.omegaLimiter = new SlewRateLimiter(config.rotationConfig().maxAccelerationRadPS2()); + headingPIDController = new PIDController( + config.rotationConfig().headingP(), + config.rotationConfig().headingI(), + config.rotationConfig().headingD() + ); + velocityPIDController = new PIDController( + config.translationConfig().velocityP(), + config.translationConfig().velocityI(), + config.translationConfig().velocityD() + ); + } + + /* + * ********************************************************************************************* + * Core methods for controlling the drivebase + */ + + private void driveSafely(ChassisSpeeds robotOrientedVelocity) { + double x = robotOrientedVelocity.vxMetersPerSecond; + double y = robotOrientedVelocity.vyMetersPerSecond; + double w = -robotOrientedVelocity.omegaRadiansPerSecond; + + // Limit change in values. Note this may not scale + // evenly - one may reach desired speed before another. - private final SlewRateLimiter xLimiter = new SlewRateLimiter(MAX_TRANSLATION_ACCELERATION_MPS2); - private final SlewRateLimiter yLimiter = new SlewRateLimiter(MAX_TRANSLATION_ACCELERATION_MPS2); - private final SlewRateLimiter omegaLimiter = new SlewRateLimiter(MAX_ROTATION_ACCELERATION_RAD_PER_SEC2); - private final HughVision hugh = new HughVision(); - private final LightingSubsystem lighting; + // Use driveFieldOriented to avoid this. - public SwerveSubsystem(LightingSubsystem lighting) { - this.lighting = lighting; + // x = xLimiter.calculate(x); + // y = yLimiter.calculate(y); + // w = omegaLimiter.calculate(w); + + ChassisSpeeds safeVelocity = new ChassisSpeeds(x, y, w); + + if (this.config.enabled()) { + this.drive.drive(x, y, w); + } } /** @@ -53,38 +87,15 @@ public SwerveSubsystem(LightingSubsystem lighting) { */ public final void driveRobotOriented(ChassisSpeeds velocity) { - double x = velocity.vxMetersPerSecond; - double y = velocity.vyMetersPerSecond; - double w = velocity.omegaRadiansPerSecond; - - // Limit change in values. Note this may not scale evenly - one may reach desired - // speed before another. This will be corrected the next time drive() is called. - - x = xLimiter.calculate(x); - y = yLimiter.calculate(y); - w = omegaLimiter.calculate(w); - - ChassisSpeeds safeVelocity = new ChassisSpeeds(x, y, w); - - Telemetry.swerve.swerve_robot_chassis_speeds = safeVelocity; - - if (!Constants.Swerve.DISABLED) { - driveRawRobotOriented(safeVelocity); - } + driveSafely(velocity); } /** - * The internal method for controlling the drivebase. This code does not apply any - * limiters or validation, and should be used by implementing swerve drive subsystems - * only. - *

- * Takes the desired chassis speeds of the robot - in a robot-oriented configuration. - * - * @param velocity The intended velocity of the robot chassis relative to itself. - * @see ChassisSpeeds for how to construct a ChassisSpeeds object including - * {@link ChassisSpeeds#fromFieldRelativeSpeeds(double, double, double, Rotation2d)} + * Stop all motors as fast as possible */ - protected abstract void driveRawRobotOriented(ChassisSpeeds velocity); + public void stop() { + driveRobotOriented(new ChassisSpeeds(0, 0, 0)); + } /** * Convenience method for controlling the robot in field-oriented drive mode. Transforms the @@ -95,17 +106,31 @@ public final void driveRobotOriented(ChassisSpeeds velocity) { * from the alliance wall, and positive y is toward the left wall when looking through the * driver station glass. * @param omega the rotation rate of the heading of the robot. CCW positive. - * @see #driveRobotOriented(ChassisSpeeds) */ public final void driveFieldOriented(Translation2d velocity, Rotation2d omega) { - double x = velocity.getX(); - double y = velocity.getY(); - double w = omega.getRadians(); - Rotation2d theta = this.getPose().getRotation(); - Telemetry.swerve.swerve_velocity_field = velocity; + + driveFieldOrientedInternal(velocity, omega); + } + + private void driveFieldOrientedInternal(Translation2d velocity, Rotation2d omega) { + + double x = velocity.getX(); + double y = velocity.getY(); + double w = omega.getRadians(); + Rotation2d theta = Rotation2d.fromDegrees(drive.getYaw()); ChassisSpeeds chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(x, y, w, theta); - this.driveRobotOriented(chassisSpeeds); + driveSafely(chassisSpeeds); + } + + /** + * Lock the swerve drive to prevent it from moving. This can only be called when the robot is + * nearly stationary. + * + * @return true if successfully locked, false otherwise + */ + public boolean lock() { + return drive.lock(); } /** @@ -114,105 +139,203 @@ public final void driveFieldOriented(Translation2d velocity, Rotation2d omega) { * * @return The robot's pose */ - public abstract Pose2d getPose(); + public Pose2d getPose() { + return new Pose2d(new Translation2d(), Rotation2d.fromDegrees(getYaw())); + } + + public double getYaw() { + return drive.getYaw(); + } /** - * Return the gyro rotation for the robot, with yaw adjusted for the configured offset + * Resets the gyro angle to zero and resets odometry to the same position, but + * facing toward 0. + */ + public void zeroGyro() { + drive.zeroGyro(); + } + + /** + * Change the robot's internal understanding of its position and rotation. This + * is not an incremental change or suggestion, it discontinuously re-sets the + * pose to the specified pose. * - * @return adjusted rotation3d from the gyro + * @param pose the new location and heading of the robot. */ - public abstract Rotation3d getGyroRotation3d(); + public void resetOdometry(Pose2d pose) { + drive.resetOdometry(pose); + } /** - * Resets the gyro angle to zero and resets odometry to the same position, but - * facing toward 0. + * Set the desired module state for the named module. This should ONLY be used when testing + * the serve drivebase in a controlled environment. + * + * This SHOULD NOT be called during normal operation - it is designed for TEST MODE ONLY! + * + * @param moduleName the module to activate + * @param desiredState the state of the specified module. + */ + public void setModuleState(String moduleName, SwerveModuleState desiredState) { + drive.setModuleState(moduleName, desiredState.speedMetersPerSecond, desiredState.angle.getDegrees()); + } + + @Override + public String toString() { + Pose2d pose = getPose(); + double x = pose.getX(); + double y = pose.getY(); + double theta = pose.getRotation().getDegrees(); + return String.format("SwerveDriveSubsystem Pose: %.2f,%.2f @ %.1f deg", x, y, theta); + } + + /* + * ********************************************************************************************* + * Convenience methods for subsystem users */ - public abstract void zeroGyro(); /** - * Stop all motors as fast as possible + * Determine if the robot is close enough to the desired heading to stop rotating. */ - public void stop() { - driveRobotOriented(new ChassisSpeeds(0, 0, 0)); + public boolean isCloseEnough(Rotation2d desiredHeading) { + return SwerveUtils.isCloseEnough( + drive.getPose().getRotation().getRadians(), + desiredHeading.getRadians(), + config.rotationConfig().toleranceRadians() + ); + } + + /** + * Determine if the robot is close enough to the desired location to stop moving. + */ + public boolean isCloseEnough(Translation2d desiredLocation) { + return SwerveUtils.isCloseEnough(drive.getPose().getTranslation(), desiredLocation, config.translationConfig().toleranceMetres()); + } + + /** + * Determine if the robot is close enough to the desired pose to stop moving. + */ + public boolean isCloseEnough(Pose2d desiredPose) { + return isCloseEnough(desiredPose.getTranslation()) && isCloseEnough(desiredPose.getRotation()); } /** - * Lock the swerve drive to prevent it from moving. + * Compute the distance to the specified field position in metres. */ - public abstract void lock(); + public double getDistanceToFieldPositionMetres(Translation2d target) { + return drive.getPose().getTranslation().getDistance(target); + } - abstract public void addVisionMeasurement(Pose2d robotPose, double timestamp, Matrix visionMeasurementStdDevs); + /** + * Compute the heading required to face the specified position on the field. + * + * @param target field position + * @return the heading toward that position. + */ + public Rotation2d getHeadingToFieldPosition(Translation2d target) { + Translation2d currentRobotLocation = drive.getPose().getTranslation(); + Translation2d delta = target.minus(currentRobotLocation); + return delta.getAngle(); + } /** - * Updates the field relative position of the robot using module - * position data from the modules themselves, plus the gyro. + * Drive as fast as safely possible to the specified pose, up ot the max speed specified. + * + * @param desiredPose the desired location on the field */ - protected abstract void updateOdometryWithStates(); + public final void driveToFieldPose(Pose2d desiredPose, double maxSpeedMPS) { + Pose2d current = getPose(); + Transform2d delta = difference(desiredPose, current); + + Translation2d velocity = computeVelocity(delta.getTranslation(), maxSpeedMPS); + Rotation2d omega = computeOmega(desiredPose.getRotation()); + + driveFieldOrientedInternal(velocity, omega); + } /** - * Update the field relative position of the robot using vision - * position data returned from the vision subsystem. + * Return a velocity that will traverse the specified translation as fast as possible without + * overshooting the location. The initial speed is expected to be 0 and the final speed is + * expected to be 0. + * + * @param translationToTravel the desired translation to travel + * @param maxSpeed the maximum speed to travel in Metres per Second + * @return the velocity vector, in metres per second that the robot can safely travel + * to traverse the distance specified */ - private void updateOdometryWithVisionInfo() { + private Translation2d computeVelocity(Translation2d translationToTravel, double maxSpeed) { + // todo: replace with PID + double distanceMetres = translationToTravel.getNorm(); - Pose2d odometryPose = getPose(); - VisionPositionInfo visPosInfo; - PoseConfidence confidence; + // don't worry about tiny translations + if (distanceMetres < config.translationConfig().toleranceMetres()) { + return new Translation2d(); + } - try { - visPosInfo = hugh.getVisionPositionInfo(odometryPose); - confidence = visPosInfo.confidence(); + // safety code + if (maxSpeed > maxTranslationSpeedMPS) { + maxSpeed = maxTranslationSpeedMPS; } - catch (InvalidVisionDataException e) { - visPosInfo = null; - confidence = NONE; + + // ensure that we have enough room to decelerate + double decelDistance = Math.pow(maxTranslationSpeedMPS, 2) / (2 * config.translationConfig().maxAccelMPS2()); + double decelDistRatio = distanceMetres / decelDistance; + if (decelDistRatio < 1) { + maxSpeed *= decelDistRatio; } - Telemetry.swerve.swerve_vispose = visPosInfo; - if (confidence != NONE) { - addVisionMeasurement(visPosInfo.pose(), visPosInfo.timestampSeconds(), visPosInfo.deviation()); + double speed; + if (distanceMetres >= decelDistance) { + // cruising + speed = maxSpeed; + } else { + // decelerating + double pctToGo = distanceMetres / decelDistance; + speed = maxSpeed * pctToGo * velocityPIDController.getP(); } - switch (confidence) { - case HIGH: - lighting.setVisionPattern(VisionConfidenceHigh.getInstance()); - break; - case MEDIUM: - lighting.setVisionPattern(VisionConfidenceMedium.getInstance()); - break; - case LOW: - lighting.setVisionPattern(VisionConfidenceLow.getInstance()); - break; - case NONE: - lighting.setVisionPattern(VisionConfidenceNone.getInstance()); - break; + // Confirm speed is not too slow to move + if (speed < config.translationConfig().minSpeedMPS()) { + speed = config.translationConfig().minSpeedMPS(); } - } - public abstract void updateTelemetry(); + Rotation2d angle = translationToTravel.getAngle(); + + double xSign = Math.signum(translationToTravel.getX()); + double ySign = Math.signum(translationToTravel.getY()); + return new Translation2d(xSign * speed * Math.abs(angle.getCos()), ySign * speed * Math.abs(angle.getSin())); + } /** - * Set the swerve module state for the specified module. This is intended to be used ONLY in - * test mode! + * Utility function to compute the required rotation speed of the robot given the heading + * provided. * - * @param module the module configuration object - used to identify the module only. - * @param desiredState the desired state of the swerve module + * @param desiredHeading the desired heading of the robot + * @return The required rotation speed of the robot */ - public abstract void setModuleStateForTestMode(Constants.Swerve.Module module, SwerveModuleState desiredState); - - public abstract void resetOdometry(Pose2d replacementPose); - - @Override - public void periodic() { - super.periodic(); - updateOdometryWithStates(); - updateOdometryWithVisionInfo(); - updateTelemetry(); - Telemetry.swerve.swerve_pose = getPose(); - } + public Rotation2d computeOmega(Rotation2d desiredHeading) { + // todo: replace with PID + double currentYaw = drive.getYaw(); + double targetRad = Math.toRadians(normalizeDegrees(desiredHeading.getDegrees())); + double currentRad = Math.toRadians(normalizeDegrees(currentYaw)); + SmartDashboard.putNumber("computeOmega/1-targetRad", targetRad); + SmartDashboard.putNumber("computeOmega/2-currentRad", currentRad); + + double errorRad = targetRad - currentRad; + errorRad = Math.toRadians(normalizeDegrees(Math.toDegrees(errorRad))); + double absErrRad = Math.abs(errorRad); + double errSignum = Math.signum(errorRad); + + final double omegaRadPerSec; + if (absErrRad < config.rotationConfig().toleranceRadians()) { + omegaRadPerSec = 0; + } else if (absErrRad < config.rotationConfig().slowZoneRadians()) { + omegaRadPerSec = errSignum * config.rotationConfig().minRotVelocityRadPS(); + } else { + omegaRadPerSec = errSignum * config.rotationConfig().maxJumpSpeedRadPS(); + } + SmartDashboard.putNumber("computeOmega/3-errorRad", errorRad); + SmartDashboard.putNumber("computeOmega/4-omegaRadPerSec", omegaRadPerSec); - @Override - public String toString() { - return "SwerveSubsystem Current Pose: " + format(getPose()); + return Rotation2d.fromRadians(omegaRadPerSec); } } diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveTranslationConfig.java b/src/main/java/frc/robot/subsystems/swerve/SwerveTranslationConfig.java new file mode 100644 index 00000000..5f978929 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveTranslationConfig.java @@ -0,0 +1,42 @@ +package frc.robot.subsystems.swerve; + +/** + * Configure the translation configuration properties of this swerve drive. + * + * @param toleranceMetres The tolerance in meters for the translation. If the robot is within this + * distance of the target, it is considered on target. + * @param minSpeedMPS The minimum speed the robot will move at. This is to prevent the robot from + * moving too slowly. + * @param maxSpeedMPS Set how fast you want the robot to actually translate across the field. + * This is the "speed limit" of the robot. + *

+ * Practically speaking 4.42 m/s is a good max, but + * consider 1-2 for development and 2-3 for competitions. + * @param maxModuleSpeedMPS Specify the maximum speed a module can physically reach in m/s. + * The SDS + * MK4i + * module with L2 gear ratio supports a maximum drive motor speed of 15.7ft/s (4.79m/s). + *

+ * Do not use this value in software to cap how fast the robot drives on the field. + * For that, use {@link #maxAccelMPS2} + * @param maxAccelMPS2 The maximum acceleration the robot will move at. This is to prevent the robot + * from accelerating dangerously quickly + * @param velocityP The proportional gain for the velocity PID controller + * This is the "strength" of the controller. Higher values will make the robot more aggressive. + * @param velocityI The integral gain for the velocity PID controller + * This is the "memory" of the controller. It will remember past errors and use them to correct the + * robot's path. + * @param velocityD The derivative gain for the velocity PID controller + * This is the "prediction" of the controller. It will predict future errors and use them to correct + * the robot's path. + */ +public record SwerveTranslationConfig( + double toleranceMetres, + double minSpeedMPS, + double maxSpeedMPS, + double maxModuleSpeedMPS, + double maxAccelMPS2, + double velocityP, + double velocityI, + double velocityD +) {} diff --git a/src/main/java/frc/robot/subsystems/swerve/runnymede/AngleMotor.java b/src/main/java/frc/robot/subsystems/swerve/runnymede/AngleMotor.java deleted file mode 100644 index da5b30a0..00000000 --- a/src/main/java/frc/robot/subsystems/swerve/runnymede/AngleMotor.java +++ /dev/null @@ -1,71 +0,0 @@ -package frc.robot.subsystems.swerve.runnymede; - -import com.revrobotics.CANSparkBase; - -import edu.wpi.first.math.geometry.Rotation2d; -import frc.robot.Constants; - -class AngleMotor extends SparkMaxNeoMotor { - - - /** - * Configure the SparkMAX and its integrated PIDF (PID + feed forward) control. - * - * @param canBusId bus id - * @param cfg motor configuration - * @{link https://docs.revrobotics.com/sparkmax/operating-modes/closed-loop-control} - */ - AngleMotor(int canBusId, Constants.Swerve.Motor cfg) { - super(canBusId); - // instantiate & configure motor - this.motor.setInverted(cfg.inverted); - configureSparkMax(() -> motor.enableVoltageCompensation(cfg.nominalVoltage)); - configureSparkMax(() -> motor.setSmartCurrentLimit(cfg.currentLimitAmps)); - configureSparkMax(() -> motor.setClosedLoopRampRate(cfg.rampRate)); - - // configure integrated encoder - final double turnConversionFactor = 1 / (cfg.gearRatio * 360); - configureSparkMax(() -> encoder.setPositionConversionFactor(turnConversionFactor)); - configureSparkMax(() -> encoder.setVelocityConversionFactor(turnConversionFactor / 60)); // todo: - // check - // 60 - - pid.setFeedbackDevice(encoder); // Configure feedback of the PID controller as the - // integrated encoder. - configureSparkMax(() -> pid.setP(cfg.p, 0)); - configureSparkMax(() -> pid.setI(cfg.i, 0)); - configureSparkMax(() -> pid.setD(cfg.d, 0)); - configureSparkMax(() -> pid.setFF(cfg.ff, 0)); - configureSparkMax(() -> pid.setIZone(cfg.iz, 0)); - configureSparkMax(() -> pid.setOutputRange(-1, 1, 0)); - - configureSparkMax(() -> pid.setPositionPIDWrappingEnabled(true)); - configureSparkMax(() -> pid.setPositionPIDWrappingMinInput(0)); - configureSparkMax(() -> pid.setPositionPIDWrappingMaxInput(90)); - - setMotorBrake(false); - - burnFlash(); - } - - Rotation2d getPosition() { - return Rotation2d.fromDegrees((encoder.getPosition() + 360) % 360); - } - - /** - * Set the integrated encoder position - * - * @param position Integrated encoder position - degrees for turning motor - */ - void setInternalEncoderPositionDegrees(double position) { - if (encoder.getPosition() != position) { - configureSparkMax(() -> encoder.setPosition(position)); - } - } - - - void setReferenceDegrees(double setpoint, double feedforward) { - configureSparkMax(() -> pid.setReference(setpoint, CANSparkBase.ControlType.kPosition, 0, feedforward)); - - } -} diff --git a/src/main/java/frc/robot/subsystems/swerve/runnymede/CanCoder.java b/src/main/java/frc/robot/subsystems/swerve/runnymede/CanCoder.java deleted file mode 100644 index 24350480..00000000 --- a/src/main/java/frc/robot/subsystems/swerve/runnymede/CanCoder.java +++ /dev/null @@ -1,107 +0,0 @@ -package frc.robot.subsystems.swerve.runnymede; - -import com.ctre.phoenix6.StatusCode; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.CANcoderConfiguration; -import com.ctre.phoenix6.configs.CANcoderConfigurator; -import com.ctre.phoenix6.configs.MagnetSensorConfigs; -import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue; -import com.ctre.phoenix6.signals.MagnetHealthValue; -import com.ctre.phoenix6.signals.SensorDirectionValue; - -import edu.wpi.first.wpilibj.DriverStation; - -/** - * Inspired by YAGSL SwerveAbsoluteEncoder and CANCoderSwerve. Designed to be used only - * internally inside the SwerveModule object. - */ -class CanCoder { - - /** - * The maximum amount of times the swerve encoder will attempt to configure itself if failures - * occur. - */ - private final int maximumRetries = 5; - - private final CANcoder encoder; - private final double absoluteEncoderOffset; - - boolean readingError = false; - - CanCoder(int canId, double absoluteEncoderOffset, boolean inverted) { - encoder = new CANcoder(canId); - encoder.clearStickyFaults(); - - CANcoderConfigurator cfg = encoder.getConfigurator(); - cfg.apply(new CANcoderConfiguration()); // resets to factory defaults - MagnetSensorConfigs magnetSensorConfiguration = new MagnetSensorConfigs(); - cfg.refresh(magnetSensorConfiguration); - cfg.apply(magnetSensorConfiguration - .withAbsoluteSensorRange(AbsoluteSensorRangeValue.Unsigned_0To1) - .withSensorDirection( - inverted ? SensorDirectionValue.Clockwise_Positive : SensorDirectionValue.CounterClockwise_Positive)); - - // note, we aren't bothering to push this offset up into the encoder, we will just manage it - // here in this class - this.absoluteEncoderOffset = absoluteEncoderOffset; - } - - int getDeviceId() { - return encoder.getDeviceID(); - } - - /** - * Get the absolute position of the encoder. - *

- * Sets readingError = true if there is a problem using the encoder. - * - * @return Absolute position in degrees from [0, 360). - */ - double getAbsolutePositionInDegrees() { - readingError = isNotHealthy(); - if (readingError) { - return 0; - } - - StatusSignal angle = encoder.getAbsolutePosition().refresh(); - - int retryCount; - for (retryCount = 0; retryCount < maximumRetries; retryCount++) { - if (angle.getStatus() == StatusCode.OK) { - break; - } - angle = angle.waitForUpdate(0.005); - } - if (angle.getStatus() != StatusCode.OK) { - readingError = true; - DriverStation.reportWarning("CANCoder " + encoder.getDeviceID() + " reading was faulty after retrying " - + maximumRetries + " times. " + angle.getStatus().getDescription(), false); - return 0; - } - if (retryCount > 0) { - DriverStation.reportWarning("CANCoder " + encoder.getDeviceID() + " read successfully but required " + retryCount - + " retries to get valid data.", false); - } - - return (angle.getValue() * 360 - absoluteEncoderOffset + 360) % 360; - } - - private boolean isNotHealthy() { - MagnetHealthValue strength = encoder.getMagnetHealth().getValue(); - switch (strength) { - case Magnet_Green: - return false; - case Magnet_Orange: { - DriverStation.reportWarning("CANCoder " + encoder.getDeviceID() + " magnetic field is less than ideal.", false); - return false; - } - case Magnet_Invalid: - DriverStation.reportWarning("CANCoder " + encoder.getDeviceID() + " not usable - magnet INVALID.", false); - case Magnet_Red: - DriverStation.reportWarning("CANCoder " + encoder.getDeviceID() + " not usable - magnet health RED.", false); - } - return true; - } - -} diff --git a/src/main/java/frc/robot/subsystems/swerve/runnymede/DriveMotor.java b/src/main/java/frc/robot/subsystems/swerve/runnymede/DriveMotor.java deleted file mode 100644 index f609e49a..00000000 --- a/src/main/java/frc/robot/subsystems/swerve/runnymede/DriveMotor.java +++ /dev/null @@ -1,57 +0,0 @@ -package frc.robot.subsystems.swerve.runnymede; - -import com.revrobotics.CANSparkBase; - -import frc.robot.Constants; - -class DriveMotor extends SparkMaxNeoMotor { - - /** - * Configure the SparkMAX and its integrated PIDF (PID + feed forward) control. - * - * @param canBusId bus id - * @param cfg motor configuration - * @param wheelRadiusMetres the wheel radius in metres - * @{link https://docs.revrobotics.com/sparkmax/operating-modes/closed-loop-control} - */ - DriveMotor(int canBusId, Constants.Swerve.Motor cfg, double wheelRadiusMetres) { - super(canBusId); - - // instantiate & configure motor - this.motor.setInverted(cfg.inverted); - configureSparkMax(() -> motor.enableVoltageCompensation(cfg.nominalVoltage)); - configureSparkMax(() -> motor.setSmartCurrentLimit(cfg.currentLimitAmps)); - configureSparkMax(() -> motor.setClosedLoopRampRate(cfg.rampRate)); - - // configure integrated encoder - final double positionConversionfactor = (60 * Math.PI * 2 * wheelRadiusMetres) / cfg.gearRatio; - configureSparkMax(() -> encoder.setPositionConversionFactor(positionConversionfactor)); - configureSparkMax(() -> encoder.setVelocityConversionFactor(positionConversionfactor / 60)); - - - pid.setFeedbackDevice(encoder); // Configure feedback of the PID controller as the - // integrated encoder. - configureSparkMax(() -> pid.setP(cfg.p, 0)); - configureSparkMax(() -> pid.setI(cfg.i, 0)); - configureSparkMax(() -> pid.setD(cfg.d, 0)); - configureSparkMax(() -> pid.setFF(cfg.ff, 0)); - configureSparkMax(() -> pid.setIZone(cfg.iz, 0)); - configureSparkMax(() -> pid.setOutputRange(-1, 1, 0)); - configureSparkMax(() -> pid.setPositionPIDWrappingEnabled(false)); - setMotorBrake(true); - - burnFlash(); - } - - double getDistanceMetres() { - return encoder.getPosition(); - } - - void setReferenceMetresPerSecond(double setpointMPS, double feedforward) { - configureSparkMax(() -> pid.setReference(setpointMPS, CANSparkBase.ControlType.kVelocity, 0, feedforward)); - } - - double getVelocityMetresPerSecond() { - return encoder.getVelocity(); - } -} diff --git a/src/main/java/frc/robot/subsystems/swerve/runnymede/Gyro.java b/src/main/java/frc/robot/subsystems/swerve/runnymede/Gyro.java deleted file mode 100644 index d0639a84..00000000 --- a/src/main/java/frc/robot/subsystems/swerve/runnymede/Gyro.java +++ /dev/null @@ -1,34 +0,0 @@ -package frc.robot.subsystems.swerve.runnymede; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.wpilibj.SerialPort; -import com.kauailabs.navx.frc.AHRS; -import frc.robot.telemetry.Telemetry; - -class Gyro { - private final AHRS gyro; - private Rotation3d offset; - - Gyro() { - gyro = new AHRS(SerialPort.Port.kMXP); - offset = gyro.getRotation3d(); - } - - void zeroGyro() { - offset = gyro.getRotation3d(); - } - - Rotation3d getRotation3d() { - return gyro.getRotation3d().minus(offset); - } - - Rotation2d getRotation2d() { - return gyro.getRotation2d().minus(offset.toRotation2d()); - } - - void updateTelemetry() { - Telemetry.swerve.rawImuDegrees = gyro.getRotation2d().getDegrees(); - Telemetry.swerve.adjustedImuDegrees = getRotation2d().getDegrees(); - } -} diff --git a/src/main/java/frc/robot/subsystems/swerve/runnymede/RunnymedeSwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/runnymede/RunnymedeSwerveSubsystem.java deleted file mode 100644 index 99147a27..00000000 --- a/src/main/java/frc/robot/subsystems/swerve/runnymede/RunnymedeSwerveSubsystem.java +++ /dev/null @@ -1,223 +0,0 @@ -package frc.robot.subsystems.swerve.runnymede; - - -import static edu.wpi.first.math.util.Units.metersToInches; -import static frc.robot.Constants.Swerve.Chassis.MAX_MODULE_SPEED_MPS; -import static frc.robot.Constants.Swerve.Chassis.MAX_ROTATIONAL_VELOCITY_PER_SEC; -import static frc.robot.Constants.Swerve.Chassis.MAX_TRANSLATION_SPEED_MPS; -import static frc.robot.Constants.Swerve.Module.BACK_LEFT; -import static frc.robot.Constants.Swerve.Module.BACK_RIGHT; -import static frc.robot.Constants.Swerve.Module.FRONT_LEFT; -import static frc.robot.Constants.Swerve.Module.FRONT_RIGHT; -import static frc.robot.Constants.Swerve.Motor.ANGLE; -import static frc.robot.Constants.Swerve.Motor.DRIVE; - -import java.util.Arrays; - -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.wpilibj.RobotBase; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import frc.robot.Constants; -import frc.robot.Robot; -import frc.robot.subsystems.lighting.LightingSubsystem; -import frc.robot.subsystems.swerve.SwerveSubsystem; -import frc.robot.telemetry.Swerve; -import frc.robot.telemetry.Telemetry; - -/** - * Represents a swerve drive style drivetrain. - */ -public class RunnymedeSwerveSubsystem extends SwerveSubsystem { - private final SwerveModule[] modules; - private final SwerveDriveKinematics kinematics; - private final Gyro gyro; - private final SimulatedIMU simulatedIMU; - public Field2d field; - - public final SwerveDrivePoseEstimator swerveDrivePoseEstimator; - - public RunnymedeSwerveSubsystem(LightingSubsystem lighting) { - super(lighting); - - modules = new SwerveModule[4]; - modules[0] = new SwerveModule(FRONT_LEFT, DRIVE, ANGLE); - modules[1] = new SwerveModule(FRONT_RIGHT, DRIVE, ANGLE); - modules[2] = new SwerveModule(BACK_LEFT, DRIVE, ANGLE); - modules[3] = new SwerveModule(BACK_RIGHT, DRIVE, ANGLE); - - kinematics = new SwerveDriveKinematics( - Arrays.stream(modules).map(SwerveModule::getLocation).toArray(Translation2d[]::new)); - - gyro = new Gyro(); - simulatedIMU = new SimulatedIMU(); - - this.swerveDrivePoseEstimator = new SwerveDrivePoseEstimator( - this.kinematics, - gyro.getRotation2d(), - Arrays.stream(modules).map(SwerveModule::getPosition).toArray(SwerveModulePosition[]::new), - new Pose2d(new Translation2d(0.0, 0.0), Rotation2d.fromDegrees(0.0))); - - - Telemetry.swerve.implementation = Swerve.Implementation.RUNNYMEDE; - field = new Field2d(); - Telemetry.swerve.field = field; - Telemetry.swerve.maxSpeed = Constants.Swerve.Chassis.MAX_TRANSLATION_SPEED_MPS; - Telemetry.swerve.maxAngularVelocity = Constants.Swerve.Chassis.MAX_ROTATIONAL_VELOCITY_PER_SEC.getDegrees(); - Telemetry.swerve.moduleCount = modules.length; - Telemetry.swerve.sizeFrontBack = metersToInches(Constants.Swerve.Chassis.WHEEL_BASE_METRES); - Telemetry.swerve.sizeLeftRight = metersToInches(Constants.Swerve.Chassis.TRACK_WIDTH_METRES); - Telemetry.swerve.wheelLocations = new double[Telemetry.swerve.moduleCount * 2]; - for (int i = 0; i < modules.length; i++) { - SwerveModule module = modules[i]; - Telemetry.swerve.wheelLocations[i * 2] = metersToInches(module.getLocation().getX()); - Telemetry.swerve.wheelLocations[i * 2 + 1] = metersToInches(module.getLocation().getY()); - } - Telemetry.swerve.measuredStates = new double[Telemetry.swerve.moduleCount * 2]; - Telemetry.swerve.desiredStates = new double[Telemetry.swerve.moduleCount * 2]; - } - - private SwerveModuleState[] getStates() { - return Arrays.stream(modules).map(SwerveModule::getState).toArray(SwerveModuleState[]::new); - } - - private Pose2d[] getModulePoses(Pose2d robotPose) { - return Arrays.stream(modules).map(m -> { - Transform2d tx = new Transform2d(m.getLocation(), m.getState().angle); - return robotPose.plus(tx); - }).toArray(Pose2d[]::new); - } - - @Override - public void updateTelemetry() { - ChassisSpeeds measuredChassisSpeeds = kinematics.toChassisSpeeds(getStates()); - Telemetry.swerve.measuredChassisSpeeds[1] = measuredChassisSpeeds.vyMetersPerSecond; - Telemetry.swerve.measuredChassisSpeeds[0] = measuredChassisSpeeds.vxMetersPerSecond; - Telemetry.swerve.measuredChassisSpeeds[2] = Math.toDegrees(measuredChassisSpeeds.omegaRadiansPerSecond); - Telemetry.swerve.robotRotation = getPose().getRotation().getDegrees(); - gyro.updateTelemetry(); - - for (int i = 0; i < modules.length; i++) { - SwerveModule module = modules[i]; - SwerveModuleState moduleState = module.getState(); - Telemetry.swerve.measuredStates[i * 2] = moduleState.angle.getDegrees(); - Telemetry.swerve.measuredStates[(i * 2) + 1] = moduleState.speedMetersPerSecond; - module.updateTelemetry(); - } - } - - @Override - public void updateOdometryWithStates() { - swerveDrivePoseEstimator.update( - gyro.getRotation2d(), - Arrays.stream(modules).map(SwerveModule::getPosition).toArray(SwerveModulePosition[]::new)); - - Pose2d robotPose = swerveDrivePoseEstimator.getEstimatedPosition(); - - field.setRobotPose(robotPose); - - if (RobotBase.isSimulation()) { - simulatedIMU.updateOdometry(kinematics, getStates(), getModulePoses(robotPose), field); - } - } - - @Override - public void resetOdometry(Pose2d pose) { - this.swerveDrivePoseEstimator.resetPosition(gyro.getRotation2d(), - Arrays.stream(modules).map(SwerveModule::getPosition).toArray(SwerveModulePosition[]::new), pose); - } - - @Override - public Pose2d getPose() { - return swerveDrivePoseEstimator.getEstimatedPosition(); - } - - @Override - public void addVisionMeasurement(Pose2d robotPose, double timestamp, Matrix visionMeasurementStdDevs) { - this.swerveDrivePoseEstimator.addVisionMeasurement(robotPose, timestamp, visionMeasurementStdDevs); - } - - - @Override - public Rotation3d getGyroRotation3d() { - return gyro.getRotation3d(); - } - - @Override - public void zeroGyro() { - gyro.zeroGyro(); - } - - @Override - protected void driveRawRobotOriented(ChassisSpeeds velocity) { - - // calculate desired states - ChassisSpeeds discretized = ChassisSpeeds.discretize(velocity, Robot.kDefaultPeriod); - Translation2d centerOfRotation = new Translation2d(); - SwerveModuleState[] swerveModuleStates = kinematics.toSwerveModuleStates(discretized, centerOfRotation); - SwerveDriveKinematics.desaturateWheelSpeeds( - swerveModuleStates, velocity, - MAX_MODULE_SPEED_MPS, MAX_TRANSLATION_SPEED_MPS, MAX_ROTATIONAL_VELOCITY_PER_SEC.getRadians()); - - Telemetry.swerve.desiredChassisSpeeds[1] = velocity.vyMetersPerSecond; - Telemetry.swerve.desiredChassisSpeeds[0] = velocity.vxMetersPerSecond; - Telemetry.swerve.desiredChassisSpeeds[2] = Math.toDegrees(velocity.omegaRadiansPerSecond); - - // set states - for (int i = 0; i < modules.length; i++) { - modules[i].setDesiredState(swerveModuleStates[i]); - Telemetry.swerve.desiredStates[i * 2] = swerveModuleStates[i].angle.getDegrees(); - Telemetry.swerve.desiredStates[(i * 2) + 1] = swerveModuleStates[i].speedMetersPerSecond; - - } - } - - @Override - public void setModuleStateForTestMode(Constants.Swerve.Module module, SwerveModuleState desiredState) { - SwerveModule swerveModule = null; - for (SwerveModule m : modules) { - if (m.getName().equals(module.name)) { - swerveModule = m; - } - } - if (swerveModule == null) { - log("Invalid module name: " + module.name); - return; - } - - // set the state - swerveModule.setDesiredState(desiredState); - } - - @Override - public void lock() { - // TODO: ADD SAFETY CODE - - // set speed to 0 and angle wheels to center - for (int i = 0; i < modules.length; i++) { - modules[i].setDesiredState(new SwerveModuleState(0.0, modules[i].getPosition().angle)); - Telemetry.swerve.desiredStates[i * 2] = modules[i].getPosition().angle.getDegrees(); - Telemetry.swerve.desiredStates[(i * 2) + 1] = 0; - } - - // tell kinematics that we aren't moving - kinematics.toSwerveModuleStates(new ChassisSpeeds()); - - } - - @Override - public String toString() { - return "Runnymede " + super.toString(); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/swerve/runnymede/SimulatedIMU.java b/src/main/java/frc/robot/subsystems/swerve/runnymede/SimulatedIMU.java deleted file mode 100644 index c1e23f4a..00000000 --- a/src/main/java/frc/robot/subsystems/swerve/runnymede/SimulatedIMU.java +++ /dev/null @@ -1,54 +0,0 @@ -package frc.robot.subsystems.swerve.runnymede; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; - -import java.util.Optional; - -class SimulatedIMU { - private final Timer timer = new Timer(); - private double lastTime; - private double angle; - - SimulatedIMU() { - this.timer.start(); - this.lastTime = this.timer.get(); - } - - Rotation2d getYaw() { - return new Rotation2d(this.angle); - } - - Rotation2d getPitch() { - return new Rotation2d(); - } - - Rotation2d getRoll() { - return new Rotation2d(); - } - - Rotation3d getGyroRotation3d() { - return new Rotation3d(0.0, 0.0, this.angle); - } - - Optional getAccel() { - return Optional.empty(); - } - - void updateOdometry(SwerveDriveKinematics kinematics, SwerveModuleState[] states, Pose2d[] modulePoses, - Field2d field) { - this.angle += kinematics.toChassisSpeeds(states).omegaRadiansPerSecond * (this.timer.get() - this.lastTime); - this.lastTime = this.timer.get(); - field.getObject("XModules").setPoses(modulePoses); - } - - void setAngle(double angle) { - this.angle = angle; - } -} diff --git a/src/main/java/frc/robot/subsystems/swerve/runnymede/SimulatedSwerveModule.java b/src/main/java/frc/robot/subsystems/swerve/runnymede/SimulatedSwerveModule.java deleted file mode 100644 index 8708af21..00000000 --- a/src/main/java/frc/robot/subsystems/swerve/runnymede/SimulatedSwerveModule.java +++ /dev/null @@ -1,40 +0,0 @@ -package frc.robot.subsystems.swerve.runnymede; - -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.Timer; - -class SimulatedSwerveModule { - private final Timer timer = new Timer(); - private double dt; - private double fakePos; - private double fakeSpeed; - private double lastTime; - private SwerveModuleState state; - - SimulatedSwerveModule() { - this.timer.start(); - this.lastTime = this.timer.get(); - this.state = new SwerveModuleState(0.0, Rotation2d.fromDegrees(0.0)); - this.fakeSpeed = 0.0; - this.fakePos = 0.0; - this.dt = 0.0; - } - - void setDesiredState(SwerveModuleState desiredState) { - this.dt = this.timer.get() - this.lastTime; - this.lastTime = this.timer.get(); - this.state = desiredState; - this.fakeSpeed = desiredState.speedMetersPerSecond; - this.fakePos += this.fakeSpeed * this.dt; - } - - SwerveModulePosition getPosition() { - return new SwerveModulePosition(this.fakePos, this.state.angle); - } - - SwerveModuleState getState() { - return this.state; - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/swerve/runnymede/SparkMaxNeoMotor.java b/src/main/java/frc/robot/subsystems/swerve/runnymede/SparkMaxNeoMotor.java deleted file mode 100644 index 9ff273d9..00000000 --- a/src/main/java/frc/robot/subsystems/swerve/runnymede/SparkMaxNeoMotor.java +++ /dev/null @@ -1,75 +0,0 @@ -package frc.robot.subsystems.swerve.runnymede; - -import java.util.function.Supplier; - -import com.revrobotics.*; - -import edu.wpi.first.wpilibj.DriverStation; - -abstract class SparkMaxNeoMotor { - - /** - * The maximum amount of times the swerve motor will attempt to configure a motor if failures - * occur. - */ - private final int maximumRetries = 5; - protected final CANSparkMax motor; - protected final RelativeEncoder encoder; - protected final SparkPIDController pid; - - SparkMaxNeoMotor(int canBusId) { - // instantiate & configure motor - this.motor = new CANSparkMax(canBusId, CANSparkLowLevel.MotorType.kBrushless); - this.encoder = this.motor.getEncoder(); - pid = motor.getPIDController(); - configureCANStatusFrames(10, 20, 20, 500, 500); - configureSparkMax(motor::restoreFactoryDefaults); - configureSparkMax(motor::clearFaults); - } - - /** - * Run the configuration until it succeeds or times out. - * - * @param config Lambda supplier returning the error state. - */ - protected final void configureSparkMax(Supplier config) { - for (int i = 0; i < maximumRetries; i++) { - if (config.get() == REVLibError.kOk) { - return; - } - } - DriverStation.reportWarning("Failure configuring motor " + motor.getDeviceId(), true); - } - - protected final void setMotorBrake(boolean isBrakeMode) { - configureSparkMax(() -> motor.setIdleMode(isBrakeMode ? CANSparkBase.IdleMode.kBrake : CANSparkBase.IdleMode.kCoast)); - } - - protected final void burnFlash() { - try { - Thread.sleep(200); - } - catch (Exception e) { - } - configureSparkMax(() -> motor.burnFlash()); - } - - /** - * Set the CAN status frames. - * - * @param CANStatus0 Applied Output, Faults, Sticky Faults, Is Follower - * @param CANStatus1 Motor Velocity, Motor Temperature, Motor Voltage, Motor Current - * @param CANStatus2 Motor Position - * @param CANStatus3 Analog Sensor Voltage, Analog Sensor Velocity, Analog Sensor Position - * @param CANStatus4 Alternate Encoder Velocity, Alternate Encoder Position - */ - private void configureCANStatusFrames(int CANStatus0, int CANStatus1, int CANStatus2, int CANStatus3, int CANStatus4) { - configureSparkMax(() -> motor.setPeriodicFramePeriod(CANSparkLowLevel.PeriodicFrame.kStatus0, CANStatus0)); - configureSparkMax(() -> motor.setPeriodicFramePeriod(CANSparkLowLevel.PeriodicFrame.kStatus1, CANStatus1)); - configureSparkMax(() -> motor.setPeriodicFramePeriod(CANSparkLowLevel.PeriodicFrame.kStatus2, CANStatus2)); - configureSparkMax(() -> motor.setPeriodicFramePeriod(CANSparkLowLevel.PeriodicFrame.kStatus3, CANStatus3)); - configureSparkMax(() -> motor.setPeriodicFramePeriod(CANSparkLowLevel.PeriodicFrame.kStatus4, CANStatus4)); - // TODO: Configure Status Frame 5 and 6 if necessary - // https://docs.revrobotics.com/sparkmax/operating-modes/control-interfaces - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/swerve/runnymede/SwerveModule.java b/src/main/java/frc/robot/subsystems/swerve/runnymede/SwerveModule.java deleted file mode 100644 index 67b2eea7..00000000 --- a/src/main/java/frc/robot/subsystems/swerve/runnymede/SwerveModule.java +++ /dev/null @@ -1,117 +0,0 @@ -package frc.robot.subsystems.swerve.runnymede; - - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.wpilibj.RobotBase; -import frc.robot.Constants; -import frc.robot.telemetry.Telemetry; - -class SwerveModule { - - private final int INTERNAL_ENCODER_UPDATE_FREQ = 10; - private int internalEncoderUpdateCount = 0; - private final String name; - private final Translation2d location; - private final DriveMotor driveMotor; - private final AngleMotor angleMotor; - private final CanCoder encoder; - private final SimulatedSwerveModule sim; - - /** - * Constructs a SwerveModule with a drive motor, turning motor, drive encoder and turning - * encoder. - */ - SwerveModule(Constants.Swerve.Module cfg, Constants.Swerve.Motor driveCfg, Constants.Swerve.Motor angleCfg) { - this.name = cfg.name; - this.location = cfg.locationMetres; - driveMotor = new DriveMotor(cfg.driveCANID, driveCfg, cfg.wheelRadiusMetres); - angleMotor = new AngleMotor(cfg.angleCANID, angleCfg); - encoder = new CanCoder(cfg.encoderCANID, cfg.encoderAbsoluteOffsetDegrees, false); - - sim = new SimulatedSwerveModule(); - - updateInternalEncoder(); - } - - String getName() { - return name; - } - - Translation2d getLocation() { - return location; - } - - SwerveModulePosition getPosition() { - if (RobotBase.isSimulation()) { - return sim.getPosition(); - } - else { - return new SwerveModulePosition(driveMotor.getDistanceMetres(), angleMotor.getPosition()); - } - } - - SwerveModuleState getState() { - if (RobotBase.isSimulation()) { - return sim.getState(); - } - else { - double velocity = driveMotor.getVelocityMetresPerSecond(); - Rotation2d azimuth = angleMotor.getPosition(); - return new SwerveModuleState(velocity, azimuth); - } - } - - void setDesiredState(SwerveModuleState desiredState) { - if (RobotBase.isSimulation()) { - sim.setDesiredState(desiredState); - } - else { - Rotation2d currentHeading = angleMotor.getPosition(); - - // Optimize the reference state to avoid spinning further than 90 degrees - desiredState = SwerveModuleState.optimize(desiredState, currentHeading); - - /* - * If the angle error is close to 0 degrees, we are aligned properly, so we can apply - * full power to drive wheels. If the angle error is close to 90 degrees, driving in - * any direction does not help. Used cosine function on the error to scale the - * desired speed. If cosine is < 0 then scale to zero so that we don't invert the - * drive for no reason. - */ - Rotation2d steerError = desiredState.angle.minus(currentHeading); - double cosineScalar = steerError.getCos(); - desiredState.speedMetersPerSecond *= (cosineScalar < 0 ? 0 : cosineScalar); - driveMotor.setReferenceMetresPerSecond(desiredState.speedMetersPerSecond, 0); - - angleMotor.setReferenceDegrees(desiredState.angle.getDegrees(), 0); - } - - updateInternalEncoder(); - - Telemetry.swerve.getModule(name).speedMetersPerSecond = desiredState.speedMetersPerSecond; - Telemetry.swerve.getModule(name).angleDegrees = desiredState.angle.getDegrees(); - } - - void updateInternalEncoder() { - - if (internalEncoderUpdateCount++ >= INTERNAL_ENCODER_UPDATE_FREQ) { - internalEncoderUpdateCount = 0; - double angle = encoder.getAbsolutePositionInDegrees(); - if (encoder.readingError) { - System.out.println("Absolute encoder " + encoder.getDeviceId() + " could not be read."); - } - else { - angleMotor.setInternalEncoderPositionDegrees(angle); - } - } - } - - void updateTelemetry() { - Telemetry.swerve.getModule(name).absoluteEncoderPositionDegrees = encoder.getAbsolutePositionInDegrees(); - Telemetry.swerve.getModule(name).angleMotorPosition = angleMotor.getPosition(); - Telemetry.swerve.getModule(name).driveMotorPosition = driveMotor.getDistanceMetres(); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/swerve/yagsl/YagslSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/yagsl/YagslSubsystem.java deleted file mode 100644 index dc54c6cf..00000000 --- a/src/main/java/frc/robot/subsystems/swerve/yagsl/YagslSubsystem.java +++ /dev/null @@ -1,123 +0,0 @@ -package frc.robot.subsystems.swerve.yagsl; - -import static frc.robot.Constants.Swerve.Chassis.MAX_TRANSLATION_SPEED_MPS; - -import java.io.File; - -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import frc.robot.Constants; -import frc.robot.subsystems.lighting.LightingSubsystem; -import frc.robot.subsystems.swerve.SwerveSubsystem; -import frc.robot.telemetry.Swerve; -import frc.robot.telemetry.Telemetry; -import swervelib.SwerveDrive; -import swervelib.SwerveModule; -import swervelib.parser.SwerveParser; -import swervelib.telemetry.SwerveDriveTelemetry; - -public class YagslSubsystem extends SwerveSubsystem { - - /** - * Swerve drive object. - */ - private final SwerveDrive swerveDrive; - - - /** - * Initialize {@link SwerveDrive} with the directory provided. - * - * @param configDirectory Directory of swerve drive config files. - */ - public YagslSubsystem(LightingSubsystem lighting, File configDirectory) { - super(lighting); - // Configure the Telemetry before creating the SwerveDrive to avoid unnecessary - // objects being created. - Telemetry.swerve.implementation = Swerve.Implementation.YAGSL; - SwerveDriveTelemetry.verbosity = SwerveDriveTelemetry.TelemetryVerbosity.HIGH; - try { - swerveDrive = new SwerveParser(configDirectory).createSwerveDrive(MAX_TRANSLATION_SPEED_MPS); - } - catch (Exception e) { - throw new RuntimeException(e); - } - // Runnymede does its own heading correction in the commands. - swerveDrive.setHeadingCorrection(false); - swerveDrive.setCosineCompensator(!SwerveDriveTelemetry.isSimulation); - } - - @Override - public void setModuleStateForTestMode(Constants.Swerve.Module module, SwerveModuleState desiredState) { - SwerveModule swerveModule = swerveDrive.getModuleMap().get(module.name); - if (swerveModule == null) { - log("Invalid module name: " + module.name); - return; - } - - // save cosine compensator setting - boolean coco = swerveModule.configuration.useCosineCompensator; - swerveModule.configuration.useCosineCompensator = false; - - // set the state - swerveModule.setDesiredState(desiredState, true, true); - - // restore the cosine compensator setting - swerveModule.configuration.useCosineCompensator = coco; - } - - @Override - protected void driveRawRobotOriented(ChassisSpeeds velocity) { - swerveDrive.drive(velocity, false, new Translation2d()); - } - - @Override - public Pose2d getPose() { - return swerveDrive.getPose(); - } - - @Override - public void updateTelemetry() { - // noop - done internally inside SwerveDrive - } - - @Override - protected void updateOdometryWithStates() { - // noop - done internally inside SwerveDrive - } - - @Override - public void addVisionMeasurement(Pose2d robotPose, double timestamp, Matrix visionMeasurementStdDevs) { - swerveDrive.addVisionMeasurement(robotPose, timestamp, visionMeasurementStdDevs); - } - - @Override - public Rotation3d getGyroRotation3d() { - return swerveDrive.getGyroRotation3d(); - } - - @Override - public void zeroGyro() { - swerveDrive.zeroGyro(); - } - - @Override - public void lock() { - swerveDrive.lockPose(); - } - - @Override - public void resetOdometry(Pose2d pose) { - swerveDrive.resetOdometry(pose); - } - - @Override - public String toString() { - return "YAGSL " + super.toString(); - } -} diff --git a/src/main/java/frc/robot/subsystems/vision/LimelightBotPose.java b/src/main/java/frc/robot/subsystems/vision/LimelightBotPose.java new file mode 100644 index 00000000..bc9a1dbf --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/LimelightBotPose.java @@ -0,0 +1,157 @@ +package frc.robot.subsystems.vision; + +import edu.wpi.first.math.geometry.*; + +import java.util.Objects; + +public class LimelightBotPose { + + private double[] botPose; + private long timestamp; + + /* Pose Data & Avg Tag Info */ + private static final int OFFSET_POSE_X = 0; + private static final int OFFSET_POSE_Y = 1; + private static final int OFFSET_POSE_Z = 2; + private static final int OFFSET_POSE_ROTATION_ROLL = 3; + private static final int OFFSET_POSE_ROTATION_PITCH = 4; + private static final int OFFSET_POSE_ROTATION_YAW = 5; + private static final int OFFSET_TOTAL_LATENCY = 6; + private static final int OFFSET_TAG_COUNT = 7; + private static final int OFFSET_TAG_SPAN = 8; + private static final int OFFSET_AVG_TAG_DIST = 9; + private static final int OFFSET_AVG_TAG_AREA = 10; + + /* Pose Data & Avg Tag Info */ + private static final int OFFSET_TAG_BASE = 11; + private static final int ELEMENTS_PER_TAG = 7; + private static final int OFFSET_TAG_ID = 0; + private static final int OFFSET_TAG_TXNC = 1; + private static final int OFFSET_TAG_TYNC = 2; + private static final int OFFSET_TAG_TA = 3; + private static final int OFFSET_TAG_DIST_TO_CAMERA = 4; + private static final int OFFSET_TAG_DIST_TO_ROBOT = 5; + private static final int OFFSET_TAG_AMBIGUITY = 6; + + public LimelightBotPose(double [] botPose, long timestamp) { + update(botPose, timestamp); + } + + public void update(double [] botPose, long timestamp) { + this.botPose = Objects.requireNonNullElseGet(botPose, () -> new double[0]); + this.timestamp = timestamp; + } + + public Translation2d getTranslation() { + return new Translation2d(getPoseX(), getPoseY()); + } + + public Pose2d getPose() { + return new Pose2d(getTranslation(), Rotation2d.fromDegrees(getPoseRotationYaw())); + } + + public double getPoseX() { + return getElement(OFFSET_POSE_X); + } + + public double getPoseY() { + return getElement(OFFSET_POSE_Y); + } + + public double getPoseZ() { + return getElement(OFFSET_POSE_Z); + } + + public double getPoseRotationRoll() { + return getElement(OFFSET_POSE_ROTATION_ROLL); + } + + public double getPoseRotationPitch() { + return getElement(OFFSET_POSE_ROTATION_PITCH); + } + + public double getPoseRotationYaw() { + return getElement(OFFSET_POSE_ROTATION_YAW); + } + + public double getTotalLatency() { + return getElement(OFFSET_TOTAL_LATENCY); + } + + public double getTagCount() { + return getElement(OFFSET_TAG_COUNT, 0); + } + + public double getTagSpan() { + return getElement(OFFSET_TAG_SPAN); + } + + public double getAvgTagDist() { + return getElement(OFFSET_AVG_TAG_DIST); + } + + public double getAvgTagArea() { + return getElement(OFFSET_AVG_TAG_AREA); + } + + /** + * Find the index of a tag in the bot pose data + * @param tagId the id of the tag to find + * @return index of the tag in the bot pose data or -1 if not found + */ + public int getTagIndex(int tagId) { + for (int i = 0; i < getTagCount(); i++) { + if (getTagId(i) == tagId) { + return i; + } + } + return -1; + } + + public double getTagId(int index) { + return getTagElement(index, OFFSET_TAG_ID); + } + + public double getTagTxnc(int index) { + return getTagElement(index, OFFSET_TAG_TXNC); + } + + public double getTagTync(int index) { + return getTagElement(index, OFFSET_TAG_TYNC); + } + + public double getTagTa(int index) { + return getTagElement(index, OFFSET_TAG_TA); + } + + public double getTagDistToCamera(int index) { + return getTagElement(index, OFFSET_TAG_DIST_TO_CAMERA); + } + + public double getTagDistToRobot(int index) { + return getTagElement(index, OFFSET_TAG_DIST_TO_ROBOT); + } + + public double getTagAmbiguity(int index) { + return getTagElement(index, OFFSET_TAG_AMBIGUITY); + } + + private double getElement(int index) { + return getElement(index, Double.MIN_VALUE); + } + + private double getElement(int index, double defaultValue) { + if (index < 0 || index >= botPose.length) { + return defaultValue; + } + return botPose[index]; + } + + private double getTagElement(int index, int offset) { + int indexCalc = OFFSET_TAG_BASE + (index*ELEMENTS_PER_TAG) + offset; + if (index < 0 || index >= getTagCount() || indexCalc >= botPose.length) { + return Double.MIN_VALUE; + } + return botPose[indexCalc]; + } +} diff --git a/src/main/java/frc/robot/subsystems/vision/LimelightVisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/LimelightVisionSubsystem.java new file mode 100644 index 00000000..ecb8e2c6 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/LimelightVisionSubsystem.java @@ -0,0 +1,135 @@ +package frc.robot.subsystems.vision; + +import edu.wpi.first.networktables.*; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class LimelightVisionSubsystem extends SubsystemBase { + + private static final long CAM_MODE_VISION = 0; + private static final long PIPELINE_APRIL_TAG_DETECT = 0; + + NetworkTable lowRiderVision = NetworkTableInstance.getDefault().getTable("limelight-hugh"); + NetworkTable elevateVision = NetworkTableInstance.getDefault().getTable("limelight-jackman"); + + // inputs/configs + NetworkTableEntry lr_camMode = lowRiderVision.getEntry("camMode"); + NetworkTableEntry lr_pipeline = lowRiderVision.getEntry("pipeline"); + DoubleEntry lr_stream = lowRiderVision.getDoubleTopic("stream").getEntry(-1); + + // output + DoubleArraySubscriber lr_MegaTag1 = lowRiderVision.getDoubleArrayTopic("botpose_wpiblue").subscribe(new double[0]); + DoubleArraySubscriber lr_MegaTag2 = lowRiderVision.getDoubleArrayTopic("botpose_orb_wpiblue").subscribe(new double[0]); + + public enum CamStreamType { + SIDE_BY_SIDE(0), + LIMELIGHT(1), + WEBCAM(2); + + private final int index; + + // Constructor for the enum, which assigns the index to each constant. + private CamStreamType(int index) { + this.index = index; + } + + // Getter method to retrieve the index of the enum constant. + public int getIndex() { + return index; + } + } + + public enum TagType { + RED_SOURCE_LEFT(1), + RED_SOURCE_RIGHT(2), + RED_PROCESSOR(3), + RED_REEF_1(6), + RED_REEF_2(7), + RED_REEF_3(8), + RED_REEF_4(9), + RED_REEF_5(10), + RED_REEF_6(11), + BLUE_SOURCE_LEFT(12), + BLUE_SOURCE_RIGHT(13), + BLUE_PROCESSOR(16), + BLUE_REEF_1(17), + BLUE_REEF_2(18), + BLUE_REEF_3(19), + BLUE_REEF_4(20), + BLUE_REEF_5(21), + BLUE_REEF_6(22); + + private int tag; + + // Constructor for the enum, which assigns the index to each constant. + private TagType(int tag) { + this.tag = tag; + } + + // Getter method to retrieve the index of the enum constant. + public int getIndex() { + return tag; + } + } + + private final LimelightBotPose limelightBotPose = new LimelightBotPose(null, 0); + private int targetTagId = 0; + + public LimelightVisionSubsystem() { + this.lr_pipeline.setNumber(PIPELINE_APRIL_TAG_DETECT); + this.lr_camMode.setNumber(CAM_MODE_VISION); + } + + @Override + public void periodic() { + TimestampedDoubleArray botPoseBlueMegaTag1 = lr_MegaTag1.getAtomic(); + limelightBotPose.update(botPoseBlueMegaTag1.value, botPoseBlueMegaTag1.timestamp); + } + + /* Public API */ + + public void setTargetTagId(TagType tag) { + this.targetTagId = tag.getIndex(); + } + + public void clearTargetTagId() { + this.targetTagId = 0; + } + + public double getVisibleTargetTagId() { + return limelightBotPose.getTagId(0); + } + + public double distanceToTarget() { + int index = 0; + if (targetTagId > 0) { + index = limelightBotPose.getTagIndex(targetTagId); + } + return limelightBotPose.getTagDistToRobot(index); + } + + public double angleToTarget() { + int index = 0; + if (targetTagId > 0) { + index = limelightBotPose.getTagIndex(targetTagId); + } + return limelightBotPose.getTagTxnc(index); + } + + /** + * Set the camera view to the specified stream. + * @param stream the camera stream to set the view to + */ + public void setCameraView(CamStreamType stream) { + lr_stream.set(stream.getIndex()); + } + + public LimelightBotPose getBotPose() { + return limelightBotPose; + } + + @Override + public String toString() { + return "Hugh Vision Subsystem"; + } + +} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConfig.java b/src/main/java/frc/robot/subsystems/vision/VisionConfig.java new file mode 100644 index 00000000..8a45081c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/VisionConfig.java @@ -0,0 +1,24 @@ +package frc.robot.subsystems.vision; + +/** + * Configuration for the vision system. + * + * @param pipelineAprilTagDetect The limelight pipeline id containing april tag detection + * @param camModeVision the camera mode for vision processing + * @param maxAmbiguity the maximum ambiguity allowed for a vision target, above which results will + * be ignored + * @param highQualityAmbiguity the ambiguity value above which the vision data is considered high + * quality + * @param maxVisposeDeltaDistanceMetres the maximum distance between two vispose measurements to be + * considered the same + * @param megatag2 Should megatag2 be used? + * @param telemetryLevel What level of telemetry should be logged + */ +public record VisionConfig( + long pipelineAprilTagDetect, + long camModeVision, + double maxAmbiguity, + double highQualityAmbiguity, + double maxVisposeDeltaDistanceMetres, + boolean megatag2, + VisionTelemetryLevel telemetryLevel) {} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionTelemetryLevel.java b/src/main/java/frc/robot/subsystems/vision/VisionTelemetryLevel.java new file mode 100644 index 00000000..8e6a652e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/VisionTelemetryLevel.java @@ -0,0 +1,9 @@ +package frc.robot.subsystems.vision; + +public enum VisionTelemetryLevel { + NONE, + REGULAR, + VERBOSE; + + private VisionTelemetryLevel() {} +} diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json new file mode 100644 index 00000000..bef4a151 --- /dev/null +++ b/vendordeps/AdvantageKit.json @@ -0,0 +1,35 @@ +{ + "fileName": "AdvantageKit.json", + "name": "AdvantageKit", + "version": "4.1.2", + "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", + "frcYear": "2025", + "mavenUrls": [ + "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/" + ], + "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", + "javaDependencies": [ + { + "groupId": "org.littletonrobotics.akit", + "artifactId": "akit-java", + "version": "4.1.2" + } + ], + "jniDependencies": [ + { + "groupId": "org.littletonrobotics.akit", + "artifactId": "akit-wpilibio", + "version": "4.1.2", + "skipInvalidPlatforms": false, + "isJar": false, + "validPlatforms": [ + "linuxathena", + "linuxx86-64", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] + } + ], + "cppDependencies": [] +} \ No newline at end of file diff --git a/vendordeps/NavX.json b/vendordeps/NavX.json deleted file mode 100644 index e978a5f7..00000000 --- a/vendordeps/NavX.json +++ /dev/null @@ -1,40 +0,0 @@ -{ - "fileName": "NavX.json", - "name": "NavX", - "version": "2024.1.0", - "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", - "frcYear": "2024", - "mavenUrls": [ - "https://dev.studica.com/maven/release/2024/" - ], - "jsonUrl": "https://dev.studica.com/releases/2024/NavX.json", - "javaDependencies": [ - { - "groupId": "com.kauailabs.navx.frc", - "artifactId": "navx-frc-java", - "version": "2024.1.0" - } - ], - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "com.kauailabs.navx.frc", - "artifactId": "navx-frc-cpp", - "version": "2024.1.0", - "headerClassifier": "headers", - "sourcesClassifier": "sources", - "sharedLibrary": false, - "libName": "navx_frc", - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "linuxraspbian", - "linuxarm32", - "linuxarm64", - "linuxx86-64", - "osxuniversal", - "windowsx86-64" - ] - } - ] -} \ No newline at end of file diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index e334459f..10f6cd22 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,9 +1,9 @@ { "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2024.1.1", + "version": "2025.2.6", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", - "frcYear": "2024", + "frcYear": "2025", "mavenUrls": [ "https://3015rangerrobotics.github.io/pathplannerlib/repo" ], @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2024.1.1" + "version": "2025.2.6" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2024.1.1", + "version": "2025.2.6", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/Phoenix5.json b/vendordeps/Phoenix5.json deleted file mode 100644 index 88a68dd0..00000000 --- a/vendordeps/Phoenix5.json +++ /dev/null @@ -1,151 +0,0 @@ -{ - "fileName": "Phoenix5.json", - "name": "CTRE-Phoenix (v5)", - "version": "5.33.0", - "frcYear": 2024, - "uuid": "ab676553-b602-441f-a38d-f1296eff6537", - "mavenUrls": [ - "https://maven.ctr-electronics.com/release/" - ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2024-latest.json", - "requires": [ - { - "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", - "errorMessage": "Phoenix 5 requires low-level libraries from Phoenix 6. Please add the Phoenix 6 vendordep before adding Phoenix 5.", - "offlineFileName": "Phoenix6.json", - "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json" - } - ], - "javaDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "api-java", - "version": "5.33.0" - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "wpiapi-java", - "version": "5.33.0" - } - ], - "jniDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "cci", - "version": "5.33.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "cci-sim", - "version": "5.33.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - } - ], - "cppDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "wpiapi-cpp", - "version": "5.33.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.33.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.33.0", - "libName": "CTRE_PhoenixCCI", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "wpiapi-cpp-sim", - "version": "5.33.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.33.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.33.0", - "libName": "CTRE_PhoenixCCISim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - } - ] -} \ No newline at end of file diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6-25.3.1.json similarity index 66% rename from vendordeps/Phoenix6.json rename to vendordeps/Phoenix6-25.3.1.json index 69a40798..a216d975 100644 --- a/vendordeps/Phoenix6.json +++ b/vendordeps/Phoenix6-25.3.1.json @@ -1,76 +1,94 @@ { - "fileName": "Phoenix6.json", + "fileName": "Phoenix6-25.3.1.json", "name": "CTRE-Phoenix (v6)", - "version": "24.1.0", - "frcYear": 2024, + "version": "25.3.1", + "frcYear": "2025", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ "https://maven.ctr-electronics.com/release/" ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json", + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-latest.json", "conflictsWith": [ { - "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a", - "errorMessage": "The combined Phoenix-6-And-5 vendordep is no longer supported. Please remove the vendordep and instead add both the latest Phoenix 6 vendordep and Phoenix 5 vendordep.", - "offlineFileName": "Phoenix6And5.json" + "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", + "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.", + "offlineFileName": "Phoenix6-replay-frc2025-latest.json" } ], "javaDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "24.1.0" + "version": "25.3.1" } ], "jniDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "api-cpp", + "version": "25.3.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.1.0", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "linuxathena" ], "simMode": "hwsim" }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "tools-sim", - "version": "24.1.0", + "artifactId": "api-cpp-sim", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonSRX", - "version": "24.1.0", + "artifactId": "tools-sim", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonFX", - "version": "24.1.0", + "artifactId": "simTalonSRX", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -78,12 +96,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.1.0", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -91,12 +110,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.1.0", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -104,12 +124,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.1.0", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -117,12 +138,27 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.1.0", + "version": "25.3.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -130,12 +166,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.1.0", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -143,12 +180,41 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.1.0", + "version": "25.3.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "25.3.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -158,7 +224,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "24.1.0", + "version": "25.3.1", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -166,6 +232,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "linuxathena" ], "simMode": "hwsim" @@ -173,7 +240,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.1.0", + "version": "25.3.1", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -181,6 +248,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "linuxathena" ], "simMode": "hwsim" @@ -188,7 +256,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "24.1.0", + "version": "25.3.1", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -196,6 +264,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -203,7 +272,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "24.1.0", + "version": "25.3.1", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -211,6 +280,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -218,7 +288,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "24.1.0", + "version": "25.3.1", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -226,81 +296,87 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonFX", - "version": "24.1.0", - "libName": "CTRE_SimTalonFX", + "artifactId": "simVictorSPX", + "version": "25.3.1", + "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simVictorSPX", - "version": "24.1.0", - "libName": "CTRE_SimVictorSPX", + "artifactId": "simPigeonIMU", + "version": "25.3.1", + "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simPigeonIMU", - "version": "24.1.0", - "libName": "CTRE_SimPigeonIMU", + "artifactId": "simCANCoder", + "version": "25.3.1", + "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simCANCoder", - "version": "24.1.0", - "libName": "CTRE_SimCANCoder", + "artifactId": "simProTalonFX", + "version": "25.3.1", + "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFX", - "version": "24.1.0", - "libName": "CTRE_SimProTalonFX", + "artifactId": "simProTalonFXS", + "version": "25.3.1", + "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -308,7 +384,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.1.0", + "version": "25.3.1", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -316,6 +392,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -323,7 +400,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.1.0", + "version": "25.3.1", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -331,6 +408,39 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "25.3.1", + "libName": "CTRE_SimProCANrange", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "25.3.1", + "libName": "CTRE_SimProCANdi", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index f85acd40..ac62be88 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,30 +1,29 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2024.2.4", - "frcYear": "2024", + "version": "2025.0.3", + "frcYear": "2025", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ "https://maven.revrobotics.com/" ], - "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2024.json", + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2025.json", "javaDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2024.2.4" + "version": "2025.0.3" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.4", + "version": "2025.0.3", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ "windowsx86-64", - "windowsx86", "linuxarm64", "linuxx86-64", "linuxathena", @@ -37,14 +36,13 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2024.2.4", + "version": "2025.0.3", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", - "windowsx86", "linuxarm64", "linuxx86-64", "linuxathena", @@ -55,14 +53,13 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.4", + "version": "2025.0.3", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", - "windowsx86", "linuxarm64", "linuxx86-64", "linuxathena", diff --git a/vendordeps/ReduxLib_2024.json b/vendordeps/ReduxLib_2024.json deleted file mode 100644 index 9f25c718..00000000 --- a/vendordeps/ReduxLib_2024.json +++ /dev/null @@ -1,55 +0,0 @@ -{ - "fileName": "ReduxLib_2024.json", - "name": "ReduxLib", - "version": "2024.1.1-beta0", - "frcYear": 2024, - "uuid": "151ecca8-670b-4026-8160-cdd2679ef2bd", - "mavenUrls": [ - "https://maven.reduxrobotics.com/" - ], - "jsonUrl": "https://frcsdk.reduxrobotics.com/ReduxLib_2024.json", - "javaDependencies": [ - { - "groupId": "com.reduxrobotics.frc", - "artifactId": "ReduxLib-java", - "version": "2024.1.1-beta0" - } - ], - "jniDependencies": [ - { - "groupId": "com.reduxrobotics.frc", - "artifactId": "ReduxLib-driver", - "version": "2024.1.1-beta0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "linuxathena", - "linuxx86-64", - "linuxarm32", - "linuxarm64", - "osxuniversal", - "windowsx86-64" - ] - } - ], - "cppDependencies": [ - { - "groupId": "com.reduxrobotics.frc", - "artifactId": "ReduxLib-cpp", - "version": "2024.1.1-beta0", - "libName": "ReduxLib-cpp", - "headerClassifier": "headers", - "sourcesClassifier": "sources", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "linuxx86-64", - "linuxarm32", - "linuxarm64", - "osxuniversal", - "windowsx86-64" - ] - } - ] -} \ No newline at end of file diff --git a/vendordeps/Studica-2025.0.1.json b/vendordeps/Studica-2025.0.1.json new file mode 100644 index 00000000..5010be04 --- /dev/null +++ b/vendordeps/Studica-2025.0.1.json @@ -0,0 +1,71 @@ +{ + "fileName": "Studica-2025.0.1.json", + "name": "Studica", + "version": "2025.0.1", + "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", + "frcYear": "2025", + "mavenUrls": [ + "https://dev.studica.com/maven/release/2025/" + ], + "jsonUrl": "https://dev.studica.com/releases/2025/Studica-2025.0.1.json", + "cppDependencies": [ + { + "artifactId": "Studica-cpp", + "binaryPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "linuxx86-64", + "osxuniversal", + "windowsx86-64" + ], + "groupId": "com.studica.frc", + "headerClassifier": "headers", + "libName": "Studica", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "version": "2025.0.1" + }, + { + "artifactId": "Studica-driver", + "binaryPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "linuxx86-64", + "osxuniversal", + "windowsx86-64" + ], + "groupId": "com.studica.frc", + "headerClassifier": "headers", + "libName": "StudicaDriver", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "version": "2025.0.1" + } + ], + "javaDependencies": [ + { + "artifactId": "Studica-java", + "groupId": "com.studica.frc", + "version": "2025.0.1" + } + ], + "jniDependencies": [ + { + "artifactId": "Studica-driver", + "groupId": "com.studica.frc", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "linuxx86-64", + "osxuniversal", + "windowsx86-64" + ], + "version": "2025.0.1" + } + ] +} \ No newline at end of file diff --git a/vendordeps/WPILibNewCommands.json b/vendordeps/WPILibNewCommands.json index 67bf3898..3718e0ac 100644 --- a/vendordeps/WPILibNewCommands.json +++ b/vendordeps/WPILibNewCommands.json @@ -3,7 +3,7 @@ "name": "WPILib-New-Commands", "version": "1.0.0", "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", - "frcYear": "2024", + "frcYear": "2025", "mavenUrls": [], "jsonUrl": "", "javaDependencies": [ diff --git a/vendordeps/yagsl.json b/vendordeps/yagsl.json deleted file mode 100644 index f9227b8b..00000000 --- a/vendordeps/yagsl.json +++ /dev/null @@ -1,22 +0,0 @@ -{ - "fileName": "yagsl.json", - "name": "YAGSL", - "version": "2024.4.8.4", - "frcYear": "2024", - "uuid": "1ccce5a4-acd2-4d18-bca3-4b8047188400", - "mavenUrls": [ - "https://broncbotz3481.github.io/YAGSL-Lib/yagsl/repos" - ], - "jsonUrl": "https://broncbotz3481.github.io/YAGSL-Lib/yagsl/yagsl.json", - "javaDependencies": [ - { - "groupId": "swervelib", - "artifactId": "YAGSL-java", - "version": "2024.4.8.4" - } - ], - "jniDependencies": [ - ], - "cppDependencies": [ - ] -} \ No newline at end of file